ngpios = <64>;
status = "okay";
};
+ i2c6: i2c@12060000 {
+ #address-cells = <1>;
+ #size-cells = <0>;
+ compatible = "snps,designware-i2c";
+ reg = <0x0 0x12060000 0x0 0x10000>;
+ interrupt-parent = <&plic>;
+ interrupts = <51>;
+ clock-frequency = <100000>;
+ i2c-sda-hold-time-ns = <300>;
+ i2c-sda-falling-time-ns = <3000>;
+ i2c-scl-falling-time-ns = <3000>;
+ auto_calc_scl_lhcnt;
+ status = "okay";
+
+
+
+ sc2235@30 {
+ compatible = "sc2235";
+ reg = <0x30>;
+ clocks = <&clk_ext_camera>;
+ clock-names = "xclk";
+ powerdown-gpios = <&gpio 0 0>;
+ reset-gpios = <&gpio 0 0>;
+ sc2235-18-gpios = <&gpio 11 0>;
+ sc2235-15-gpios = <&gpio 12 0>;
+ sc2235-28-gpios = <&gpio 10 0>;
+ sc2235-reset-gpios = <&gpio 16 0>;
+ sc2235-pwdn-gpios = <&gpio 15 0>;
+ sc2235-esync-gpios = <&gpio 17 0>;
+ sc2235-oen-gpios = <&gpio 18 0>;
+ //DOVDD-supply = <&v2v8>;
+
+ port {
+ /* Parallel bus endpoint */
+ sc2235_to_parallel: endpoint {
+ remote-endpoint = <¶llel_from_sc2235>;
+ bus-type = <5>; /* Parallel */
+ bus-width = <8>;
+ data-shift = <2>; /* lines 13:6 are used */
+ hsync-active = <1>;
+ vsync-active = <1>;
+ pclk-sample = <1>;
+ };
+ };
+ };
+ };
i2c0: i2c@10030000 {
#address-cells = <1>;
#size-cells = <0>;
/*fixed-emmc-driver-type;*/
post-power-on-delay-ms = <200>;
};
+ vin_sysctl:vin_sysctl@19800000 {
+ compatible = "starfive,stf-vin";
+ reg = <0x0 0x19800000 0x0 0x10000>,//mipi-csi-0 mipi0
+ <0x0 0x19810000 0x0 0x10000>,//dom-isp-cfg vclk
+ <0x0 0x19820000 0x0 0x10000>,//mipirx-dphyapb config vrst
+ <0x0 0x19830000 0x0 0x10000>,//reserved mipi1
+ <0x0 0x19840000 0x0 0x10000>,//dom-isp-syscon sctrl
+ <0x0 0x19870000 0x0 0x30000>,//ispv1-mini isp0
+ <0x0 0x198a0000 0x0 0x30000>,//reserved isp1
+ <0x0 0x11800000 0x0 0x10000>,//reserved tclk
+ <0x0 0x11840000 0x0 0x10000>,//reserved trst
+ <0x0 0x11858000 0x0 0x10000>,//reserved iopad
+ <0x0 0x17030000 0x0 0x10000>, //pmu
+ <0x0 0x13020000 0x0 0x10000>; //sys_crg
+ reg-names = "mipi0", "vclk", "vrst", "mipi1", "sctrl", "isp0", "isp1", "tclk", "trst", "iopad", "pmu", "syscrg";
+ interrupt-parent = <&plic>;
+ interrupts = <92 87 86>;
+ // memory-region = <&vin_reserved>;
+
+ ports {
+ port@3 {
+ reg = <2>; //dvp sensor
+
+ /* Parallel bus endpoint */
+ parallel_from_sc2235: endpoint {
+ remote-endpoint = <&sc2235_to_parallel>;
+ bus-type = <5>; /* Parallel */
+ bus-width = <8>;
+ data-shift = <2>; /* lines 9:2 are used */
+ hsync-active = <1>;
+ vsync-active = <1>;
+ pclk-sample = <1>;
+ status = "okay";
+ };
+ };
+ };
+ };
jpu: jpu@11900000 {
compatible = "starfive,jpu";
reg = <0x0 0x13090000 0x0 0x300>;
<0x0 0x0 0x0 0x2 &plic 0x2>,
<0x0 0x0 0x0 0x3 &plic 0x3>,
<0x0 0x0 0x0 0x4 &plic 0x4>;
+ status = "disabled";
};
};
};
compatible = "fixed-clock";
clock-frequency = <12288000>;
};
+ clk_ext_camera: clk-ext-camera {
+ #clock-cells = <0>;
+ compatible = "fixed-clock";
+ clock-frequency = <24000000>;
+ };
canclk: canclk {
#clock-cells = <0>;
compatible = "fixed-clock";
# CONFIG_HWMON is not set
CONFIG_WATCHDOG=y
CONFIG_STARFIVE_WATCHDOG=y
+# CONFIG_MEDIA_CEC_SUPPORT is not set
+CONFIG_MEDIA_SUPPORT=y
+CONFIG_V4L_PLATFORM_DRIVERS=y
+CONFIG_VIDEO_STF_VIN=y
+CONFIG_VIN_SENSOR_SC2235=y
CONFIG_DRM=y
CONFIG_DRM_VGEM=y
CONFIG_DRM_VKMS=y
-# CONFIG_DRM_IMG_NULLDISP is not set
CONFIG_FB=y
CONFIG_BACKLIGHT_CLASS_DEVICE=y
CONFIG_FRAMEBUFFER_CONSOLE=y
To compile this driver as a module, choose M here: the module
will be called stm32-dcmi.
+config VIDEO_STF_VIN
+ tristate "starfive VIC video in support"
+ depends on VIDEO_V4L2 && OF
+ select MEDIA_CONTROLLER
+ select VIDEOBUF2_DMA_CONTIG
+ select VIDEO_V4L2_SUBDEV_API
+ select V4L2_FWNODE
+ help
+ To compile this driver as a module, choose M here: the module
+ will be called stf-vin.
+
config VIDEO_RENESAS_CEU
tristate "Renesas Capture Engine Unit (CEU) driver"
depends on VIDEO_DEV && VIDEO_V4L2
source "drivers/media/platform/rcar-vin/Kconfig"
source "drivers/media/platform/atmel/Kconfig"
source "drivers/media/platform/sunxi/Kconfig"
+source "drivers/media/platform/starfive/Kconfig"
config VIDEO_TI_CAL
tristate "TI CAL (Camera Adaptation Layer) driver"
obj-y += stm32/
+obj-y += starfive/
+
obj-y += davinci/
obj-$(CONFIG_VIDEO_SH_VOU) += sh_vou.o
--- /dev/null
+#
+# VIN sensor driver configuration
+#
+config VIN_SENSOR_OV5640
+ bool "VIN SENSOR support OV5640"
+ depends on VIDEO_STF_VIN
+ select V4L2_FWNODE
+ default n
+ help
+ Say Y here if you want to have support for VIN sensor OV5640
+
+config VIN_SENSOR_SC2235
+ bool "VIN SENSOR support SC2235"
+ depends on VIDEO_STF_VIN
+ select V4L2_FWNODE
+ default n
+ help
+ Say Y here if you want to have support for VIN sensor SC2235
+
+config VIN_SENSOR_OV4689
+ bool "VIN SENSOR support OV4689"
+ depends on VIDEO_STF_VIN
+ select V4L2_FWNODE
+ default n
+ help
+ Say Y here if you want to have support for VIN sensor OV4689
--- /dev/null
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for RTC class/drivers.
+#
+
+#obj-$(CONFIG_VIN_SENSOR_OV5640) += ov5640_dvp.o
+#obj-$(CONFIG_VIN_SENSOR_SC2235) += sc2235.o
+#obj-$(CONFIG_VIN_SENSOR_OV4689) += ov4689_mipi.o
+#obj-$(CONFIG_VIDEO_STF_VIN) += stf_vin.o stf_event.o stf_isp.o stf_csi.o
+
+obj-$(CONFIG_VIN_SENSOR_OV5640) += v4l2_driver/ov5640.o
+obj-$(CONFIG_VIN_SENSOR_SC2235) += v4l2_driver/sc2235.o
+obj-$(CONFIG_VIN_SENSOR_OV4689) += v4l2_driver/ov4689_mipi.o
+
+obj-$(CONFIG_VIDEO_STF_VIN) += v4l2_driver/stfcamss.o \
+ v4l2_driver/stf_event.o \
+ v4l2_driver/stf_dvp.o \
+ v4l2_driver/stf_csi.o \
+ v4l2_driver/stf_csiphy.o \
+ v4l2_driver/stf_isp.o \
+ v4l2_driver/stf_video.o \
+ v4l2_driver/stf_vin.o \
+ v4l2_driver/stf_vin_hw_ops.o \
+ v4l2_driver/stf_csi_hw_ops.o \
+ v4l2_driver/stf_csiphy_hw_ops.o \
+ v4l2_driver/stf_isp_hw_ops.o \
+ v4l2_driver/stf_dvp_hw_ops.o
--- /dev/null
+
+
+/dev/video0 sensor配置为ov5640的设备节点
+/dev/video1 sensor配置为ov4689(i2c0)的设备节点
+/dev/video2 sensor配置为sc2235/ov4689(i2c2)的设备节点
+
+确认conf/sdk_210209_defconfig
+CONFIG_VIN_SENSOR_OV5640=y
+CONFIG_VIN_SENSOR_SC2235=y
+CONFIG_VIN_SENSOR_OV4689=y
+
+
+只支持DPHY的lane0/lane5做clk通道，lane1/2/3/4做数据通道。
+
+sensor port 设为okay, 硬件需要接入对应的sensor，否则驱动不能使用。
+
+1. ov5640 config dts:
+ parallel_from_ov5640 port status 设置为okay, sc2235 port status 设为failed.
+ port@2 {
+ reg = <2>; // dvp sensor
+
+ /* Parallel bus endpoint */
+ parallel_from_ov5640: endpoint {
+ remote-endpoint = <&ov5640_to_parallel>;
+ bus-type = <5>; /* Parallel */
+ bus-width = <8>;
+ data-shift = <2>; /* lines 9:2 are used */
+ hsync-active = <1>;
+ vsync-active = <0>;
+ pclk-sample = <1>;
+ sensor-type = <0>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+ status = "okay";
+ };
+ };
+
+2. SC2235 config dts:
+ stf_isp_hw_ops.c:
+ stf_isp_set_format函数里面注释掉：
+ // isp_settings = isp_1920_1080_settings;
+
+ parallel_from_sc2235 port status 设置为okay, ov5640/ov4689(i2c2) port status设为failed.
+ port@3 {
+ reg = <2>; // dvp sensor
+
+ /* Parallel bus endpoint */
+ parallel_from_sc2235: endpoint {
+ remote-endpoint = <&sc2235_to_parallel>;
+ bus-type = <5>; /* Parallel */
+ bus-width = <8>;
+ data-shift = <2>; /* lines 9:2 are used */
+ hsync-active = <1>;
+ vsync-active = <1>;
+ pclk-sample = <1>;
+ sensor-type = <2>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+ status = "okay";
+ };
+ };
+
+3. i2c0 ov4689 config dts:
+ csi2rx0_from_ov4689 port status 设置为okay.
+ port@4 {
+ reg = <3>; // csi2rx0 sensor
+
+ /* CSI2 bus endpoint */
+ csi2rx0_from_ov4689: endpoint {
+ remote-endpoint = <&ov4689_to_csi2rx0>;
+ bus-type = <4>; /* MIPI CSI-2 D-PHY */
+ clock-lanes = <0>;
+ data-lanes = <1 2>;
+ sensor-type = <1>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+ csi-dt = <0x2b>;
+ status = "okay";
+ };
+ };
+
+4. i2c2 ov4689 config dts:
+
+ stf_isp_hw_ops.c:
+ stf_isp_set_format函数里面346行不要注释掉：
+ isp_settings = isp_1920_1080_settings;
+
+ csi2rx1_from_ov4689 port status 设置为okay, sc2235 port status 设为failed.
+ port@5 {
+ reg = <4>; // csi2rx1 sensor
+
+ /* CSI2 bus endpoint */
+ csi2rx1_from_ov4689: endpoint {
+ remote-endpoint = <&ov4689_to_csi2rx1>;
+ bus-type = <4>; /* MIPI CSI-2 D-PHY */
+ clock-lanes = <5>;
+ data-lanes = <4 3>;
+ lane-polarities = <1 1 1>;
+ sensor-type = <2>; //0:SENSOR_VIN 1:SENSOR_ISP0 2:SENSOR_ISP1
+ csi-dt = <0x2b>;
+ status = "okay";
+ };
+ };
+
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.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 "stfcamss.h"
+
+
+#define OV4689_LANES 2
+
+#define OV4689_LINK_FREQ_500MHZ 500000000LL
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV4689_XCLK_MIN 6000000
+#define OV4689_XCLK_MAX 54000000
+
+#define OV4689_CHIP_ID (0x4688)
+
+#define OV4689_CHIP_ID_HIGH_BYTE 0x300a // max should be 0x46
+#define OV4689_CHIP_ID_LOW_BYTE 0x300b // max should be 0x88
+#define OV4689_REG_CHIP_ID 0x300a
+
+#define OV4689_REG_H_OUTPUT_SIZE 0x3808
+#define OV4689_REG_V_OUTPUT_SIZE 0x380a
+#define OV4689_REG_TIMING_HTS 0x380c
+#define OV4689_REG_TIMING_VTS 0x380e
+
+#define OV4689_REG_EXPOSURE_HI 0x3500
+#define OV4689_REG_EXPOSURE_MED 0x3501
+#define OV4689_REG_EXPOSURE_LO 0x3502
+#define OV4689_REG_GAIN_H 0x3507
+#define OV4689_REG_GAIN_M 0x3508
+#define OV4689_REG_GAIN_L 0x3509
+#define OV4689_REG_TEST_PATTERN 0x5040
+#define OV4689_REG_TIMING_TC_REG20 0x3820
+#define OV4689_REG_TIMING_TC_REG21 0x3821
+
+#define OV4689_REG_AWB_R_GAIN 0x500C
+#define OV4689_REG_AWB_B_GAIN 0x5010
+
+
+enum ov4689_mode_id {
+ OV4689_MODE_720P_1280_720 = 0,
+ OV4689_MODE_1080P_1920_1080,
+ OV4689_MODE_4M_2688_1520,
+ OV4689_NUM_MODES,
+};
+
+enum ov4689_frame_rate {
+ OV4689_15_FPS = 0,
+ OV4689_30_FPS,
+ OV4689_45_FPS,
+ OV4689_60_FPS,
+ OV4689_90_FPS,
+ OV4689_120_FPS,
+ OV4689_150_FPS,
+ OV4689_180_FPS,
+ OV4689_330_FPS,
+ OV4689_NUM_FRAMERATES,
+};
+
+enum ov4689_format_mux {
+ OV4689_FMT_MUX_RAW,
+};
+
+static const int ov4689_framerates[] = {
+ [OV4689_15_FPS] = 15,
+ [OV4689_30_FPS] = 30,
+ [OV4689_45_FPS] = 45,
+ [OV4689_60_FPS] = 60,
+ [OV4689_90_FPS] = 90,
+ [OV4689_120_FPS] = 120,
+ [OV4689_150_FPS] = 150,
+ [OV4689_180_FPS] = 180,
+ [OV4689_330_FPS] = 330,
+};
+
+/* regulator supplies */
+static const char * const ov4689_supply_name[] = {
+ "DOVDD", /* Digital I/O (1.8V) supply */
+ "AVDD", /* Analog (2.8V) supply */
+ "DVDD", /* Digital Core (1.5V) supply */
+};
+
+#define OV4689_NUM_SUPPLIES ARRAY_SIZE(ov4689_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov4689_downsize_mode {
+ SUBSAMPLING,
+ SCALING,
+};
+
+struct reg_value {
+ u16 reg_addr;
+ u8 val;
+ u8 mask;
+ u32 delay_ms;
+};
+
+struct ov4689_mode_info {
+ enum ov4689_mode_id id;
+ enum ov4689_downsize_mode dn_mode;
+ u32 hact;
+ u32 htot;
+ u32 vact;
+ u32 vtot;
+ const struct reg_value *reg_data;
+ u32 reg_data_size;
+ u32 max_fps;
+};
+
+struct ov4689_ctrls {
+ struct v4l2_ctrl_handler handler;
+ struct v4l2_ctrl *pixel_rate;
+ struct {
+ struct v4l2_ctrl *exposure;
+ };
+ struct {
+ struct v4l2_ctrl *auto_wb;
+ struct v4l2_ctrl *blue_balance;
+ struct v4l2_ctrl *red_balance;
+ };
+ struct {
+ struct v4l2_ctrl *anal_gain;
+ };
+ struct v4l2_ctrl *brightness;
+ struct v4l2_ctrl *light_freq;
+ struct v4l2_ctrl *link_freq;
+ struct v4l2_ctrl *saturation;
+ struct v4l2_ctrl *contrast;
+ struct v4l2_ctrl *hue;
+ struct v4l2_ctrl *test_pattern;
+ struct v4l2_ctrl *hflip;
+ struct v4l2_ctrl *vflip;
+};
+
+struct ov4689_dev {
+ struct i2c_client *i2c_client;
+ struct v4l2_subdev sd;
+ struct media_pad pad;
+ struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+ struct clk *xclk; /* system clock to OV4689 */
+ u32 xclk_freq;
+
+ struct regulator_bulk_data supplies[OV4689_NUM_SUPPLIES];
+ struct gpio_desc *reset_gpio;
+ struct gpio_desc *pwdn_gpio;
+ bool upside_down;
+
+ /* lock to protect all members below */
+ struct mutex lock;
+
+ int power_count;
+
+ struct v4l2_mbus_framefmt fmt;
+
+ const struct ov4689_mode_info *current_mode;
+ const struct ov4689_mode_info *last_mode;
+ enum ov4689_frame_rate current_fr;
+ struct v4l2_fract frame_interval;
+
+ struct ov4689_ctrls ctrls;
+
+ u32 prev_sysclk, prev_hts;
+ u32 ae_low, ae_high, ae_target;
+
+ bool pending_mode_change;
+ bool streaming;
+};
+
+static inline struct ov4689_dev *to_ov4689_dev(struct v4l2_subdev *sd)
+{
+ return container_of(sd, struct ov4689_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+ return &container_of(ctrl->handler, struct ov4689_dev,
+ ctrls.handler)->sd;
+}
+
+/* ov4689 initial register */
+static const struct reg_value ov4689_init_setting_30fps_1080P[] = {
+
+};
+
+static const struct reg_value ov4689_setting_VGA_640_480[] = {
+ //@@ RES_640x480_2x_Bin_330fps_816Mbps
+ //OV4689_AM01B_640x480_24M_2lane_816Mbps_330fps_20140210.txt
+ {0x0103, 0x01, 0, 0},
+ {0x3638, 0x00, 0, 0},
+ {0x0300, 0x00, 0, 0}, // 00
+ {0x0302, 0x22, 0, 0}, // 816Mbps 5a ; 64 ; 5a ; 78 ; 78 ; 2a
+ {0x0303, 0x00, 0, 0}, // 03 ; 01 ; 02 ;
+ {0x0304, 0x03, 0, 0},
+ {0x030b, 0x00, 0, 0},
+ {0x030d, 0x1e, 0, 0},
+ {0x030e, 0x04, 0, 0},
+ {0x030f, 0x01, 0, 0},
+ {0x0312, 0x01, 0, 0},
+ {0x031e, 0x00, 0, 0},
+ {0x3000, 0x20, 0, 0},
+ {0x3002, 0x00, 0, 0},
+ {0x3018, 0x32, 0, 0}, // 32/72 2lane/4lane
+ {0x3019, 0x0c, 0, 0}, // 0c/00 2lane/4lane
+ {0x3020, 0x93, 0, 0},
+ {0x3021, 0x03, 0, 0},
+ {0x3022, 0x01, 0, 0},
+ {0x3031, 0x0a, 0, 0},
+ {0x303f, 0x0c, 0, 0},
+ {0x3305, 0xf1, 0, 0},
+ {0x3307, 0x04, 0, 0},
+ {0x3309, 0x29, 0, 0},
+ {0x3500, 0x00, 0, 0},
+ {0x3501, 0x4c, 0, 0},
+ {0x3502, 0x00, 0, 0},
+ {0x3503, 0x04, 0, 0},
+ {0x3504, 0x00, 0, 0},
+ {0x3505, 0x00, 0, 0},
+ {0x3506, 0x00, 0, 0},
+ {0x3507, 0x00, 0, 0},
+ {0x3508, 0x00, 0, 0},
+ {0x3509, 0x80, 0, 0}, // 8X
+ {0x350a, 0x00, 0, 0},
+ {0x350b, 0x00, 0, 0},
+ {0x350c, 0x00, 0, 0},
+ {0x350d, 0x00, 0, 0},
+ {0x350e, 0x00, 0, 0},
+ {0x350f, 0x80, 0, 0},
+ {0x3510, 0x00, 0, 0},
+ {0x3511, 0x00, 0, 0},
+ {0x3512, 0x00, 0, 0},
+ {0x3513, 0x00, 0, 0},
+ {0x3514, 0x00, 0, 0},
+ {0x3515, 0x80, 0, 0},
+ {0x3516, 0x00, 0, 0},
+ {0x3517, 0x00, 0, 0},
+ {0x3518, 0x00, 0, 0},
+ {0x3519, 0x00, 0, 0},
+ {0x351a, 0x00, 0, 0},
+ {0x351b, 0x80, 0, 0},
+ {0x351c, 0x00, 0, 0},
+ {0x351d, 0x00, 0, 0},
+ {0x351e, 0x00, 0, 0},
+ {0x351f, 0x00, 0, 0},
+ {0x3520, 0x00, 0, 0},
+ {0x3521, 0x80, 0, 0},
+ {0x3522, 0x08, 0, 0},
+ {0x3524, 0x08, 0, 0},
+ {0x3526, 0x08, 0, 0},
+ {0x3528, 0x08, 0, 0},
+ {0x352a, 0x08, 0, 0},
+ {0x3602, 0x00, 0, 0},
+ {0x3603, 0x40, 0, 0},
+ {0x3604, 0x02, 0, 0},
+ {0x3605, 0x00, 0, 0},
+ {0x3606, 0x00, 0, 0},
+ {0x3607, 0x00, 0, 0},
+ {0x3609, 0x12, 0, 0},
+ {0x360a, 0x40, 0, 0},
+ {0x360c, 0x08, 0, 0},
+ {0x360f, 0xe5, 0, 0},
+ {0x3608, 0x8f, 0, 0},
+ {0x3611, 0x00, 0, 0},
+ {0x3613, 0xf7, 0, 0},
+ {0x3616, 0x58, 0, 0},
+ {0x3619, 0x99, 0, 0},
+ {0x361b, 0x60, 0, 0},
+ {0x361c, 0x7a, 0, 0},
+ {0x361e, 0x79, 0, 0},
+ {0x361f, 0x02, 0, 0},
+ {0x3632, 0x05, 0, 0},
+ {0x3633, 0x10, 0, 0},
+ {0x3634, 0x10, 0, 0},
+ {0x3635, 0x10, 0, 0},
+ {0x3636, 0x15, 0, 0},
+ {0x3646, 0x86, 0, 0},
+ {0x364a, 0x0b, 0, 0},
+ {0x3700, 0x17, 0, 0},
+ {0x3701, 0x22, 0, 0},
+ {0x3703, 0x10, 0, 0},
+ {0x370a, 0x37, 0, 0},
+ {0x3705, 0x00, 0, 0},
+ {0x3706, 0x63, 0, 0},
+ {0x3709, 0x3c, 0, 0},
+ {0x370b, 0x01, 0, 0},
+ {0x370c, 0x30, 0, 0},
+ {0x3710, 0x24, 0, 0},
+ {0x3711, 0x0c, 0, 0},
+ {0x3716, 0x00, 0, 0},
+ {0x3720, 0x28, 0, 0},
+ {0x3729, 0x7b, 0, 0},
+ {0x372a, 0x84, 0, 0},
+ {0x372b, 0xbd, 0, 0},
+ {0x372c, 0xbc, 0, 0},
+ {0x372e, 0x52, 0, 0},
+ {0x373c, 0x0e, 0, 0},
+ {0x373e, 0x33, 0, 0},
+ {0x3743, 0x10, 0, 0},
+ {0x3744, 0x88, 0, 0},
+ {0x3745, 0xc0, 0, 0},
+ {0x374a, 0x43, 0, 0},
+ {0x374c, 0x00, 0, 0},
+ {0x374e, 0x23, 0, 0},
+ {0x3751, 0x7b, 0, 0},
+ {0x3752, 0x84, 0, 0},
+ {0x3753, 0xbd, 0, 0},
+ {0x3754, 0xbc, 0, 0},
+ {0x3756, 0x52, 0, 0},
+ {0x375c, 0x00, 0, 0},
+ {0x3760, 0x00, 0, 0},
+ {0x3761, 0x00, 0, 0},
+ {0x3762, 0x00, 0, 0},
+ {0x3763, 0x00, 0, 0},
+ {0x3764, 0x00, 0, 0},
+ {0x3767, 0x04, 0, 0},
+ {0x3768, 0x04, 0, 0},
+ {0x3769, 0x08, 0, 0},
+ {0x376a, 0x08, 0, 0},
+ {0x376b, 0x40, 0, 0},
+ {0x376c, 0x00, 0, 0},
+ {0x376d, 0x00, 0, 0},
+ {0x376e, 0x00, 0, 0},
+ {0x3773, 0x00, 0, 0},
+ {0x3774, 0x51, 0, 0},
+ {0x3776, 0xbd, 0, 0},
+ {0x3777, 0xbd, 0, 0},
+ {0x3781, 0x18, 0, 0},
+ {0x3783, 0x25, 0, 0},
+ {0x3798, 0x1b, 0, 0},
+ {0x3800, 0x00, 0, 0},
+ {0x3801, 0x48, 0, 0},
+ {0x3802, 0x00, 0, 0},
+ {0x3803, 0x2C, 0, 0},
+ {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x57, 0, 0},
+ {0x3806, 0x05, 0, 0},
+ {0x3807, 0xD3, 0, 0},
+ {0x3808, 0x02, 0, 0},
+ {0x3809, 0x80, 0, 0},
+ {0x380a, 0x01, 0, 0},
+ {0x380b, 0xe0, 0, 0},
+
+ {0x380c, 0x02, 0, 0}, // 0a ; 03
+ {0x380d, 0x04, 0, 0}, // 1c ; 5C
+
+ {0x380e, 0x03, 0, 0},
+ {0x380f, 0x05, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x04, 0, 0},
+ {0x3812, 0x00, 0, 0},
+ {0x3813, 0x02, 0, 0},
+ {0x3814, 0x03, 0, 0},
+ {0x3815, 0x01, 0, 0},
+ {0x3819, 0x01, 0, 0},
+ {0x3820, 0x06, 0, 0},
+ {0x3821, 0x00, 0, 0},
+ {0x3829, 0x00, 0, 0},
+ {0x382a, 0x03, 0, 0},
+ {0x382b, 0x01, 0, 0},
+ {0x382d, 0x7f, 0, 0},
+ {0x3830, 0x08, 0, 0},
+ {0x3836, 0x02, 0, 0},
+ {0x3837, 0x00, 0, 0},
+ {0x3841, 0x02, 0, 0},
+ {0x3846, 0x08, 0, 0},
+ {0x3847, 0x07, 0, 0},
+ {0x3d85, 0x36, 0, 0},
+ {0x3d8c, 0x71, 0, 0},
+ {0x3d8d, 0xcb, 0, 0},
+ {0x3f0a, 0x00, 0, 0},
+ {0x4000, 0x71, 0, 0},
+ {0x4001, 0x50, 0, 0},
+ {0x4002, 0x04, 0, 0},
+ {0x4003, 0x14, 0, 0},
+ {0x400e, 0x00, 0, 0},
+ {0x4011, 0x00, 0, 0},
+ {0x401a, 0x00, 0, 0},
+ {0x401b, 0x00, 0, 0},
+ {0x401c, 0x00, 0, 0},
+ {0x401d, 0x00, 0, 0},
+ {0x401f, 0x00, 0, 0},
+ {0x4020, 0x00, 0, 0},
+ {0x4021, 0x10, 0, 0},
+ {0x4022, 0x03, 0, 0},
+ {0x4023, 0x93, 0, 0},
+ {0x4024, 0x04, 0, 0},
+ {0x4025, 0xC0, 0, 0},
+ {0x4026, 0x04, 0, 0},
+ {0x4027, 0xD0, 0, 0},
+ {0x4028, 0x00, 0, 0},
+ {0x4029, 0x02, 0, 0},
+ {0x402a, 0x06, 0, 0},
+ {0x402b, 0x04, 0, 0},
+ {0x402c, 0x02, 0, 0},
+ {0x402d, 0x02, 0, 0},
+ {0x402e, 0x0e, 0, 0},
+ {0x402f, 0x04, 0, 0},
+ {0x4302, 0xff, 0, 0},
+ {0x4303, 0xff, 0, 0},
+ {0x4304, 0x00, 0, 0},
+ {0x4305, 0x00, 0, 0},
+ {0x4306, 0x00, 0, 0},
+ {0x4308, 0x02, 0, 0},
+ {0x4500, 0x6c, 0, 0},
+ {0x4501, 0xc4, 0, 0},
+ {0x4502, 0x44, 0, 0},
+ {0x4503, 0x01, 0, 0},
+ {0x4600, 0x00, 0, 0},
+ {0x4601, 0x4F, 0, 0},
+ {0x4800, 0x04, 0, 0},
+ {0x4813, 0x08, 0, 0},
+ {0x481f, 0x40, 0, 0},
+ {0x4829, 0x78, 0, 0},
+ {0x4837, 0x10, 0, 0}, // 20 ; 10
+ {0x4b00, 0x2a, 0, 0},
+ {0x4b0d, 0x00, 0, 0},
+ {0x4d00, 0x04, 0, 0},
+ {0x4d01, 0x42, 0, 0},
+ {0x4d02, 0xd1, 0, 0},
+ {0x4d03, 0x93, 0, 0},
+ {0x4d04, 0xf5, 0, 0},
+ {0x4d05, 0xc1, 0, 0},
+ {0x5000, 0xf3, 0, 0},
+ {0x5001, 0x11, 0, 0},
+ {0x5004, 0x00, 0, 0},
+ {0x500a, 0x00, 0, 0},
+ {0x500b, 0x00, 0, 0},
+ {0x5032, 0x00, 0, 0},
+ {0x5040, 0x00, 0, 0},
+ {0x5050, 0x3c, 0, 0},
+ {0x5500, 0x00, 0, 0},
+ {0x5501, 0x10, 0, 0},
+ {0x5502, 0x01, 0, 0},
+ {0x5503, 0x0f, 0, 0},
+ {0x8000, 0x00, 0, 0},
+ {0x8001, 0x00, 0, 0},
+ {0x8002, 0x00, 0, 0},
+ {0x8003, 0x00, 0, 0},
+ {0x8004, 0x00, 0, 0},
+ {0x8005, 0x00, 0, 0},
+ {0x8006, 0x00, 0, 0},
+ {0x8007, 0x00, 0, 0},
+ {0x8008, 0x00, 0, 0},
+ {0x3638, 0x00, 0, 0},
+};
+
+static const struct reg_value ov4689_setting_720P_1280_720[] = {
+ //@@ RES_1280x720_2x_Bin_150fps_816Mbps
+ //OV4689_AM01B_1280x720_24M_2lane_816Mbps_150fps_20140210.txt
+ {0x0103, 0x01, 0, 0},
+ {0x3638, 0x00, 0, 0},
+ {0x0300, 0x00, 0, 0}, // 00
+ {0x0302, 0x22, 0, 0}, // 816Mbps 5a ; 64 ; 5a ; 78 ; 78 ; 2a
+ {0x0303, 0x00, 0, 0}, // 03 ; 01 ; 02 ;
+ {0x0304, 0x03, 0, 0},
+ {0x030b, 0x00, 0, 0},
+ {0x030d, 0x1e, 0, 0},
+ {0x030e, 0x04, 0, 0},
+ {0x030f, 0x01, 0, 0},
+ {0x0312, 0x01, 0, 0},
+ {0x031e, 0x00, 0, 0},
+ {0x3000, 0x20, 0, 0},
+ {0x3002, 0x00, 0, 0},
+ {0x3018, 0x32, 0, 0}, // 32/72 2lane/4lane
+ {0x3019, 0x0c, 0, 0}, // 0c/00 2lane/4lane
+ {0x3020, 0x93, 0, 0},
+ {0x3021, 0x03, 0, 0},
+ {0x3022, 0x01, 0, 0},
+ {0x3031, 0x0a, 0, 0},
+ {0x303f, 0x0c, 0, 0},
+ {0x3305, 0xf1, 0, 0},
+ {0x3307, 0x04, 0, 0},
+ {0x3309, 0x29, 0, 0},
+ {0x3500, 0x00, 0, 0},
+ {0x3501, 0x30, 0, 0},
+ {0x3502, 0x00, 0, 0},
+ {0x3503, 0x04, 0, 0},
+ {0x3504, 0x00, 0, 0},
+ {0x3505, 0x00, 0, 0},
+ {0x3506, 0x00, 0, 0},
+ {0x3507, 0x00, 0, 0},
+ {0x3508, 0x07, 0, 0},
+ {0x3509, 0x78, 0, 0}, // 8X
+ {0x350a, 0x00, 0, 0},
+ {0x350b, 0x00, 0, 0},
+ {0x350c, 0x00, 0, 0},
+ {0x350d, 0x00, 0, 0},
+ {0x350e, 0x00, 0, 0},
+ {0x350f, 0x80, 0, 0},
+ {0x3510, 0x00, 0, 0},
+ {0x3511, 0x00, 0, 0},
+ {0x3512, 0x00, 0, 0},
+ {0x3513, 0x00, 0, 0},
+ {0x3514, 0x00, 0, 0},
+ {0x3515, 0x80, 0, 0},
+ {0x3516, 0x00, 0, 0},
+ {0x3517, 0x00, 0, 0},
+ {0x3518, 0x00, 0, 0},
+ {0x3519, 0x00, 0, 0},
+ {0x351a, 0x00, 0, 0},
+ {0x351b, 0x80, 0, 0},
+ {0x351c, 0x00, 0, 0},
+ {0x351d, 0x00, 0, 0},
+ {0x351e, 0x00, 0, 0},
+ {0x351f, 0x00, 0, 0},
+ {0x3520, 0x00, 0, 0},
+ {0x3521, 0x80, 0, 0},
+ {0x3522, 0x08, 0, 0},
+ {0x3524, 0x08, 0, 0},
+ {0x3526, 0x08, 0, 0},
+ {0x3528, 0x08, 0, 0},
+ {0x352a, 0x08, 0, 0},
+ {0x3602, 0x00, 0, 0},
+ {0x3603, 0x40, 0, 0},
+ {0x3604, 0x02, 0, 0},
+ {0x3605, 0x00, 0, 0},
+ {0x3606, 0x00, 0, 0},
+ {0x3607, 0x00, 0, 0},
+ {0x3609, 0x12, 0, 0},
+ {0x360a, 0x40, 0, 0},
+ {0x360c, 0x08, 0, 0},
+ {0x360f, 0xe5, 0, 0},
+ {0x3608, 0x8f, 0, 0},
+ {0x3611, 0x00, 0, 0},
+ {0x3613, 0xf7, 0, 0},
+ {0x3616, 0x58, 0, 0},
+ {0x3619, 0x99, 0, 0},
+ {0x361b, 0x60, 0, 0},
+ {0x361c, 0x7a, 0, 0},
+ {0x361e, 0x79, 0, 0},
+ {0x361f, 0x02, 0, 0},
+ {0x3632, 0x05, 0, 0},
+ {0x3633, 0x10, 0, 0},
+ {0x3634, 0x10, 0, 0},
+ {0x3635, 0x10, 0, 0},
+ {0x3636, 0x15, 0, 0},
+ {0x3646, 0x86, 0, 0},
+ {0x364a, 0x0b, 0, 0},
+ {0x3700, 0x17, 0, 0},
+ {0x3701, 0x22, 0, 0},
+ {0x3703, 0x10, 0, 0},
+ {0x370a, 0x37, 0, 0},
+ {0x3705, 0x00, 0, 0},
+ {0x3706, 0x63, 0, 0},
+ {0x3709, 0x3c, 0, 0},
+ {0x370b, 0x01, 0, 0},
+ {0x370c, 0x30, 0, 0},
+ {0x3710, 0x24, 0, 0},
+ {0x3711, 0x0c, 0, 0},
+ {0x3716, 0x00, 0, 0},
+ {0x3720, 0x28, 0, 0},
+ {0x3729, 0x7b, 0, 0},
+ {0x372a, 0x84, 0, 0},
+ {0x372b, 0xbd, 0, 0},
+ {0x372c, 0xbc, 0, 0},
+ {0x372e, 0x52, 0, 0},
+ {0x373c, 0x0e, 0, 0},
+ {0x373e, 0x33, 0, 0},
+ {0x3743, 0x10, 0, 0},
+ {0x3744, 0x88, 0, 0},
+ {0x3745, 0xc0, 0, 0},
+ {0x374a, 0x43, 0, 0},
+ {0x374c, 0x00, 0, 0},
+ {0x374e, 0x23, 0, 0},
+ {0x3751, 0x7b, 0, 0},
+ {0x3752, 0x84, 0, 0},
+ {0x3753, 0xbd, 0, 0},
+ {0x3754, 0xbc, 0, 0},
+ {0x3756, 0x52, 0, 0},
+ {0x375c, 0x00, 0, 0},
+ {0x3760, 0x00, 0, 0},
+ {0x3761, 0x00, 0, 0},
+ {0x3762, 0x00, 0, 0},
+ {0x3763, 0x00, 0, 0},
+ {0x3764, 0x00, 0, 0},
+ {0x3767, 0x04, 0, 0},
+ {0x3768, 0x04, 0, 0},
+ {0x3769, 0x08, 0, 0},
+ {0x376a, 0x08, 0, 0},
+ {0x376b, 0x40, 0, 0},
+ {0x376c, 0x00, 0, 0},
+ {0x376d, 0x00, 0, 0},
+ {0x376e, 0x00, 0, 0},
+ {0x3773, 0x00, 0, 0},
+ {0x3774, 0x51, 0, 0},
+ {0x3776, 0xbd, 0, 0},
+ {0x3777, 0xbd, 0, 0},
+ {0x3781, 0x18, 0, 0},
+ {0x3783, 0x25, 0, 0},
+ {0x3798, 0x1b, 0, 0},
+ {0x3800, 0x00, 0, 0},
+ {0x3801, 0x48, 0, 0},
+ {0x3802, 0x00, 0, 0},
+ {0x3803, 0x2C, 0, 0},
+ {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x57, 0, 0},
+ {0x3806, 0x05, 0, 0},
+ {0x3807, 0xD3, 0, 0},
+ {0x3808, 0x05, 0, 0},
+ {0x3809, 0x00, 0, 0},
+ {0x380a, 0x02, 0, 0},
+ {0x380b, 0xD0, 0, 0},
+#if 1
+ {0x380c, 0x04, 0, 0}, // 0a ; 03
+ {0x380d, 0x08, 0, 0}, // 1c ; 5C
+#else
+ {0x380c, 0x05, 0, 0}, // 120fps
+ {0x380d, 0x0A, 0, 0},
+#endif
+ {0x380e, 0x03, 0, 0},
+ {0x380f, 0x05, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x04, 0, 0},
+ {0x3812, 0x00, 0, 0},
+ {0x3813, 0x02, 0, 0},
+ {0x3814, 0x03, 0, 0},
+ {0x3815, 0x01, 0, 0},
+ {0x3819, 0x01, 0, 0},
+ {0x3820, 0x06, 0, 0},
+ {0x3821, 0x00, 0, 0},
+ {0x3829, 0x00, 0, 0},
+ {0x382a, 0x03, 0, 0},
+ {0x382b, 0x01, 0, 0},
+ {0x382d, 0x7f, 0, 0},
+ {0x3830, 0x08, 0, 0},
+ {0x3836, 0x02, 0, 0},
+ {0x3837, 0x00, 0, 0},
+ {0x3841, 0x02, 0, 0},
+ {0x3846, 0x08, 0, 0},
+ {0x3847, 0x07, 0, 0},
+ {0x3d85, 0x36, 0, 0},
+ {0x3d8c, 0x71, 0, 0},
+ {0x3d8d, 0xcb, 0, 0},
+ {0x3f0a, 0x00, 0, 0},
+ {0x4000, 0x71, 0, 0},
+ {0x4001, 0x50, 0, 0},
+ {0x4002, 0x04, 0, 0},
+ {0x4003, 0x14, 0, 0},
+ {0x400e, 0x00, 0, 0},
+ {0x4011, 0x00, 0, 0},
+ {0x401a, 0x00, 0, 0},
+ {0x401b, 0x00, 0, 0},
+ {0x401c, 0x00, 0, 0},
+ {0x401d, 0x00, 0, 0},
+ {0x401f, 0x00, 0, 0},
+ {0x4020, 0x00, 0, 0},
+ {0x4021, 0x10, 0, 0},
+ {0x4022, 0x03, 0, 0},
+ {0x4023, 0x93, 0, 0},
+ {0x4024, 0x04, 0, 0},
+ {0x4025, 0xC0, 0, 0},
+ {0x4026, 0x04, 0, 0},
+ {0x4027, 0xD0, 0, 0},
+ {0x4028, 0x00, 0, 0},
+ {0x4029, 0x02, 0, 0},
+ {0x402a, 0x06, 0, 0},
+ {0x402b, 0x04, 0, 0},
+ {0x402c, 0x02, 0, 0},
+ {0x402d, 0x02, 0, 0},
+ {0x402e, 0x0e, 0, 0},
+ {0x402f, 0x04, 0, 0},
+ {0x4302, 0xff, 0, 0},
+ {0x4303, 0xff, 0, 0},
+ {0x4304, 0x00, 0, 0},
+ {0x4305, 0x00, 0, 0},
+ {0x4306, 0x00, 0, 0},
+ {0x4308, 0x02, 0, 0},
+ {0x4500, 0x6c, 0, 0},
+ {0x4501, 0xc4, 0, 0},
+ {0x4502, 0x44, 0, 0},
+ {0x4503, 0x01, 0, 0},
+ {0x4600, 0x00, 0, 0},
+ {0x4601, 0x4F, 0, 0},
+ {0x4800, 0x04, 0, 0},
+ {0x4813, 0x08, 0, 0},
+ {0x481f, 0x40, 0, 0},
+ {0x4829, 0x78, 0, 0},
+ {0x4837, 0x10, 0, 0}, // 20 ; 10
+ {0x4b00, 0x2a, 0, 0},
+ {0x4b0d, 0x00, 0, 0},
+ {0x4d00, 0x04, 0, 0},
+ {0x4d01, 0x42, 0, 0},
+ {0x4d02, 0xd1, 0, 0},
+ {0x4d03, 0x93, 0, 0},
+ {0x4d04, 0xf5, 0, 0},
+ {0x4d05, 0xc1, 0, 0},
+ {0x5000, 0xf3, 0, 0},
+ {0x5001, 0x11, 0, 0},
+ {0x5004, 0x00, 0, 0},
+ {0x500a, 0x00, 0, 0},
+ {0x500b, 0x00, 0, 0},
+ {0x5032, 0x00, 0, 0},
+ {0x5040, 0x00, 0, 0},
+ {0x5050, 0x3c, 0, 0},
+ {0x5500, 0x00, 0, 0},
+ {0x5501, 0x10, 0, 0},
+ {0x5502, 0x01, 0, 0},
+ {0x5503, 0x0f, 0, 0},
+ {0x8000, 0x00, 0, 0},
+ {0x8001, 0x00, 0, 0},
+ {0x8002, 0x00, 0, 0},
+ {0x8003, 0x00, 0, 0},
+ {0x8004, 0x00, 0, 0},
+ {0x8005, 0x00, 0, 0},
+ {0x8006, 0x00, 0, 0},
+ {0x8007, 0x00, 0, 0},
+ {0x8008, 0x00, 0, 0},
+ {0x3638, 0x00, 0, 0},
+};
+
+static const struct reg_value ov4689_setting_1080P_1920_1080[] = {
+ //@@ RES_1920x1080_60fps_816Mbps 2lanes
+ {0x0103, 0x01, 0, 0},
+ {0x3638, 0x00, 0, 0},
+ {0x0300, 0x00, 0, 0}, // clk
+ {0x0302, 0x22, 0, 0},
+ {0x0303, 0x00, 0, 0},
+ {0x0304, 0x03, 0, 0},
+ {0x030b, 0x00, 0, 0},
+ {0x030d, 0x1e, 0, 0},
+ {0x030e, 0x04, 0, 0},
+ {0x030f, 0x01, 0, 0},
+ {0x0312, 0x01, 0, 0},
+ {0x031e, 0x00, 0, 0},
+ {0x3000, 0x20, 0, 0},
+ {0x3002, 0x00, 0, 0},
+ {0x3018, 0x32, 0, 0},
+ {0x3019, 0x0c, 0, 0},
+ {0x3020, 0x93, 0, 0},
+ {0x3021, 0x03, 0, 0},
+ {0x3022, 0x01, 0, 0},
+ {0x3031, 0x0a, 0, 0},
+ {0x303f, 0x0c, 0, 0},
+ {0x3305, 0xf1, 0, 0},
+ {0x3307, 0x04, 0, 0},
+ {0x3309, 0x29, 0, 0},
+ {0x3500, 0x00, 0, 0}, // AEC
+ {0x3501, 0x4c, 0, 0},
+ {0x3502, 0x00, 0, 0},
+ {0x3503, 0x04, 0, 0},
+ {0x3504, 0x00, 0, 0},
+ {0x3505, 0x00, 0, 0},
+ {0x3506, 0x00, 0, 0},
+ {0x3507, 0x00, 0, 0},
+ {0x3508, 0x00, 0, 0},
+ {0x3509, 0x80, 0, 0},
+ {0x350a, 0x00, 0, 0},
+ {0x350b, 0x00, 0, 0},
+ {0x350c, 0x00, 0, 0},
+ {0x350d, 0x00, 0, 0},
+ {0x350e, 0x00, 0, 0},
+ {0x350f, 0x80, 0, 0},
+ {0x3510, 0x00, 0, 0},
+ {0x3511, 0x00, 0, 0},
+ {0x3512, 0x00, 0, 0},
+ {0x3513, 0x00, 0, 0},
+ {0x3514, 0x00, 0, 0},
+ {0x3515, 0x80, 0, 0},
+ {0x3516, 0x00, 0, 0},
+ {0x3517, 0x00, 0, 0},
+ {0x3518, 0x00, 0, 0},
+ {0x3519, 0x00, 0, 0},
+ {0x351a, 0x00, 0, 0},
+ {0x351b, 0x80, 0, 0},
+ {0x351c, 0x00, 0, 0},
+ {0x351d, 0x00, 0, 0},
+ {0x351e, 0x00, 0, 0},
+ {0x351f, 0x00, 0, 0},
+ {0x3520, 0x00, 0, 0},
+ {0x3521, 0x80, 0, 0},
+ {0x3522, 0x08, 0, 0},
+ {0x3524, 0x08, 0, 0},
+ {0x3526, 0x08, 0, 0},
+ {0x3528, 0x08, 0, 0},
+ {0x352a, 0x08, 0, 0},
+ {0x3602, 0x00, 0, 0},
+ {0x3603, 0x40, 0, 0},
+ {0x3604, 0x02, 0, 0},
+ {0x3605, 0x00, 0, 0},
+ {0x3606, 0x00, 0, 0},
+ {0x3607, 0x00, 0, 0},
+ {0x3609, 0x12, 0, 0},
+ {0x360a, 0x40, 0, 0},
+ {0x360c, 0x08, 0, 0},
+ {0x360f, 0xe5, 0, 0},
+ {0x3608, 0x8f, 0, 0},
+ {0x3611, 0x00, 0, 0},
+ {0x3613, 0xf7, 0, 0},
+ {0x3616, 0x58, 0, 0},
+ {0x3619, 0x99, 0, 0},
+ {0x361b, 0x60, 0, 0},
+ {0x361c, 0x7a, 0, 0},
+ {0x361e, 0x79, 0, 0},
+ {0x361f, 0x02, 0, 0},
+ {0x3632, 0x00, 0, 0},
+ {0x3633, 0x10, 0, 0},
+ {0x3634, 0x10, 0, 0},
+ {0x3635, 0x10, 0, 0},
+ {0x3636, 0x15, 0, 0},
+ {0x3646, 0x86, 0, 0},
+ {0x364a, 0x0b, 0, 0},
+ {0x3700, 0x17, 0, 0},
+ {0x3701, 0x22, 0, 0},
+ {0x3703, 0x10, 0, 0},
+ {0x370a, 0x37, 0, 0},
+ {0x3705, 0x00, 0, 0},
+ {0x3706, 0x63, 0, 0},
+ {0x3709, 0x3c, 0, 0},
+ {0x370b, 0x01, 0, 0},
+ {0x370c, 0x30, 0, 0},
+ {0x3710, 0x24, 0, 0},
+ {0x3711, 0x0c, 0, 0},
+ {0x3716, 0x00, 0, 0},
+ {0x3720, 0x28, 0, 0},
+ {0x3729, 0x7b, 0, 0},
+ {0x372a, 0x84, 0, 0},
+ {0x372b, 0xbd, 0, 0},
+ {0x372c, 0xbc, 0, 0},
+ {0x372e, 0x52, 0, 0},
+ {0x373c, 0x0e, 0, 0},
+ {0x373e, 0x33, 0, 0},
+ {0x3743, 0x10, 0, 0},
+ {0x3744, 0x88, 0, 0},
+ {0x3745, 0xc0, 0, 0},
+ {0x374a, 0x43, 0, 0},
+ {0x374c, 0x00, 0, 0},
+ {0x374e, 0x23, 0, 0},
+ {0x3751, 0x7b, 0, 0},
+ {0x3752, 0x84, 0, 0},
+ {0x3753, 0xbd, 0, 0},
+ {0x3754, 0xbc, 0, 0},
+ {0x3756, 0x52, 0, 0},
+ {0x375c, 0x00, 0, 0},
+ {0x3760, 0x00, 0, 0},
+ {0x3761, 0x00, 0, 0},
+ {0x3762, 0x00, 0, 0},
+ {0x3763, 0x00, 0, 0},
+ {0x3764, 0x00, 0, 0},
+ {0x3767, 0x04, 0, 0},
+ {0x3768, 0x04, 0, 0},
+ {0x3769, 0x08, 0, 0},
+ {0x376a, 0x08, 0, 0},
+ {0x376b, 0x20, 0, 0},
+ {0x376c, 0x00, 0, 0},
+ {0x376d, 0x00, 0, 0},
+ {0x376e, 0x00, 0, 0},
+ {0x3773, 0x00, 0, 0},
+ {0x3774, 0x51, 0, 0},
+ {0x3776, 0xbd, 0, 0},
+ {0x3777, 0xbd, 0, 0},
+ {0x3781, 0x18, 0, 0},
+ {0x3783, 0x25, 0, 0},
+ {0x3798, 0x1b, 0, 0},
+ {0x3800, 0x01, 0, 0}, // timings
+ {0x3801, 0x88, 0, 0},
+ {0x3802, 0x00, 0, 0},
+ {0x3803, 0xe0, 0, 0},
+ {0x3804, 0x09, 0, 0},
+ {0x3805, 0x17, 0, 0},
+ {0x3806, 0x05, 0, 0},
+ {0x3807, 0x1f, 0, 0},
+ {0x3808, 0x07, 0, 0},
+ {0x3809, 0x80, 0, 0},
+ {0x380a, 0x04, 0, 0},
+ {0x380b, 0x38, 0, 0},
+ {0x380c, 0x06, 0, 0},
+ {0x380d, 0xe0, 0, 0},
+ {0x380e, 0x04, 0, 0},
+ {0x380f, 0x70, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x08, 0, 0},
+ {0x3812, 0x00, 0, 0},
+ {0x3813, 0x04, 0, 0},
+ {0x3814, 0x01, 0, 0},
+ {0x3815, 0x01, 0, 0},
+ {0x3819, 0x01, 0, 0},
+ {0x3820, 0x06, 0, 0},
+ {0x3821, 0x00, 0, 0},
+ {0x3829, 0x00, 0, 0},
+ {0x382a, 0x01, 0, 0},
+ {0x382b, 0x01, 0, 0},
+ {0x382d, 0x7f, 0, 0},
+ {0x3830, 0x04, 0, 0},
+ {0x3836, 0x01, 0, 0},
+ {0x3837, 0x00, 0, 0},
+ {0x3841, 0x02, 0, 0},
+ {0x3846, 0x08, 0, 0},
+ {0x3847, 0x07, 0, 0},
+ {0x3d85, 0x36, 0, 0},
+ {0x3d8c, 0x71, 0, 0},
+ {0x3d8d, 0xcb, 0, 0},
+ {0x3f0a, 0x00, 0, 0},
+ {0x4000, 0xf1, 0, 0},
+ {0x4001, 0x40, 0, 0},
+ {0x4002, 0x04, 0, 0},
+ {0x4003, 0x14, 0, 0},
+ {0x400e, 0x00, 0, 0},
+ {0x4011, 0x00, 0, 0},
+ {0x401a, 0x00, 0, 0},
+ {0x401b, 0x00, 0, 0},
+ {0x401c, 0x00, 0, 0},
+ {0x401d, 0x00, 0, 0},
+ {0x401f, 0x00, 0, 0},
+ {0x4020, 0x00, 0, 0},
+ {0x4021, 0x10, 0, 0},
+ {0x4022, 0x06, 0, 0},
+ {0x4023, 0x13, 0, 0},
+ {0x4024, 0x07, 0, 0},
+ {0x4025, 0x40, 0, 0},
+ {0x4026, 0x07, 0, 0},
+ {0x4027, 0x50, 0, 0},
+ {0x4028, 0x00, 0, 0},
+ {0x4029, 0x02, 0, 0},
+ {0x402a, 0x06, 0, 0},
+ {0x402b, 0x04, 0, 0},
+ {0x402c, 0x02, 0, 0},
+ {0x402d, 0x02, 0, 0},
+ {0x402e, 0x0e, 0, 0},
+ {0x402f, 0x04, 0, 0},
+ {0x4302, 0xff, 0, 0},
+ {0x4303, 0xff, 0, 0},
+ {0x4304, 0x00, 0, 0},
+ {0x4305, 0x00, 0, 0},
+ {0x4306, 0x00, 0, 0},
+ {0x4308, 0x02, 0, 0},
+ {0x4500, 0x6c, 0, 0},
+ {0x4501, 0xc4, 0, 0},
+ {0x4502, 0x40, 0, 0},
+ {0x4503, 0x01, 0, 0},
+ {0x4601, 0x77, 0, 0},
+ {0x4800, 0x04, 0, 0},
+ {0x4813, 0x08, 0, 0},
+ {0x481f, 0x40, 0, 0},
+ {0x4829, 0x78, 0, 0},
+ {0x4837, 0x10, 0, 0},
+ {0x4b00, 0x2a, 0, 0},
+ {0x4b0d, 0x00, 0, 0},
+ {0x4d00, 0x04, 0, 0},
+ {0x4d01, 0x42, 0, 0},
+ {0x4d02, 0xd1, 0, 0},
+ {0x4d03, 0x93, 0, 0},
+ {0x4d04, 0xf5, 0, 0},
+ {0x4d05, 0xc1, 0, 0},
+ {0x5000, 0xf3, 0, 0},
+ {0x5001, 0x11, 0, 0},
+ {0x5004, 0x00, 0, 0},
+ {0x500a, 0x00, 0, 0},
+ {0x500b, 0x00, 0, 0},
+ {0x5032, 0x00, 0, 0},
+ {0x5040, 0x00, 0, 0},
+ {0x5050, 0x0c, 0, 0},
+ {0x5500, 0x00, 0, 0},
+ {0x5501, 0x10, 0, 0},
+ {0x5502, 0x01, 0, 0},
+ {0x5503, 0x0f, 0, 0},
+ {0x8000, 0x00, 0, 0},
+ {0x8001, 0x00, 0, 0},
+ {0x8002, 0x00, 0, 0},
+ {0x8003, 0x00, 0, 0},
+ {0x8004, 0x00, 0, 0},
+ {0x8005, 0x00, 0, 0},
+ {0x8006, 0x00, 0, 0},
+ {0x8007, 0x00, 0, 0},
+ {0x8008, 0x00, 0, 0},
+ {0x3638, 0x00, 0, 0},
+};
+
+static const struct reg_value ov4689_setting_4M_2688_1520[] = {
+ //@@ 0 10 RES_2688x1520_default(60fps)
+ //102 2630 960
+ {0x0103, 0x01, 0, 0},
+ {0x3638, 0x00, 0, 0},
+ {0x0300, 0x00, 0, 0},
+ {0x0302, 0x22, 0, 0}, // 2a ;1008Mbps,23 ;; 840Mbps
+ {0x0304, 0x03, 0, 0},
+ {0x030b, 0x00, 0, 0},
+ {0x030d, 0x1e, 0, 0},
+ {0x030e, 0x04, 0, 0},
+ {0x030f, 0x01, 0, 0},
+ {0x0312, 0x01, 0, 0},
+ {0x031e, 0x00, 0, 0},
+ {0x3000, 0x20, 0, 0},
+ {0x3002, 0x00, 0, 0},
+ {0x3018, 0x32, 0, 0},
+ {0x3019, 0x0C, 0, 0},
+ {0x3020, 0x93, 0, 0},
+ {0x3021, 0x03, 0, 0},
+ {0x3022, 0x01, 0, 0},
+ {0x3031, 0x0a, 0, 0},
+ {0x303f, 0x0c, 0, 0},
+ {0x3305, 0xf1, 0, 0},
+ {0x3307, 0x04, 0, 0},
+ {0x3309, 0x29, 0, 0},
+ {0x3500, 0x00, 0, 0},
+ {0x3501, 0x60, 0, 0},
+ {0x3502, 0x00, 0, 0},
+ {0x3503, 0x04, 0, 0},
+ {0x3504, 0x00, 0, 0},
+ {0x3505, 0x00, 0, 0},
+ {0x3506, 0x00, 0, 0},
+ {0x3507, 0x00, 0, 0},
+ {0x3508, 0x00, 0, 0},
+ {0x3509, 0x80, 0, 0},
+ {0x350a, 0x00, 0, 0},
+ {0x350b, 0x00, 0, 0},
+ {0x350c, 0x00, 0, 0},
+ {0x350d, 0x00, 0, 0},
+ {0x350e, 0x00, 0, 0},
+ {0x350f, 0x80, 0, 0},
+ {0x3510, 0x00, 0, 0},
+ {0x3511, 0x00, 0, 0},
+ {0x3512, 0x00, 0, 0},
+ {0x3513, 0x00, 0, 0},
+ {0x3514, 0x00, 0, 0},
+ {0x3515, 0x80, 0, 0},
+ {0x3516, 0x00, 0, 0},
+ {0x3517, 0x00, 0, 0},
+ {0x3518, 0x00, 0, 0},
+ {0x3519, 0x00, 0, 0},
+ {0x351a, 0x00, 0, 0},
+ {0x351b, 0x80, 0, 0},
+ {0x351c, 0x00, 0, 0},
+ {0x351d, 0x00, 0, 0},
+ {0x351e, 0x00, 0, 0},
+ {0x351f, 0x00, 0, 0},
+ {0x3520, 0x00, 0, 0},
+ {0x3521, 0x80, 0, 0},
+ {0x3522, 0x08, 0, 0},
+ {0x3524, 0x08, 0, 0},
+ {0x3526, 0x08, 0, 0},
+ {0x3528, 0x08, 0, 0},
+ {0x352a, 0x08, 0, 0},
+ {0x3602, 0x00, 0, 0},
+ {0x3603, 0x40, 0, 0},
+ {0x3604, 0x02, 0, 0},
+ {0x3605, 0x00, 0, 0},
+ {0x3606, 0x00, 0, 0},
+ {0x3607, 0x00, 0, 0},
+ {0x3609, 0x12, 0, 0},
+ {0x360a, 0x40, 0, 0},
+ {0x360c, 0x08, 0, 0},
+ {0x360f, 0xe5, 0, 0},
+ {0x3608, 0x8f, 0, 0},
+ {0x3611, 0x00, 0, 0},
+ {0x3613, 0xf7, 0, 0},
+ {0x3616, 0x58, 0, 0},
+ {0x3619, 0x99, 0, 0},
+ {0x361b, 0x60, 0, 0},
+ {0x361c, 0x7a, 0, 0},
+ {0x361e, 0x79, 0, 0},
+ {0x361f, 0x02, 0, 0},
+ {0x3632, 0x00, 0, 0},
+ {0x3633, 0x10, 0, 0},
+ {0x3634, 0x10, 0, 0},
+ {0x3635, 0x10, 0, 0},
+ {0x3636, 0x15, 0, 0},
+ {0x3646, 0x86, 0, 0},
+ {0x364a, 0x0b, 0, 0},
+ {0x3700, 0x17, 0, 0},
+ {0x3701, 0x22, 0, 0},
+ {0x3703, 0x10, 0, 0},
+ {0x370a, 0x37, 0, 0},
+ {0x3705, 0x00, 0, 0},
+ {0x3706, 0x63, 0, 0},
+ {0x3709, 0x3c, 0, 0},
+ {0x370b, 0x01, 0, 0},
+ {0x370c, 0x30, 0, 0},
+ {0x3710, 0x24, 0, 0},
+ {0x3711, 0x0c, 0, 0},
+ {0x3716, 0x00, 0, 0},
+ {0x3720, 0x28, 0, 0},
+ {0x3729, 0x7b, 0, 0},
+ {0x372a, 0x84, 0, 0},
+ {0x372b, 0xbd, 0, 0},
+ {0x372c, 0xbc, 0, 0},
+ {0x372e, 0x52, 0, 0},
+ {0x373c, 0x0e, 0, 0},
+ {0x373e, 0x33, 0, 0},
+ {0x3743, 0x10, 0, 0},
+ {0x3744, 0x88, 0, 0},
+ {0x3745, 0xc0, 0, 0},
+ {0x374a, 0x43, 0, 0},
+ {0x374c, 0x00, 0, 0},
+ {0x374e, 0x23, 0, 0},
+ {0x3751, 0x7b, 0, 0},
+ {0x3752, 0x84, 0, 0},
+ {0x3753, 0xbd, 0, 0},
+ {0x3754, 0xbc, 0, 0},
+ {0x3756, 0x52, 0, 0},
+ {0x375c, 0x00, 0, 0},
+ {0x3760, 0x00, 0, 0},
+ {0x3761, 0x00, 0, 0},
+ {0x3762, 0x00, 0, 0},
+ {0x3763, 0x00, 0, 0},
+ {0x3764, 0x00, 0, 0},
+ {0x3767, 0x04, 0, 0},
+ {0x3768, 0x04, 0, 0},
+ {0x3769, 0x08, 0, 0},
+ {0x376a, 0x08, 0, 0},
+ {0x376b, 0x20, 0, 0},
+ {0x376c, 0x00, 0, 0},
+ {0x376d, 0x00, 0, 0},
+ {0x376e, 0x00, 0, 0},
+ {0x3773, 0x00, 0, 0},
+ {0x3774, 0x51, 0, 0},
+ {0x3776, 0xbd, 0, 0},
+ {0x3777, 0xbd, 0, 0},
+ {0x3781, 0x18, 0, 0},
+ {0x3783, 0x25, 0, 0},
+ {0x3798, 0x1b, 0, 0},
+ {0x3800, 0x00, 0, 0},
+ {0x3801, 0x08, 0, 0},
+ {0x3802, 0x00, 0, 0},
+ {0x3803, 0x04, 0, 0},
+ {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x97, 0, 0},
+ {0x3806, 0x05, 0, 0},
+ {0x3807, 0xfb, 0, 0},
+ {0x3808, 0x0a, 0, 0},
+ {0x3809, 0x80, 0, 0},
+ {0x380a, 0x05, 0, 0},
+ {0x380b, 0xf0, 0, 0},
+ {0x380c, 0x03, 0, 0},
+ {0x380d, 0x5c, 0, 0},
+ {0x380e, 0x06, 0, 0},
+ {0x380f, 0x12, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x08, 0, 0},
+ {0x3812, 0x00, 0, 0},
+ {0x3813, 0x04, 0, 0},
+ {0x3814, 0x01, 0, 0},
+ {0x3815, 0x01, 0, 0},
+ {0x3819, 0x01, 0, 0},
+ {0x3820, 0x00, 0, 0},
+ {0x3821, 0x06, 0, 0},
+ {0x3829, 0x00, 0, 0},
+ {0x382a, 0x01, 0, 0},
+ {0x382b, 0x01, 0, 0},
+ {0x382d, 0x7f, 0, 0},
+ {0x3830, 0x04, 0, 0},
+ {0x3836, 0x01, 0, 0},
+ {0x3837, 0x00, 0, 0},
+ {0x3841, 0x02, 0, 0},
+ {0x3846, 0x08, 0, 0},
+ {0x3847, 0x07, 0, 0},
+ {0x3d85, 0x36, 0, 0},
+ {0x3d8c, 0x71, 0, 0},
+ {0x3d8d, 0xcb, 0, 0},
+ {0x3f0a, 0x00, 0, 0},
+ {0x4000, 0x71, 0, 0},
+ {0x4001, 0x40, 0, 0},
+ {0x4002, 0x04, 0, 0},
+ {0x4003, 0x14, 0, 0},
+ {0x400e, 0x00, 0, 0},
+ {0x4011, 0x00, 0, 0},
+ {0x401a, 0x00, 0, 0},
+ {0x401b, 0x00, 0, 0},
+ {0x401c, 0x00, 0, 0},
+ {0x401d, 0x00, 0, 0},
+ {0x401f, 0x00, 0, 0},
+ {0x4020, 0x00, 0, 0},
+ {0x4021, 0x10, 0, 0},
+ {0x4022, 0x07, 0, 0},
+ {0x4023, 0xcf, 0, 0},
+ {0x4024, 0x09, 0, 0},
+ {0x4025, 0x60, 0, 0},
+ {0x4026, 0x09, 0, 0},
+ {0x4027, 0x6f, 0, 0},
+ {0x4028, 0x00, 0, 0},
+ {0x4029, 0x02, 0, 0},
+ {0x402a, 0x06, 0, 0},
+ {0x402b, 0x04, 0, 0},
+ {0x402c, 0x02, 0, 0},
+ {0x402d, 0x02, 0, 0},
+ {0x402e, 0x0e, 0, 0},
+ {0x402f, 0x04, 0, 0},
+ {0x4302, 0xff, 0, 0},
+ {0x4303, 0xff, 0, 0},
+ {0x4304, 0x00, 0, 0},
+ {0x4305, 0x00, 0, 0},
+ {0x4306, 0x00, 0, 0},
+ {0x4308, 0x02, 0, 0},
+ {0x4500, 0x6c, 0, 0},
+ {0x4501, 0xc4, 0, 0},
+ {0x4502, 0x40, 0, 0},
+ {0x4503, 0x01, 0, 0},
+ {0x4601, 0x04, 0, 0},
+ {0x4800, 0x04, 0, 0},
+ {0x4813, 0x08, 0, 0},
+ {0x481f, 0x40, 0, 0},
+ {0x4829, 0x78, 0, 0},
+ {0x4837, 0x14, 0, 0}, // 10
+ {0x4b00, 0x2a, 0, 0},
+ {0x4b0d, 0x00, 0, 0},
+ {0x4d00, 0x04, 0, 0},
+ {0x4d01, 0x42, 0, 0},
+ {0x4d02, 0xd1, 0, 0},
+ {0x4d03, 0x93, 0, 0},
+ {0x4d04, 0xf5, 0, 0},
+ {0x4d05, 0xc1, 0, 0},
+ {0x5000, 0xf3, 0, 0},
+ {0x5001, 0x11, 0, 0},
+ {0x5004, 0x00, 0, 0},
+ {0x500a, 0x00, 0, 0},
+ {0x500b, 0x00, 0, 0},
+ {0x5032, 0x00, 0, 0},
+ {0x5040, 0x00, 0, 0},
+ {0x5050, 0x0c, 0, 0},
+ {0x5500, 0x00, 0, 0},
+ {0x5501, 0x10, 0, 0},
+ {0x5502, 0x01, 0, 0},
+ {0x5503, 0x0f, 0, 0},
+ {0x8000, 0x00, 0, 0},
+ {0x8001, 0x00, 0, 0},
+ {0x8002, 0x00, 0, 0},
+ {0x8003, 0x00, 0, 0},
+ {0x8004, 0x00, 0, 0},
+ {0x8005, 0x00, 0, 0},
+ {0x8006, 0x00, 0, 0},
+ {0x8007, 0x00, 0, 0},
+ {0x8008, 0x00, 0, 0},
+ {0x3638, 0x00, 0, 0},
+// {0x0100, 0x01, 0, 0},
+
+// {0x0100, 0x00, 0, 0},
+ {0x380c, 0x0A, 0, 0}, // 05
+ {0x380d, 0x0A, 0, 0}, // 10
+ {0x380e, 0x06, 0, 0},
+ {0x380f, 0x12, 0, 0},
+// {0x0100, 0x01, 0, 0},
+ {0x3105, 0x31, 0, 0},
+ {0x301a, 0xf9, 0, 0},
+ {0x3508, 0x07, 0, 0},
+ {0x484b, 0x05, 0, 0},
+ {0x4805, 0x03, 0, 0},
+ {0x3601, 0x01, 0, 0},
+ {0x3745, 0xc0, 0, 0},
+ {0x3798, 0x1b, 0, 0},
+// {0x0100, 0x01, 0, 0},
+ {0xffff, 0x0a, 0, 0},
+ {0x3105, 0x11, 0, 0},
+ {0x301a, 0xf1, 0, 0},
+ {0x4805, 0x00, 0, 0},
+ {0x301a, 0xf0, 0, 0},
+ {0x3208, 0x00, 0, 0},
+ {0x302a, 0x00, 0, 0},
+ {0x302a, 0x00, 0, 0},
+ {0x302a, 0x00, 0, 0},
+ {0x302a, 0x00, 0, 0},
+ {0x302a, 0x00, 0, 0},
+ {0x3601, 0x00, 0, 0},
+ {0x3638, 0x00, 0, 0},
+ {0x3208, 0x10, 0, 0},
+ {0x3208, 0xa0, 0, 0},
+};
+
+/* power-on sensor init reg table */
+static const struct ov4689_mode_info ov4689_mode_init_data = {
+ OV4689_MODE_1080P_1920_1080, SCALING,
+ 1920, 0x6e0, 1080, 0x470,
+ ov4689_init_setting_30fps_1080P,
+ ARRAY_SIZE(ov4689_init_setting_30fps_1080P),
+ OV4689_60_FPS,
+};
+
+static const struct ov4689_mode_info
+ov4689_mode_data[OV4689_NUM_MODES] = {
+ {OV4689_MODE_720P_1280_720, SUBSAMPLING,
+ 1280, 0x408, 720, 0x305,
+ ov4689_setting_720P_1280_720,
+ ARRAY_SIZE(ov4689_setting_720P_1280_720),
+ OV4689_150_FPS},
+ {OV4689_MODE_1080P_1920_1080, SCALING,
+ 1920, 0x6e0, 1080, 0x470,
+ ov4689_setting_1080P_1920_1080,
+ ARRAY_SIZE(ov4689_setting_1080P_1920_1080),
+ OV4689_60_FPS},
+ {OV4689_MODE_4M_2688_1520, SCALING,
+ 2688, 0xa0a, 1520, 0x612,
+ ov4689_setting_4M_2688_1520,
+ ARRAY_SIZE(ov4689_setting_4M_2688_1520),
+ OV4689_60_FPS},
+};
+
+static int ov4689_write_reg(struct ov4689_dev *sensor, u16 reg, u8 val)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg;
+ u8 buf[3];
+ int ret;
+
+ buf[0] = reg >> 8;
+ buf[1] = reg & 0xff;
+ buf[2] = val;
+
+ msg.addr = client->addr;
+ msg.flags = client->flags;
+ msg.buf = buf;
+ msg.len = sizeof(buf);
+
+ ret = i2c_transfer(client->adapter, &msg, 1);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+ __func__, reg, val);
+ return ret;
+ }
+
+ return 0;
+}
+
+static int ov4689_read_reg(struct ov4689_dev *sensor, u16 reg, u8 *val)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg[2];
+ u8 buf[2];
+ int ret;
+
+ buf[0] = reg >> 8;
+ buf[1] = reg & 0xff;
+
+ msg[0].addr = client->addr;
+ msg[0].flags = client->flags;
+ msg[0].buf = buf;
+ msg[0].len = sizeof(buf);
+
+ msg[1].addr = client->addr;
+ msg[1].flags = client->flags | I2C_M_RD;
+ msg[1].buf = buf;
+ msg[1].len = 1;
+
+ ret = i2c_transfer(client->adapter, msg, 2);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: error: reg=%x\n",
+ __func__, reg);
+ return ret;
+ }
+
+ *val = buf[0];
+ return 0;
+}
+
+static int ov4689_read_reg16(struct ov4689_dev *sensor, u16 reg, u16 *val)
+{
+ u8 hi, lo;
+ int ret;
+
+ ret = ov4689_read_reg(sensor, reg, &hi);
+ if (ret)
+ return ret;
+ ret = ov4689_read_reg(sensor, reg + 1, &lo);
+ if (ret)
+ return ret;
+
+ *val = ((u16)hi << 8) | (u16)lo;
+ return 0;
+}
+
+static int ov4689_write_reg16(struct ov4689_dev *sensor, u16 reg, u16 val)
+{
+ int ret;
+
+ ret = ov4689_write_reg(sensor, reg, val >> 8);
+ if (ret)
+ return ret;
+
+ return ov4689_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov4689_mod_reg(struct ov4689_dev *sensor, u16 reg,
+ u8 mask, u8 val)
+{
+ u8 readval;
+ int ret;
+
+ ret = ov4689_read_reg(sensor, reg, &readval);
+ if (ret)
+ return ret;
+
+ readval &= ~mask;
+ val &= mask;
+ val |= readval;
+
+ return ov4689_write_reg(sensor, reg, val);
+}
+
+static int ov4689_set_timings(struct ov4689_dev *sensor,
+ const struct ov4689_mode_info *mode)
+{
+ int ret;
+
+ return 0;
+}
+
+static int ov4689_load_regs(struct ov4689_dev *sensor,
+ const struct ov4689_mode_info *mode)
+{
+ const struct reg_value *regs = mode->reg_data;
+ unsigned int i;
+ u32 delay_ms;
+ u16 reg_addr;
+ u8 mask, val;
+ int ret = 0;
+
+ st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
+ for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+ delay_ms = regs->delay_ms;
+ reg_addr = regs->reg_addr;
+ val = regs->val;
+ mask = regs->mask;
+
+ if (mask)
+ ret = ov4689_mod_reg(sensor, reg_addr, mask, val);
+ else
+ ret = ov4689_write_reg(sensor, reg_addr, val);
+ if (ret)
+ break;
+
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+
+ return ov4689_set_timings(sensor, mode);
+}
+
+static int ov4689_get_exposure(struct ov4689_dev *sensor)
+{
+ int exp, ret;
+ u8 temp;
+
+ ret = ov4689_read_reg(sensor, OV4689_REG_EXPOSURE_HI, &temp);
+ if (ret)
+ return ret;
+ exp = ((int)temp & 0x0f) << 16;
+ ret = ov4689_read_reg(sensor, OV4689_REG_EXPOSURE_MED, &temp);
+ if (ret)
+ return ret;
+ exp |= ((int)temp << 8);
+ ret = ov4689_read_reg(sensor, OV4689_REG_EXPOSURE_LO, &temp);
+ if (ret)
+ return ret;
+ exp |= (int)temp;
+
+ return exp >> 4;
+}
+
+static int ov4689_set_exposure(struct ov4689_dev *sensor, u32 exposure)
+{
+ int ret;
+
+ st_info(ST_SENSOR, "%s, exposure = 0x%x\n", __func__, exposure);
+ exposure <<= 4;
+
+ ret = ov4689_write_reg(sensor,
+ OV4689_REG_EXPOSURE_LO,
+ exposure & 0xff);
+ if (ret)
+ return ret;
+ ret = ov4689_write_reg(sensor,
+ OV4689_REG_EXPOSURE_MED,
+ (exposure >> 8) & 0xff);
+ if (ret)
+ return ret;
+ return ov4689_write_reg(sensor,
+ OV4689_REG_EXPOSURE_HI,
+ (exposure >> 16) & 0x0f);
+}
+
+static int ov4689_get_gain(struct ov4689_dev *sensor)
+{
+ u32 gain = 0;
+ u8 val;
+
+ ov4689_read_reg(sensor, OV4689_REG_GAIN_H, &val);
+ gain = (val & 0x3) << 16;
+ ov4689_read_reg(sensor, OV4689_REG_GAIN_M, &val);
+ gain |= val << 8;
+ ov4689_read_reg(sensor, OV4689_REG_GAIN_L, &val);
+ gain |= val;
+
+ return gain;
+}
+
+static int ov4689_set_gain(struct ov4689_dev *sensor, int gain)
+{
+ u8 val;
+
+ ov4689_write_reg(sensor, OV4689_REG_GAIN_H,
+ (gain >> 16) & 0x3);
+ ov4689_write_reg(sensor, OV4689_REG_GAIN_M,
+ (gain >> 8) & 0xff);
+ ov4689_write_reg(sensor, OV4689_REG_GAIN_L,
+ gain & 0xff);
+ return 0;
+}
+
+static int ov4689_set_stream_mipi(struct ov4689_dev *sensor, bool on)
+{
+ return 0;
+}
+
+static int ov4689_get_sysclk(struct ov4689_dev *sensor)
+{
+ return 0;
+}
+
+static int ov4689_set_night_mode(struct ov4689_dev *sensor)
+{
+ return 0;
+}
+
+static int ov4689_get_hts(struct ov4689_dev *sensor)
+{
+ /* read HTS from register settings */
+ u16 hts;
+ int ret;
+
+ ret = ov4689_read_reg16(sensor, OV4689_REG_TIMING_HTS, &hts);
+ if (ret)
+ return ret;
+ return hts;
+}
+
+static int ov4689_get_vts(struct ov4689_dev *sensor)
+{
+ u16 vts;
+ int ret;
+
+ ret = ov4689_read_reg16(sensor, OV4689_REG_TIMING_VTS, &vts);
+ if (ret)
+ return ret;
+ return vts;
+}
+
+static int ov4689_set_vts(struct ov4689_dev *sensor, int vts)
+{
+ return ov4689_write_reg16(sensor, OV4689_REG_TIMING_VTS, vts);
+}
+
+static int ov4689_get_light_freq(struct ov4689_dev *sensor)
+{
+ return 0;
+}
+
+static int ov4689_set_bandingfilter(struct ov4689_dev *sensor)
+{
+ return 0;
+}
+
+static int ov4689_set_ae_target(struct ov4689_dev *sensor, int target)
+{
+ return 0;
+}
+
+static int ov4689_get_binning(struct ov4689_dev *sensor)
+{
+ return 0;
+}
+
+static int ov4689_set_binning(struct ov4689_dev *sensor, bool enable)
+{
+ return 0;
+}
+
+static const struct ov4689_mode_info *
+ov4689_find_mode(struct ov4689_dev *sensor, enum ov4689_frame_rate fr,
+ int width, int height, bool nearest)
+{
+ const struct ov4689_mode_info *mode;
+
+ mode = v4l2_find_nearest_size(ov4689_mode_data,
+ ARRAY_SIZE(ov4689_mode_data),
+ hact, vact,
+ width, height);
+
+ if (!mode ||
+ (!nearest && (mode->hact != width || mode->vact != height)))
+ return NULL;
+
+ /* Check to see if the current mode exceeds the max frame rate */
+ if (ov4689_framerates[fr] > ov4689_framerates[mode->max_fps])
+ return NULL;
+
+ return mode;
+}
+
+static u64 ov4689_calc_pixel_rate(struct ov4689_dev *sensor)
+{
+ u64 rate;
+
+ rate = sensor->current_mode->vact * sensor->current_mode->hact;
+ rate *= ov4689_framerates[sensor->current_fr];
+
+ return rate;
+}
+
+/*
+ * After trying the various combinations, reading various
+ * documentations spread around the net, and from the various
+ * feedback, the clock tree is probably as follows:
+ *
+ * +--------------+
+ * | Ext. Clock |
+ * +-+------------+
+ * | +----------+
+ * +->| PLL1 | - reg 0x030a, bit0 for the pre-dividerp
+ * +-+--------+ - reg 0x0300, bits 0-2 for the pre-divider
+ * +-+--------+ - reg 0x0301~0x0302, for the multiplier
+ * | +--------------+
+ * +->| MIPI Divider | - reg 0x0303, bits 0-3 for the pre-divider
+ * | +---------> MIPI PHY CLK
+ * | +-----+
+ * | +->| PLL1_DIV_MIPI | - reg 0x0304, bits 0-1 for the divider
+ * | +----------------> PCLK
+ * | +-----+
+ *
+ * +--------------+
+ * | Ext. Clock |
+ * +-+------------+
+ * | +----------+
+ * +->| PLL2 | - reg 0x0311, bit0 for the pre-dividerp
+ * +-+--------+ - reg 0x030b, bits 0-2 for the pre-divider
+ * +-+--------+ - reg 0x030c~0x030d, for the multiplier
+ * | +--------------+
+ * +->| SCLK Divider | - reg 0x030F, bits 0-3 for the pre-divider
+ * +-+--------+ - reg 0x030E, bits 0-2 for the divider
+ * | +---------> SCLK
+ *
+ * | +-----+
+ * +->| DAC Divider | - reg 0x0312, bits 0-3 for the divider
+ * | +----------------> DACCLK
+ **
+ */
+
+/*
+ * ov4689_set_mipi_pclk() - Calculate the clock tree configuration values
+ * for the MIPI CSI-2 output.
+ *
+ * @rate: The requested bandwidth per lane in bytes per second.
+ * 'Bandwidth Per Lane' is calculated as:
+ * bpl = HTOT * VTOT * FPS * bpp / num_lanes;
+ *
+ * This function use the requested bandwidth to calculate:
+ *
+ * - mipi_pclk = bpl / 2; ( / 2 is for CSI-2 DDR)
+ * - mipi_phy_clk = mipi_pclk * PLL1_DIV_MIPI;
+ *
+ * with these fixed parameters:
+ * PLL1_PREDIVP = 1;
+ * PLL1_PREDIV = 1; (MIPI_BIT_MODE == 8 ? 2 : 2,5);
+ * PLL1_DIVM = 1;
+ * PLL1_DIV_MIPI = 4;
+ *
+ * FIXME: this have been tested with 10-bit raw and 2 lanes setup only.
+ * MIPI_DIV is fixed to value 2, but it -might- be changed according to the
+ * above formula for setups with 1 lane or image formats with different bpp.
+ *
+ * FIXME: this deviates from the sensor manual documentation which is quite
+ * thin on the MIPI clock tree generation part.
+ */
+
+#define PLL1_PREDIVP 1 // bypass
+#define PLL1_PREDIV 1 // bypass
+#define PLL1_DIVM 1 // bypass
+#define PLL1_DIV_MIPI 3 // div
+#define PLL1_DIV_MIPI_BASE 1 // div
+
+#define PLL1_DIVSP 1 // no use
+#define PLL1_DIVS 1 // no use
+
+#define PLL2_PREDIVP 0
+#define PLL2_PREDIV 0
+#define PLL2_DIVSP 1
+#define PLL2_DIVS 4
+#define PLL2_DIVDAC 1
+
+#define OV4689_PLL1_PREDIVP 0x030a // bits[0]
+#define OV4689_PLL1_PREDIV 0x0300 // bits[2:0]
+#define OV4689_PLL1_MULTIPLIER 0x0301 // bits[9:8] 0x0302 bits[7:0]
+#define OV4689_PLL1_DIVM 0x0303 // bits[3:0]
+#define OV4689_PLL1_DIV_MIPI 0x0304 // bits[1:0]
+
+#define OV4689_PLL1_DIVSP 0x0305 //bits[1:0]
+#define OV4689_PLL1_DIVS 0x0306 // bits[0]
+
+#define OV4689_PLL2_PREDIVP 0x0311 // bits[0]
+#define OV4689_PLL2_PREDIV 0x030b // bits[2:0]
+#define OV4689_PLL2_MULTIPLIER 0x030c // bits[9:8] 0x030d bits[7:0]
+#define OV4689_PLL2_DIVSP 0x030f // bits[3:0]
+#define OV4689_PLL2_DIVS 0x030e // bits[2:0]
+#define OV4689_PLL2_DIVDAC 0x0312 // bits[3:0]
+
+#define OV4689_TIMING_HTS 0x380c
+
+static int ov4689_set_mipi_pclk(struct ov4689_dev *sensor,
+ unsigned long rate)
+{
+ const struct ov4689_mode_info *mode = sensor->current_mode;
+ const struct ov4689_mode_info *orig_mode = sensor->last_mode;
+ u8 mult, val;
+ int ret = 0;
+ int fps = ov4689_framerates[sensor->current_fr];
+ u16 htot, val16;
+
+ htot = mode->htot * ov4689_framerates[mode->max_fps] / fps;
+
+ ret = ov4689_write_reg16(sensor, OV4689_TIMING_HTS, htot);
+
+ ret = ov4689_read_reg(sensor, OV4689_TIMING_HTS, &val);
+ val16 = val << 8;
+ ret = ov4689_read_reg(sensor, OV4689_TIMING_HTS + 1, &val);
+ val16 |= val;
+ st_info(ST_SENSOR, "fps = %d, max_fps = %d, mode->htot = 0x%x, "
+ "htot = 0x%x, 0x%x = 0x%x\n",
+ fps, mode->max_fps, mode->htot,
+ htot, OV4689_TIMING_HTS, val16);
+ return 0;
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov4689_set_mode_direct(struct ov4689_dev *sensor,
+ const struct ov4689_mode_info *mode)
+{
+ if (!mode->reg_data)
+ return -EINVAL;
+
+ /* Write capture setting */
+ return ov4689_load_regs(sensor, mode);
+}
+
+static int ov4689_set_mode(struct ov4689_dev *sensor)
+{
+ const struct ov4689_mode_info *mode = sensor->current_mode;
+ const struct ov4689_mode_info *orig_mode = sensor->last_mode;
+ int ret = 0;
+
+ ret = ov4689_set_mode_direct(sensor, mode);
+ if (ret < 0)
+ return ret;
+
+ /*
+ * we support have 10 bits raw RGB(mipi)
+ */
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov4689_set_mipi_pclk(sensor, 0);
+
+ if (ret < 0)
+ return 0;
+
+ sensor->pending_mode_change = false;
+ sensor->last_mode = mode;
+ return 0;
+}
+
+/* restore the last set video mode after chip power-on */
+static int ov4689_restore_mode(struct ov4689_dev *sensor)
+{
+ int ret;
+
+ /* first load the initial register values */
+ ret = ov4689_load_regs(sensor, &ov4689_mode_init_data);
+ if (ret < 0)
+ return ret;
+ sensor->last_mode = &ov4689_mode_init_data;
+
+ /* now restore the last capture mode */
+ ret = ov4689_set_mode(sensor);
+ if (ret < 0)
+ return ret;
+
+ return ret;
+}
+
+static void ov4689_power(struct ov4689_dev *sensor, bool enable)
+{
+ if (!sensor->pwdn_gpio)
+ return;
+ gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void ov4689_reset(struct ov4689_dev *sensor)
+{
+ if (!sensor->reset_gpio)
+ return;
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+
+ usleep_range(5000, 25000);
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+ usleep_range(1000, 2000);
+}
+
+static int ov4689_set_power_on(struct ov4689_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret;
+
+ ret = clk_prepare_enable(sensor->xclk);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable clock\n",
+ __func__);
+ return ret;
+ }
+
+ ret = regulator_bulk_enable(OV4689_NUM_SUPPLIES,
+ sensor->supplies);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable regulators\n",
+ __func__);
+ goto xclk_off;
+ }
+
+ ov4689_reset(sensor);
+ ov4689_power(sensor, true);
+
+ return 0;
+
+xclk_off:
+ clk_disable_unprepare(sensor->xclk);
+ return ret;
+}
+
+static void ov4689_set_power_off(struct ov4689_dev *sensor)
+{
+ ov4689_power(sensor, false);
+ regulator_bulk_disable(OV4689_NUM_SUPPLIES, sensor->supplies);
+ clk_disable_unprepare(sensor->xclk);
+}
+
+static int ov4689_set_power_mipi(struct ov4689_dev *sensor, bool on)
+{
+ return 0;
+}
+
+static int ov4689_set_power(struct ov4689_dev *sensor, bool on)
+{
+ int ret = 0;
+ u16 chip_id;
+
+ if (on) {
+ ret = ov4689_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+ ret = ov4689_read_reg16(sensor, OV4689_REG_CHIP_ID, &chip_id);
+ if (ret) {
+ dev_err(&sensor->i2c_client->dev, "%s: failed to read chip identifier\n",
+ __func__);
+ ret = -ENODEV;
+ goto power_off;
+ }
+
+ if (chip_id != OV4689_CHIP_ID) {
+ dev_err(&sensor->i2c_client->dev,
+ "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+ __func__, OV4689_CHIP_ID, chip_id);
+ ret = -ENXIO;
+ goto power_off;
+ }
+ dev_err(&sensor->i2c_client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+
+ ret = ov4689_restore_mode(sensor);
+ if (ret)
+ goto power_off;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov4689_set_power_mipi(sensor, on);
+ if (ret)
+ goto power_off;
+
+ if (!on)
+ ov4689_set_power_off(sensor);
+
+ return 0;
+
+power_off:
+ ov4689_set_power_off(sensor);
+ return ret;
+}
+
+static int ov4689_s_power(struct v4l2_subdev *sd, int on)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ /*
+ * If the power count is modified from 0 to != 0 or from != 0 to 0,
+ * update the power state.
+ */
+ if (sensor->power_count == !on) {
+ ret = ov4689_set_power(sensor, !!on);
+ if (ret)
+ goto out;
+ }
+
+ /* Update the power count. */
+ sensor->power_count += on ? 1 : -1;
+ WARN_ON(sensor->power_count < 0);
+out:
+ mutex_unlock(&sensor->lock);
+
+ if (on && !ret && sensor->power_count == 1) {
+ /* restore controls */
+ ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+ }
+
+ return ret;
+}
+
+static int ov4689_try_frame_interval(struct ov4689_dev *sensor,
+ struct v4l2_fract *fi,
+ u32 width, u32 height)
+{
+ const struct ov4689_mode_info *mode;
+ enum ov4689_frame_rate rate = OV4689_15_FPS;
+ int minfps, maxfps, best_fps, fps;
+ int i;
+
+ minfps = ov4689_framerates[OV4689_15_FPS];
+ maxfps = ov4689_framerates[OV4689_NUM_FRAMERATES - 1];
+
+ if (fi->numerator == 0) {
+ fi->denominator = maxfps;
+ fi->numerator = 1;
+ rate = OV4689_60_FPS;
+ goto find_mode;
+ }
+
+ fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+ minfps, maxfps);
+
+ best_fps = minfps;
+ for (i = 0; i < ARRAY_SIZE(ov4689_framerates); i++) {
+ int curr_fps = ov4689_framerates[i];
+ if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+ best_fps = curr_fps;
+ rate = i;
+ }
+ }
+ st_info(ST_SENSOR, "best_fps = %d, fps = %d\n", best_fps, fps);
+
+ fi->numerator = 1;
+ fi->denominator = best_fps;
+
+find_mode:
+ mode = ov4689_find_mode(sensor, rate, width, height, false);
+ return mode ? rate : -EINVAL;
+}
+
+static int ov4689_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ if (code->pad != 0)
+ return -EINVAL;
+
+ if (code->index)
+ return -EINVAL;
+
+ code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+ return 0;
+}
+
+static int ov4689_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *format)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ struct v4l2_mbus_framefmt *fmt;
+
+ if (format->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+ fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+ format->pad);
+ else
+ fmt = &sensor->fmt;
+
+ format->format = *fmt;
+
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int ov4689_try_fmt_internal(struct v4l2_subdev *sd,
+ struct v4l2_mbus_framefmt *fmt,
+ enum ov4689_frame_rate fr,
+ const struct ov4689_mode_info **new_mode)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ const struct ov4689_mode_info *mode;
+ int i;
+
+ mode = ov4689_find_mode(sensor, fr, fmt->width, fmt->height, true);
+ if (!mode)
+ return -EINVAL;
+ fmt->width = mode->hact;
+ fmt->height = mode->vact;
+
+ if (new_mode)
+ *new_mode = mode;
+
+ fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+ return 0;
+}
+
+static int ov4689_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *format)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ const struct ov4689_mode_info *new_mode;
+ struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+ struct v4l2_mbus_framefmt *fmt;
+ int ret;
+
+ if (format->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ ret = ov4689_try_fmt_internal(sd, mbus_fmt, 0, &new_mode);
+ if (ret)
+ goto out;
+
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+ fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+ else
+ fmt = &sensor->fmt;
+
+ *fmt = *mbus_fmt;
+
+ if (new_mode != sensor->current_mode) {
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+ if (new_mode->max_fps < sensor->current_fr) {
+ sensor->current_fr = new_mode->max_fps;
+ sensor->frame_interval.numerator = 1;
+ sensor->frame_interval.denominator =
+ ov4689_framerates[sensor->current_fr];
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ ov4689_calc_pixel_rate(sensor));
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov4689_set_ctrl_hue(struct ov4689_dev *sensor, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov4689_set_ctrl_contrast(struct ov4689_dev *sensor, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov4689_set_ctrl_saturation(struct ov4689_dev *sensor, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov4689_set_ctrl_white_balance(struct ov4689_dev *sensor, int awb)
+{
+ struct ov4689_ctrls *ctrls = &sensor->ctrls;
+ int ret = 0;
+
+ if (!awb && (ctrls->red_balance->is_new
+ || ctrls->blue_balance->is_new)) {
+ u16 red = (u16)ctrls->red_balance->val;
+ u16 blue = (u16)ctrls->blue_balance->val;
+
+ st_info(ST_SENSOR, "red = 0x%x, blue = 0x%x\n", red, blue);
+ ret = ov4689_write_reg16(sensor, OV4689_REG_AWB_R_GAIN, red);
+ if (ret)
+ return ret;
+ ret = ov4689_write_reg16(sensor, OV4689_REG_AWB_B_GAIN, blue);
+ }
+ return ret;
+}
+
+static int ov4689_set_ctrl_exposure(struct ov4689_dev *sensor,
+ enum v4l2_exposure_auto_type auto_exposure)
+{
+ struct ov4689_ctrls *ctrls = &sensor->ctrls;
+ bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+ int ret = 0;
+
+ if (!auto_exp && ctrls->exposure->is_new) {
+ u16 max_exp = 0;
+
+ ret = ov4689_read_reg16(sensor, OV4689_REG_V_OUTPUT_SIZE,
+ &max_exp);
+
+ ret = ov4689_get_vts(sensor);
+ if (ret < 0)
+ return ret;
+ max_exp += ret;
+ ret = 0;
+
+ st_info(ST_SENSOR, "%s, max_exp = 0x%x\n", __func__, max_exp);
+ if (ctrls->exposure->val < max_exp)
+ ret = ov4689_set_exposure(sensor, ctrls->exposure->val);
+ }
+
+ return ret;
+}
+
+static const s64 link_freq_menu_items[] = {
+ OV4689_LINK_FREQ_500MHZ
+};
+
+static const char * const test_pattern_menu[] = {
+ "Disabled",
+ "Color bars",
+ "Color bars w/ rolling bar",
+ "Color squares",
+ "Color squares w/ rolling bar",
+};
+
+#define OV4689_TEST_ENABLE BIT(7)
+#define OV4689_TEST_ROLLING BIT(6) /* rolling horizontal bar */
+#define OV4689_TEST_TRANSPARENT BIT(5)
+#define OV4689_TEST_SQUARE_BW BIT(4) /* black & white squares */
+#define OV4689_TEST_BAR_STANDARD (0 << 2)
+#define OV4689_TEST_BAR_DARKER_1 (1 << 2)
+#define OV4689_TEST_BAR_DARKER_2 (2 << 2)
+#define OV4689_TEST_BAR_DARKER_3 (3 << 2)
+#define OV4689_TEST_BAR (0 << 0)
+#define OV4689_TEST_RANDOM (1 << 0)
+#define OV4689_TEST_SQUARE (2 << 0)
+#define OV4689_TEST_BLACK (3 << 0)
+
+static const u8 test_pattern_val[] = {
+ 0,
+ OV4689_TEST_ENABLE | OV4689_TEST_BAR_STANDARD |
+ OV4689_TEST_BAR,
+ OV4689_TEST_ENABLE | OV4689_TEST_ROLLING |
+ OV4689_TEST_BAR_DARKER_1 | OV4689_TEST_BAR,
+ OV4689_TEST_ENABLE | OV4689_TEST_SQUARE,
+ OV4689_TEST_ENABLE | OV4689_TEST_ROLLING | OV4689_TEST_SQUARE,
+};
+
+static int ov4689_set_ctrl_test_pattern(struct ov4689_dev *sensor, int value)
+{
+ return ov4689_write_reg(sensor, OV4689_REG_TEST_PATTERN,
+ test_pattern_val[value]);
+}
+
+static int ov4689_set_ctrl_light_freq(struct ov4689_dev *sensor, int value)
+{
+ return 0;
+}
+
+static int ov4689_set_ctrl_hflip(struct ov4689_dev *sensor, int value)
+{
+ /*
+ * TIMING TC REG21:
+ * - [2]: Digital mirror
+ * - [1]: Array mirror
+ */
+ return ov4689_mod_reg(sensor, OV4689_REG_TIMING_TC_REG21,
+ BIT(2) | BIT(1),
+ (!(value ^ sensor->upside_down)) ?
+ (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov4689_set_ctrl_vflip(struct ov4689_dev *sensor, int value)
+{
+ /*
+ * TIMING TC REG20:
+ * - [2]: Digital vflip
+ * - [1]: Array vflip
+ */
+ return ov4689_mod_reg(sensor, OV4689_REG_TIMING_TC_REG20,
+ BIT(2) | BIT(1),
+ (value ^ sensor->upside_down) ?
+ (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov4689_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ int val;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ switch (ctrl->id) {
+ case V4L2_CID_ANALOGUE_GAIN:
+ val = ov4689_get_gain(sensor);
+ break;
+ }
+
+ return 0;
+}
+
+static int ov4689_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ int ret;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ /*
+ * If the device is not powered up by the host driver do
+ * not apply any controls to H/W at this time. Instead
+ * the controls will be restored right after power-up.
+ */
+ if (sensor->power_count == 0)
+ return 0;
+
+ switch (ctrl->id) {
+ case V4L2_CID_ANALOGUE_GAIN:
+ ret = ov4689_set_gain(sensor, ctrl->val);
+ break;
+ case V4L2_CID_EXPOSURE:
+ ret = ov4689_set_ctrl_exposure(sensor, V4L2_EXPOSURE_MANUAL);
+ break;
+ case V4L2_CID_AUTO_WHITE_BALANCE:
+ ret = ov4689_set_ctrl_white_balance(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ ret = ov4689_set_ctrl_hue(sensor, ctrl->val);
+ break;
+ case V4L2_CID_CONTRAST:
+ ret = ov4689_set_ctrl_contrast(sensor, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ ret = ov4689_set_ctrl_saturation(sensor, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN:
+ ret = ov4689_set_ctrl_test_pattern(sensor, ctrl->val);
+ break;
+ case V4L2_CID_POWER_LINE_FREQUENCY:
+ ret = ov4689_set_ctrl_light_freq(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ ret = ov4689_set_ctrl_hflip(sensor, ctrl->val);
+ break;
+ case V4L2_CID_VFLIP:
+ ret = ov4689_set_ctrl_vflip(sensor, ctrl->val);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+ return ret;
+}
+
+static const struct v4l2_ctrl_ops ov4689_ctrl_ops = {
+ .g_volatile_ctrl = ov4689_g_volatile_ctrl,
+ .s_ctrl = ov4689_s_ctrl,
+};
+
+static int ov4689_init_controls(struct ov4689_dev *sensor)
+{
+ const struct v4l2_ctrl_ops *ops = &ov4689_ctrl_ops;
+ struct ov4689_ctrls *ctrls = &sensor->ctrls;
+ struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+ int ret;
+
+ v4l2_ctrl_handler_init(hdl, 32);
+
+ /* we can use our own mutex for the ctrl lock */
+ hdl->lock = &sensor->lock;
+
+ /* Clock related controls */
+ ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+ 0, INT_MAX, 1,
+ ov4689_calc_pixel_rate(sensor));
+
+ /* Auto/manual white balance */
+ ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+ V4L2_CID_AUTO_WHITE_BALANCE,
+ 0, 1, 1, 0);
+ ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+ 0, 4095, 1, 1024);
+ ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+ 0, 4095, 1, 1024);
+
+ ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+ 4, 0xfff8, 1, 0x4c00);
+ ctrls->anal_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+ 0x10, 0xfff8, 1, 0x0080);
+ ctrls->test_pattern =
+ v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+ ARRAY_SIZE(test_pattern_menu) - 1,
+ 0, 0, test_pattern_menu);
+ ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+ 0, 1, 1, 0);
+ ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+ 0, 1, 1, 0);
+ ctrls->light_freq =
+ v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_POWER_LINE_FREQUENCY,
+ V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+ V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+ ctrls->link_freq = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+ 0, 0, link_freq_menu_items);
+ if (hdl->error) {
+ ret = hdl->error;
+ goto free_ctrls;
+ }
+
+ ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+ ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+ // ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+ // ctrls->anal_gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+ v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+
+ sensor->sd.ctrl_handler = hdl;
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(hdl);
+ return ret;
+}
+
+static int ov4689_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ if (fse->pad != 0)
+ return -EINVAL;
+ if (fse->index >= OV4689_NUM_MODES)
+ return -EINVAL;
+
+ fse->min_width =
+ ov4689_mode_data[fse->index].hact;
+ fse->max_width = fse->min_width;
+ fse->min_height =
+ ov4689_mode_data[fse->index].vact;
+ fse->max_height = fse->min_height;
+
+ return 0;
+}
+
+static int ov4689_enum_frame_interval(
+ struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_interval_enum *fie)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ struct v4l2_fract tpf;
+ int ret;
+
+ if (fie->pad != 0)
+ return -EINVAL;
+ if (fie->index >= OV4689_NUM_FRAMERATES)
+ return -EINVAL;
+
+ tpf.numerator = 1;
+ tpf.denominator = ov4689_framerates[fie->index];
+
+/* ret = ov4689_try_frame_interval(sensor, &tpf,
+ fie->width, fie->height);
+ if (ret < 0)
+ return -EINVAL;
+*/
+ fie->interval = tpf;
+
+ return 0;
+}
+
+static int ov4689_g_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+
+ mutex_lock(&sensor->lock);
+ fi->interval = sensor->frame_interval;
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int ov4689_s_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ const struct ov4689_mode_info *mode;
+ int frame_rate, ret = 0;
+
+ if (fi->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ mode = sensor->current_mode;
+
+ frame_rate = ov4689_try_frame_interval(sensor, &fi->interval,
+ mode->hact, mode->vact);
+ if (frame_rate < 0) {
+ /* Always return a valid frame interval value */
+ fi->interval = sensor->frame_interval;
+ goto out;
+ }
+
+ mode = ov4689_find_mode(sensor, frame_rate, mode->hact,
+ mode->vact, true);
+ if (!mode) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ if (mode != sensor->current_mode ||
+ frame_rate != sensor->current_fr) {
+ sensor->current_fr = frame_rate;
+ sensor->frame_interval = fi->interval;
+ sensor->current_mode = mode;
+ sensor->pending_mode_change = true;
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ ov4689_calc_pixel_rate(sensor));
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int ov4689_stream_start(struct ov4689_dev *sensor, int enable)
+{
+ u8 val;
+ int result = ov4689_write_reg(sensor, 0x100, enable);
+
+ ov4689_read_reg(sensor, 0x100, &val);
+ mdelay(200);
+ return result;
+}
+
+static int ov4689_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming == !enable) {
+ if (enable && sensor->pending_mode_change) {
+ ret = ov4689_set_mode(sensor);
+ if (ret)
+ goto out;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov4689_set_stream_mipi(sensor, enable);
+
+ ret = ov4689_stream_start(sensor, enable);
+
+ if (!ret)
+ sensor->streaming = enable;
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static const struct v4l2_subdev_core_ops ov4689_core_ops = {
+ .s_power = ov4689_s_power,
+ .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 ov4689_video_ops = {
+ .g_frame_interval = ov4689_g_frame_interval,
+ .s_frame_interval = ov4689_s_frame_interval,
+ .s_stream = ov4689_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov4689_pad_ops = {
+ .enum_mbus_code = ov4689_enum_mbus_code,
+ .get_fmt = ov4689_get_fmt,
+ .set_fmt = ov4689_set_fmt,
+ .enum_frame_size = ov4689_enum_frame_size,
+ .enum_frame_interval = ov4689_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops ov4689_subdev_ops = {
+ .core = &ov4689_core_ops,
+ .video = &ov4689_video_ops,
+ .pad = &ov4689_pad_ops,
+};
+
+static int ov4689_get_regulators(struct ov4689_dev *sensor)
+{
+ int i;
+
+ for (i = 0; i < OV4689_NUM_SUPPLIES; i++)
+ sensor->supplies[i].supply = ov4689_supply_name[i];
+
+ return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+ OV4689_NUM_SUPPLIES,
+ sensor->supplies);
+}
+
+static int ov4689_check_chip_id(struct ov4689_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret = 0;
+ u16 chip_id;
+
+ ret = ov4689_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+ ret = ov4689_read_reg16(sensor, OV4689_REG_CHIP_ID, &chip_id);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to read chip identifier\n",
+ __func__);
+ goto power_off;
+ }
+
+ if (chip_id != OV4689_CHIP_ID) {
+ dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+ __func__, OV4689_CHIP_ID, chip_id);
+ ret = -ENXIO;
+ }
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+
+power_off:
+ ov4689_set_power_off(sensor);
+ return ret;
+}
+
+static int ov4689_probe(struct i2c_client *client)
+{
+ struct device *dev = &client->dev;
+ struct fwnode_handle *endpoint;
+ struct ov4689_dev *sensor;
+ struct v4l2_mbus_framefmt *fmt;
+ u32 rotation;
+ int ret;
+ u8 chip_id_high, chip_id_low;
+
+ sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+ if (!sensor)
+ return -ENOMEM;
+
+ sensor->i2c_client = client;
+
+ fmt = &sensor->fmt;
+ fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+ 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 = 1920;
+ fmt->height = 1080;
+ fmt->field = V4L2_FIELD_NONE;
+ sensor->frame_interval.numerator = 1;
+ sensor->frame_interval.denominator = ov4689_framerates[OV4689_30_FPS];
+ sensor->current_fr = OV4689_30_FPS;
+ sensor->current_mode =
+ &ov4689_mode_data[OV4689_MODE_1080P_1920_1080];
+ sensor->last_mode = sensor->current_mode;
+
+ sensor->ae_target = 52;
+
+ /* optional indication of physical rotation of sensor */
+ ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
+ &rotation);
+ if (!ret) {
+ switch (rotation) {
+ case 180:
+ sensor->upside_down = true;
+ fallthrough;
+ case 0:
+ break;
+ default:
+ dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
+ rotation);
+ }
+ }
+
+ endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+ NULL);
+ if (!endpoint) {
+ dev_err(dev, "endpoint node not found\n");
+ return -EINVAL;
+ }
+
+ ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+ fwnode_handle_put(endpoint);
+ if (ret) {
+ dev_err(dev, "Could not parse endpoint\n");
+ return ret;
+ }
+
+ if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+ sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+ sensor->ep.bus_type != V4L2_MBUS_BT656) {
+ dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+ return -EINVAL;
+ }
+
+ /* get system clock (xclk) */
+ sensor->xclk = devm_clk_get(dev, "xclk");
+ if (IS_ERR(sensor->xclk)) {
+ dev_err(dev, "failed to get xclk\n");
+ return PTR_ERR(sensor->xclk);
+ }
+
+ sensor->xclk_freq = clk_get_rate(sensor->xclk);
+ if (sensor->xclk_freq < OV4689_XCLK_MIN ||
+ sensor->xclk_freq > OV4689_XCLK_MAX) {
+ dev_err(dev, "xclk frequency out of range: %d Hz\n",
+ sensor->xclk_freq);
+ return -EINVAL;
+ }
+
+ /* request optional power down pin */
+ sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->pwdn_gpio))
+ return PTR_ERR(sensor->pwdn_gpio);
+
+ /* request optional reset pin */
+ sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->reset_gpio))
+ return PTR_ERR(sensor->reset_gpio);
+
+ v4l2_i2c_subdev_init(&sensor->sd, client, &ov4689_subdev_ops);
+
+ sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+ V4L2_SUBDEV_FL_HAS_EVENTS;
+ sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+ sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+ ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+ if (ret)
+ return ret;
+
+ ret = ov4689_get_regulators(sensor);
+ if (ret)
+ return ret;
+
+ mutex_init(&sensor->lock);
+
+ ret = ov4689_check_chip_id(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = ov4689_init_controls(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = v4l2_async_register_subdev_sensor_common(&sensor->sd);
+ if (ret)
+ goto free_ctrls;
+
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+ media_entity_cleanup(&sensor->sd.entity);
+ mutex_destroy(&sensor->lock);
+ return ret;
+}
+
+static int ov4689_remove(struct i2c_client *client)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+
+ v4l2_async_unregister_subdev(&sensor->sd);
+ media_entity_cleanup(&sensor->sd.entity);
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+ mutex_destroy(&sensor->lock);
+
+ return 0;
+}
+
+static const struct i2c_device_id ov4689_id[] = {
+ {"ov4689", 0},
+ {},
+};
+MODULE_DEVICE_TABLE(i2c, ov4689_id);
+
+static const struct of_device_id ov4689_dt_ids[] = {
+ { .compatible = "ovti,ov4689" },
+ { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov4689_dt_ids);
+
+static struct i2c_driver ov4689_i2c_driver = {
+ .driver = {
+ .name = "ov4689",
+ .of_match_table = ov4689_dt_ids,
+ },
+ .id_table = ov4689_id,
+ .probe_new = ov4689_probe,
+ .remove = ov4689_remove,
+};
+
+module_i2c_driver(ov4689_i2c_driver);
+
+MODULE_DESCRIPTION("OV4689 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.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 "stfcamss.h"
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV5640_XCLK_MIN 6000000
+#define OV5640_XCLK_MAX 54000000
+
+#define OV5640_SKIP_FRAMES 4
+
+#define OV5640_CHIP_ID 0x5640
+#define OV5640_DEFAULT_SLAVE_ID 0x3c
+
+#define OV5640_REG_SYS_RESET02 0x3002
+#define OV5640_REG_SYS_CLOCK_ENABLE02 0x3006
+#define OV5640_REG_SYS_CTRL0 0x3008
+#define OV5640_REG_SYS_CTRL0_SW_PWDN 0x42
+#define OV5640_REG_SYS_CTRL0_SW_PWUP 0x02
+#define OV5640_REG_CHIP_ID 0x300a
+#define OV5640_REG_IO_MIPI_CTRL00 0x300e
+#define OV5640_REG_PAD_OUTPUT_ENABLE01 0x3017
+#define OV5640_REG_PAD_OUTPUT_ENABLE02 0x3018
+#define OV5640_REG_PAD_OUTPUT00 0x3019
+#define OV5640_REG_SYSTEM_CONTROL1 0x302e
+#define OV5640_REG_SC_PLL_CTRL0 0x3034
+#define OV5640_REG_SC_PLL_CTRL1 0x3035
+#define OV5640_REG_SC_PLL_CTRL2 0x3036
+#define OV5640_REG_SC_PLL_CTRL3 0x3037
+#define OV5640_REG_SLAVE_ID 0x3100
+#define OV5640_REG_SCCB_SYS_CTRL1 0x3103
+#define OV5640_REG_SYS_ROOT_DIVIDER 0x3108
+#define OV5640_REG_AWB_R_GAIN 0x3400
+#define OV5640_REG_AWB_G_GAIN 0x3402
+#define OV5640_REG_AWB_B_GAIN 0x3404
+#define OV5640_REG_AWB_MANUAL_CTRL 0x3406
+#define OV5640_REG_AEC_PK_EXPOSURE_HI 0x3500
+#define OV5640_REG_AEC_PK_EXPOSURE_MED 0x3501
+#define OV5640_REG_AEC_PK_EXPOSURE_LO 0x3502
+#define OV5640_REG_AEC_PK_MANUAL 0x3503
+#define OV5640_REG_AEC_PK_REAL_GAIN 0x350a
+#define OV5640_REG_AEC_PK_VTS 0x350c
+#define OV5640_REG_TIMING_DVPHO 0x3808
+#define OV5640_REG_TIMING_DVPVO 0x380a
+#define OV5640_REG_TIMING_HTS 0x380c
+#define OV5640_REG_TIMING_VTS 0x380e
+#define OV5640_REG_TIMING_TC_REG20 0x3820
+#define OV5640_REG_TIMING_TC_REG21 0x3821
+#define OV5640_REG_AEC_CTRL00 0x3a00
+#define OV5640_REG_AEC_B50_STEP 0x3a08
+#define OV5640_REG_AEC_B60_STEP 0x3a0a
+#define OV5640_REG_AEC_CTRL0D 0x3a0d
+#define OV5640_REG_AEC_CTRL0E 0x3a0e
+#define OV5640_REG_AEC_CTRL0F 0x3a0f
+#define OV5640_REG_AEC_CTRL10 0x3a10
+#define OV5640_REG_AEC_CTRL11 0x3a11
+#define OV5640_REG_AEC_CTRL1B 0x3a1b
+#define OV5640_REG_AEC_CTRL1E 0x3a1e
+#define OV5640_REG_AEC_CTRL1F 0x3a1f
+#define OV5640_REG_HZ5060_CTRL00 0x3c00
+#define OV5640_REG_HZ5060_CTRL01 0x3c01
+#define OV5640_REG_SIGMADELTA_CTRL0C 0x3c0c
+#define OV5640_REG_FRAME_CTRL01 0x4202
+#define OV5640_REG_FORMAT_CONTROL00 0x4300
+#define OV5640_REG_VFIFO_HSIZE 0x4602
+#define OV5640_REG_VFIFO_VSIZE 0x4604
+#define OV5640_REG_JPG_MODE_SELECT 0x4713
+#define OV5640_REG_CCIR656_CTRL00 0x4730
+#define OV5640_REG_POLARITY_CTRL00 0x4740
+#define OV5640_REG_MIPI_CTRL00 0x4800
+#define OV5640_REG_DEBUG_MODE 0x4814
+#define OV5640_REG_ISP_FORMAT_MUX_CTRL 0x501f
+#define OV5640_REG_PRE_ISP_TEST_SET1 0x503d
+#define OV5640_REG_SDE_CTRL0 0x5580
+#define OV5640_REG_SDE_CTRL1 0x5581
+#define OV5640_REG_SDE_CTRL3 0x5583
+#define OV5640_REG_SDE_CTRL4 0x5584
+#define OV5640_REG_SDE_CTRL5 0x5585
+#define OV5640_REG_AVG_READOUT 0x56a1
+
+enum ov5640_mode_id {
+ OV5640_MODE_QCIF_176_144 = 0,
+ OV5640_MODE_QVGA_320_240,
+ OV5640_MODE_VGA_640_480,
+ OV5640_MODE_NTSC_720_480,
+ OV5640_MODE_PAL_720_576,
+ OV5640_MODE_XGA_1024_768,
+ OV5640_MODE_720P_1280_720,
+ OV5640_MODE_1080P_1920_1080,
+ OV5640_MODE_QSXGA_2592_1944,
+ OV5640_NUM_MODES,
+};
+
+enum ov5640_frame_rate {
+ OV5640_15_FPS = 0,
+ OV5640_30_FPS,
+ OV5640_60_FPS,
+ OV5640_NUM_FRAMERATES,
+};
+
+enum ov5640_format_mux {
+ OV5640_FMT_MUX_YUV422 = 0,
+ OV5640_FMT_MUX_RGB,
+ OV5640_FMT_MUX_DITHER,
+ OV5640_FMT_MUX_RAW_DPC,
+ OV5640_FMT_MUX_SNR_RAW,
+ OV5640_FMT_MUX_RAW_CIP,
+};
+
+struct ov5640_pixfmt {
+ u32 code;
+ u32 colorspace;
+};
+
+static const struct ov5640_pixfmt ov5640_formats[] = {
+ { MEDIA_BUS_FMT_JPEG_1X8, V4L2_COLORSPACE_JPEG, },
+ { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_COLORSPACE_SRGB, },
+};
+
+/*
+ * FIXME: remove this when a subdev API becomes available
+ * to set the MIPI CSI-2 virtual channel.
+ */
+static unsigned int virtual_channel;
+module_param(virtual_channel, uint, 0444);
+MODULE_PARM_DESC(virtual_channel,
+ "MIPI CSI-2 virtual channel (0..3), default 0");
+
+static const int ov5640_framerates[] = {
+ [OV5640_15_FPS] = 15,
+ [OV5640_30_FPS] = 30,
+ [OV5640_60_FPS] = 60,
+};
+
+/* regulator supplies */
+static const char * const ov5640_supply_name[] = {
+ "DOVDD", /* Digital I/O (1.8V) supply */
+ "AVDD", /* Analog (2.8V) supply */
+ "DVDD", /* Digital Core (1.5V) supply */
+};
+
+#define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov5640_downsize_mode {
+ SUBSAMPLING,
+ SCALING,
+};
+
+struct reg_value {
+ u16 reg_addr;
+ u8 val;
+ u8 mask;
+ u32 delay_ms;
+};
+
+struct ov5640_mode_info {
+ enum ov5640_mode_id id;
+ enum ov5640_downsize_mode dn_mode;
+ u32 hact;
+ u32 htot;
+ u32 vact;
+ u32 vtot;
+ const struct reg_value *reg_data;
+ u32 reg_data_size;
+ u32 max_fps;
+};
+
+struct ov5640_ctrls {
+ struct v4l2_ctrl_handler handler;
+ struct v4l2_ctrl *pixel_rate;
+ struct {
+ struct v4l2_ctrl *auto_exp;
+ struct v4l2_ctrl *exposure;
+ };
+ struct {
+ struct v4l2_ctrl *auto_wb;
+ struct v4l2_ctrl *blue_balance;
+ struct v4l2_ctrl *red_balance;
+ };
+ struct {
+ struct v4l2_ctrl *auto_gain;
+ struct v4l2_ctrl *gain;
+ };
+ struct v4l2_ctrl *brightness;
+ struct v4l2_ctrl *light_freq;
+ struct v4l2_ctrl *saturation;
+ struct v4l2_ctrl *contrast;
+ struct v4l2_ctrl *hue;
+ struct v4l2_ctrl *test_pattern;
+ struct v4l2_ctrl *hflip;
+ struct v4l2_ctrl *vflip;
+};
+
+struct ov5640_dev {
+ struct i2c_client *i2c_client;
+ struct v4l2_subdev sd;
+ struct media_pad pad;
+ struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+ struct clk *xclk; /* system clock to OV5640 */
+ u32 xclk_freq;
+
+ struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
+ struct gpio_desc *reset_gpio;
+ struct gpio_desc *pwdn_gpio;
+ bool upside_down;
+
+ /* lock to protect all members below */
+ struct mutex lock;
+
+ int power_count;
+
+ struct v4l2_mbus_framefmt fmt;
+ bool pending_fmt_change;
+
+ const struct ov5640_mode_info *current_mode;
+ const struct ov5640_mode_info *last_mode;
+ enum ov5640_frame_rate current_fr;
+ struct v4l2_fract frame_interval;
+
+ struct ov5640_ctrls ctrls;
+
+ u32 prev_sysclk, prev_hts;
+ u32 ae_low, ae_high, ae_target;
+
+ bool pending_mode_change;
+ bool streaming;
+};
+
+static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
+{
+ return container_of(sd, struct ov5640_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+ return &container_of(ctrl->handler, struct ov5640_dev,
+ ctrls.handler)->sd;
+}
+
+/*
+ * FIXME: all of these register tables are likely filled with
+ * entries that set the register to their power-on default values,
+ * and which are otherwise not touched by this driver. Those entries
+ * should be identified and removed to speed register load time
+ * over i2c.
+ */
+/* YUV422 UYVY VGA@30fps */
+static const struct reg_value ov5640_init_setting_30fps_VGA[] = {
+ {0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
+ {0x3103, 0x03, 0, 0}, {0x3630, 0x36, 0, 0},
+ {0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
+ {0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
+ {0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
+ {0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
+ {0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
+ {0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
+ {0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
+ {0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
+ {0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
+ {0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
+ {0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
+ {0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
+ {0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
+ {0x501f, 0x00, 0, 0}, {0x4407, 0x04, 0, 0},
+ {0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x4837, 0x0a, 0, 0}, {0x3824, 0x02, 0, 0},
+ {0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
+ {0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
+ {0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
+ {0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
+ {0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
+ {0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
+ {0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
+ {0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
+ {0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
+ {0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
+ {0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
+ {0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
+ {0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
+ {0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
+ {0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
+ {0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
+ {0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
+ {0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
+ {0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
+ {0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
+ {0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
+ {0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
+ {0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
+ {0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
+ {0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
+ {0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
+ {0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
+ {0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
+ {0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
+ {0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
+ {0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
+ {0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
+ {0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
+ {0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
+ {0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
+ {0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
+ {0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
+ {0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
+ {0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
+ {0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
+ {0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
+ {0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
+ {0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
+ {0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
+ {0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
+ {0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
+ {0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
+ {0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
+ {0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
+ {0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
+};
+
+static const struct reg_value ov5640_setting_VGA_640_480[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_XGA_1024_768[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QVGA_320_240[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QCIF_176_144[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_NTSC_720_480[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_PAL_720_576[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x38, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_720P_1280_720[] = {
+ {0x3c07, 0x07, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x31, 0, 0},
+ {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+ {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
+ {0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
+ {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
+ {0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+ {0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x11, 0, 0},
+ {0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+ {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+ {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
+ {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0},
+ {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
+ {0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
+ {0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0},
+ {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
+ {0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
+ {0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
+ {0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
+ {0x3a15, 0x60, 0, 0}, {0x4407, 0x04, 0, 0},
+ {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
+ {0x4005, 0x1a, 0, 0},
+};
+
+static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
+ {0x3c07, 0x08, 0, 0},
+ {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
+ {0x3814, 0x11, 0, 0},
+ {0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
+ {0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
+ {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
+ {0x3810, 0x00, 0, 0},
+ {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
+ {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
+ {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
+ {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
+ {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
+ {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
+ {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
+ {0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
+ {0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 70},
+};
+
+/* power-on sensor init reg table */
+static const struct ov5640_mode_info ov5640_mode_init_data = {
+ 0, SUBSAMPLING, 640, 1896, 480, 984,
+ ov5640_init_setting_30fps_VGA,
+ ARRAY_SIZE(ov5640_init_setting_30fps_VGA),
+ OV5640_30_FPS,
+};
+
+static const struct ov5640_mode_info
+ov5640_mode_data[OV5640_NUM_MODES] = {
+ {OV5640_MODE_QCIF_176_144, SUBSAMPLING,
+ 176, 1896, 144, 984,
+ ov5640_setting_QCIF_176_144,
+ ARRAY_SIZE(ov5640_setting_QCIF_176_144),
+ OV5640_30_FPS},
+ {OV5640_MODE_QVGA_320_240, SUBSAMPLING,
+ 320, 1896, 240, 984,
+ ov5640_setting_QVGA_320_240,
+ ARRAY_SIZE(ov5640_setting_QVGA_320_240),
+ OV5640_30_FPS},
+ {OV5640_MODE_VGA_640_480, SUBSAMPLING,
+ 640, 1896, 480, 1080,
+ ov5640_setting_VGA_640_480,
+ ARRAY_SIZE(ov5640_setting_VGA_640_480),
+ OV5640_60_FPS},
+ {OV5640_MODE_NTSC_720_480, SUBSAMPLING,
+ 720, 1896, 480, 984,
+ ov5640_setting_NTSC_720_480,
+ ARRAY_SIZE(ov5640_setting_NTSC_720_480),
+ OV5640_30_FPS},
+ {OV5640_MODE_PAL_720_576, SUBSAMPLING,
+ 720, 1896, 576, 984,
+ ov5640_setting_PAL_720_576,
+ ARRAY_SIZE(ov5640_setting_PAL_720_576),
+ OV5640_30_FPS},
+ {OV5640_MODE_XGA_1024_768, SUBSAMPLING,
+ 1024, 1896, 768, 1080,
+ ov5640_setting_XGA_1024_768,
+ ARRAY_SIZE(ov5640_setting_XGA_1024_768),
+ OV5640_30_FPS},
+ {OV5640_MODE_720P_1280_720, SUBSAMPLING,
+ 1280, 1892, 720, 740,
+ ov5640_setting_720P_1280_720,
+ ARRAY_SIZE(ov5640_setting_720P_1280_720),
+ OV5640_30_FPS},
+ {OV5640_MODE_1080P_1920_1080, SCALING,
+ 1920, 2500, 1080, 1120,
+ ov5640_setting_1080P_1920_1080,
+ ARRAY_SIZE(ov5640_setting_1080P_1920_1080),
+ OV5640_30_FPS},
+ {OV5640_MODE_QSXGA_2592_1944, SCALING,
+ 2592, 2844, 1944, 1968,
+ ov5640_setting_QSXGA_2592_1944,
+ ARRAY_SIZE(ov5640_setting_QSXGA_2592_1944),
+ OV5640_15_FPS},
+};
+
+static int ov5640_init_slave_id(struct ov5640_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg;
+ u8 buf[3];
+ int ret;
+
+ if (client->addr == OV5640_DEFAULT_SLAVE_ID)
+ return 0;
+
+ buf[0] = OV5640_REG_SLAVE_ID >> 8;
+ buf[1] = OV5640_REG_SLAVE_ID & 0xff;
+ buf[2] = client->addr << 1;
+
+ msg.addr = OV5640_DEFAULT_SLAVE_ID;
+ msg.flags = 0;
+ msg.buf = buf;
+ msg.len = sizeof(buf);
+
+ ret = i2c_transfer(client->adapter, &msg, 1);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
+ return ret;
+ }
+
+ return 0;
+}
+
+static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg;
+ u8 buf[3];
+ int ret;
+
+ buf[0] = reg >> 8;
+ buf[1] = reg & 0xff;
+ buf[2] = val;
+
+ msg.addr = client->addr;
+ msg.flags = client->flags;
+ msg.buf = buf;
+ msg.len = sizeof(buf);
+
+ ret = i2c_transfer(client->adapter, &msg, 1);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+ __func__, reg, val);
+ return ret;
+ }
+
+ return 0;
+}
+
+static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg[2];
+ u8 buf[2];
+ int ret;
+
+ buf[0] = reg >> 8;
+ buf[1] = reg & 0xff;
+
+ msg[0].addr = client->addr;
+ msg[0].flags = client->flags;
+ msg[0].buf = buf;
+ msg[0].len = sizeof(buf);
+
+ msg[1].addr = client->addr;
+ msg[1].flags = client->flags | I2C_M_RD;
+ msg[1].buf = buf;
+ msg[1].len = 1;
+
+ ret = i2c_transfer(client->adapter, msg, 2);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: error: reg=%x\n",
+ __func__, reg);
+ return ret;
+ }
+
+ *val = buf[0];
+ return 0;
+}
+
+static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
+{
+ u8 hi, lo;
+ int ret;
+
+ ret = ov5640_read_reg(sensor, reg, &hi);
+ if (ret)
+ return ret;
+ ret = ov5640_read_reg(sensor, reg + 1, &lo);
+ if (ret)
+ return ret;
+
+ *val = ((u16)hi << 8) | (u16)lo;
+ return 0;
+}
+
+static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
+{
+ int ret;
+
+ ret = ov5640_write_reg(sensor, reg, val >> 8);
+ if (ret)
+ return ret;
+
+ return ov5640_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
+ u8 mask, u8 val)
+{
+ u8 readval;
+ int ret;
+
+ ret = ov5640_read_reg(sensor, reg, &readval);
+ if (ret)
+ return ret;
+
+ readval &= ~mask;
+ val &= mask;
+ val |= readval;
+
+ return ov5640_write_reg(sensor, reg, val);
+}
+
+/*
+ * After trying the various combinations, reading various
+ * documentations spread around the net, and from the various
+ * feedback, the clock tree is probably as follows:
+ *
+ * +--------------+
+ * | Ext. Clock |
+ * +-+------------+
+ * | +----------+
+ * +->| PLL1 | - reg 0x3036, for the multiplier
+ * +-+--------+ - reg 0x3037, bits 0-3 for the pre-divider
+ * | +--------------+
+ * +->| System Clock | - reg 0x3035, bits 4-7
+ * +-+------------+
+ * | +--------------+
+ * +->| MIPI Divider | - reg 0x3035, bits 0-3
+ * | +-+------------+
+ * | +----------------> MIPI SCLK
+ * | + +-----+
+ * | +->| / 2 |-------> MIPI BIT CLK
+ * | +-----+
+ * | +--------------+
+ * +->| PLL Root Div | - reg 0x3037, bit 4
+ * +-+------------+
+ * | +---------+
+ * +->| Bit Div | - reg 0x3034, bits 0-3
+ * +-+-------+
+ * | +-------------+
+ * +->| SCLK Div | - reg 0x3108, bits 0-1
+ * | +-+-----------+
+ * | +---------------> SCLK
+ * | +-------------+
+ * +->| SCLK 2X Div | - reg 0x3108, bits 2-3
+ * | +-+-----------+
+ * | +---------------> SCLK 2X
+ * | +-------------+
+ * +->| PCLK Div | - reg 0x3108, bits 4-5
+ * ++------------+
+ * + +-----------+
+ * +->| P_DIV | - reg 0x3035, bits 0-3
+ * +-----+-----+
+ * +------------> PCLK
+ *
+ * This is deviating from the datasheet at least for the register
+ * 0x3108, since it's said here that the PCLK would be clocked from
+ * the PLL.
+ *
+ * There seems to be also (unverified) constraints:
+ * - the PLL pre-divider output rate should be in the 4-27MHz range
+ * - the PLL multiplier output rate should be in the 500-1000MHz range
+ * - PCLK >= SCLK * 2 in YUV, >= SCLK in Raw or JPEG
+ *
+ * In the two latter cases, these constraints are met since our
+ * factors are hardcoded. If we were to change that, we would need to
+ * take this into account. The only varying parts are the PLL
+ * multiplier and the system clock divider, which are shared between
+ * all these clocks so won't cause any issue.
+ */
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 3 in the vendor kernels.
+ */
+#define OV5640_PLL_PREDIV 3
+
+#define OV5640_PLL_MULT_MIN 4
+#define OV5640_PLL_MULT_MAX 252
+
+/*
+ * This is supposed to be ranging from 1 to 16, but the value is
+ * always set to either 1 or 2 in the vendor kernels.
+ */
+#define OV5640_SYSDIV_MIN 1
+#define OV5640_SYSDIV_MAX 16
+
+/*
+ * Hardcode these values for scaler and non-scaler modes.
+ * FIXME: to be re-calcualted for 1 data lanes setups
+ */
+#define OV5640_MIPI_DIV_PCLK 2
+#define OV5640_MIPI_DIV_SCLK 1
+
+/*
+ * This is supposed to be ranging from 1 to 2, but the value is always
+ * set to 2 in the vendor kernels.
+ */
+#define OV5640_PLL_ROOT_DIV 2
+#define OV5640_PLL_CTRL3_PLL_ROOT_DIV_2 BIT(4)
+
+/*
+ * We only supports 8-bit formats at the moment
+ */
+#define OV5640_BIT_DIV 2
+#define OV5640_PLL_CTRL0_MIPI_MODE_8BIT 0x08
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 2 in the vendor kernels.
+ */
+#define OV5640_SCLK_ROOT_DIV 2
+
+/*
+ * This is hardcoded so that the consistency is maintained between SCLK and
+ * SCLK 2x.
+ */
+#define OV5640_SCLK2X_ROOT_DIV (OV5640_SCLK_ROOT_DIV / 2)
+
+/*
+ * This is supposed to be ranging from 1 to 8, but the value is always
+ * set to 1 in the vendor kernels.
+ */
+#define OV5640_PCLK_ROOT_DIV 1
+#define OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS 0x00
+
+static unsigned long ov5640_compute_sys_clk(struct ov5640_dev *sensor,
+ u8 pll_prediv, u8 pll_mult,
+ u8 sysdiv)
+{
+ unsigned long sysclk = sensor->xclk_freq / pll_prediv * pll_mult;
+
+ /* PLL1 output cannot exceed 1GHz. */
+ if (sysclk / 1000000 > 1000)
+ return 0;
+
+ return sysclk / sysdiv;
+}
+
+static unsigned long ov5640_calc_sys_clk(struct ov5640_dev *sensor,
+ unsigned long rate,
+ u8 *pll_prediv, u8 *pll_mult,
+ u8 *sysdiv)
+{
+ unsigned long best = ~0;
+ u8 best_sysdiv = 1, best_mult = 1;
+ u8 _sysdiv, _pll_mult;
+
+ for (_sysdiv = OV5640_SYSDIV_MIN;
+ _sysdiv <= OV5640_SYSDIV_MAX;
+ _sysdiv++) {
+ for (_pll_mult = OV5640_PLL_MULT_MIN;
+ _pll_mult <= OV5640_PLL_MULT_MAX;
+ _pll_mult++) {
+ unsigned long _rate;
+
+ /*
+ * The PLL multiplier cannot be odd if above
+ * 127.
+ */
+ if (_pll_mult > 127 && (_pll_mult % 2))
+ continue;
+
+ _rate = ov5640_compute_sys_clk(sensor,
+ OV5640_PLL_PREDIV,
+ _pll_mult, _sysdiv);
+
+ /*
+ * We have reached the maximum allowed PLL1 output,
+ * increase sysdiv.
+ */
+ if (!_rate)
+ break;
+
+ /*
+ * Prefer rates above the expected clock rate than
+ * below, even if that means being less precise.
+ */
+ if (_rate < rate)
+ continue;
+
+ if (abs(rate - _rate) < abs(rate - best)) {
+ best = _rate;
+ best_sysdiv = _sysdiv;
+ best_mult = _pll_mult;
+ }
+
+ if (_rate == rate)
+ goto out;
+ }
+ }
+
+out:
+ *sysdiv = best_sysdiv;
+ *pll_prediv = OV5640_PLL_PREDIV;
+ *pll_mult = best_mult;
+
+ return best;
+}
+
+/*
+ * ov5640_set_mipi_pclk() - Calculate the clock tree configuration values
+ * for the MIPI CSI-2 output.
+ *
+ * @rate: The requested bandwidth per lane in bytes per second.
+ * 'Bandwidth Per Lane' is calculated as:
+ * bpl = HTOT * VTOT * FPS * bpp / num_lanes;
+ *
+ * This function use the requested bandwidth to calculate:
+ * - sample_rate = bpl / (bpp / num_lanes);
+ * = bpl / (PLL_RDIV * BIT_DIV * PCLK_DIV * MIPI_DIV / num_lanes);
+ *
+ * - mipi_sclk = bpl / MIPI_DIV / 2; ( / 2 is for CSI-2 DDR)
+ *
+ * with these fixed parameters:
+ * PLL_RDIV = 2;
+ * BIT_DIVIDER = 2; (MIPI_BIT_MODE == 8 ? 2 : 2,5);
+ * PCLK_DIV = 1;
+ *
+ * The MIPI clock generation differs for modes that use the scaler and modes
+ * that do not. In case the scaler is in use, the MIPI_SCLK generates the MIPI
+ * BIT CLk, and thus:
+ *
+ * - mipi_sclk = bpl / MIPI_DIV / 2;
+ * MIPI_DIV = 1;
+ *
+ * For modes that do not go through the scaler, the MIPI BIT CLOCK is generated
+ * from the pixel clock, and thus:
+ *
+ * - sample_rate = bpl / (bpp / num_lanes);
+ * = bpl / (2 * 2 * 1 * MIPI_DIV / num_lanes);
+ * = bpl / (4 * MIPI_DIV / num_lanes);
+ * - MIPI_DIV = bpp / (4 * num_lanes);
+ *
+ * FIXME: this have been tested with 16bpp and 2 lanes setup only.
+ * MIPI_DIV is fixed to value 2, but it -might- be changed according to the
+ * above formula for setups with 1 lane or image formats with different bpp.
+ *
+ * FIXME: this deviates from the sensor manual documentation which is quite
+ * thin on the MIPI clock tree generation part.
+ */
+static int ov5640_set_mipi_pclk(struct ov5640_dev *sensor,
+ unsigned long rate)
+{
+ const struct ov5640_mode_info *mode = sensor->current_mode;
+ u8 prediv, mult, sysdiv;
+ u8 mipi_div;
+ int ret;
+
+ /*
+ * 1280x720 is reported to use 'SUBSAMPLING' only,
+ * but according to the sensor manual it goes through the
+ * scaler before subsampling.
+ */
+ if (mode->dn_mode == SCALING ||
+ (mode->id == OV5640_MODE_720P_1280_720))
+ mipi_div = OV5640_MIPI_DIV_SCLK;
+ else
+ mipi_div = OV5640_MIPI_DIV_PCLK;
+
+ ov5640_calc_sys_clk(sensor, rate, &prediv, &mult, &sysdiv);
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
+ 0x0f, OV5640_PLL_CTRL0_MIPI_MODE_8BIT);
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
+ 0xff, sysdiv << 4 | mipi_div);
+ if (ret)
+ return ret;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2, 0xff, mult);
+ if (ret)
+ return ret;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
+ 0x1f, OV5640_PLL_CTRL3_PLL_ROOT_DIV_2 | prediv);
+ if (ret)
+ return ret;
+
+ return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER,
+ 0x30, OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS);
+}
+
+static unsigned long ov5640_calc_pclk(struct ov5640_dev *sensor,
+ unsigned long rate,
+ u8 *pll_prediv, u8 *pll_mult, u8 *sysdiv,
+ u8 *pll_rdiv, u8 *bit_div, u8 *pclk_div)
+{
+ unsigned long _rate = rate * OV5640_PLL_ROOT_DIV * OV5640_BIT_DIV *
+ OV5640_PCLK_ROOT_DIV;
+
+ _rate = ov5640_calc_sys_clk(sensor, _rate, pll_prediv, pll_mult,
+ sysdiv);
+ *pll_rdiv = OV5640_PLL_ROOT_DIV;
+ *bit_div = OV5640_BIT_DIV;
+ *pclk_div = OV5640_PCLK_ROOT_DIV;
+
+ return _rate / *pll_rdiv / *bit_div / *pclk_div;
+}
+
+static int ov5640_set_dvp_pclk(struct ov5640_dev *sensor, unsigned long rate)
+{
+ u8 prediv, mult, sysdiv, pll_rdiv, bit_div, pclk_div;
+ int ret;
+
+ ov5640_calc_pclk(sensor, rate, &prediv, &mult, &sysdiv, &pll_rdiv,
+ &bit_div, &pclk_div);
+
+#ifndef CONFIG_VIN_SENSOR_OV5640
+ if (bit_div == 2)
+ bit_div = 8;
+#else
+ bit_div = 0xa;
+#endif
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
+ 0x0f, bit_div);
+ if (ret)
+ return ret;
+
+ /*
+ * We need to set sysdiv according to the clock, and to clear
+ * the MIPI divider.
+ */
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
+ 0xff, sysdiv << 4);
+ if (ret)
+ return ret;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2,
+ 0xff, mult);
+ if (ret)
+ return ret;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
+ 0x1f, prediv | ((pll_rdiv - 1) << 4));
+ if (ret)
+ return ret;
+
+ return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x30,
+ (ilog2(pclk_div) << 4));
+}
+
+/* set JPEG framing sizes */
+static int ov5640_set_jpeg_timings(struct ov5640_dev *sensor,
+ const struct ov5640_mode_info *mode)
+{
+ int ret;
+
+ /*
+ * compression mode 3 timing
+ *
+ * Data is transmitted with programmable width (VFIFO_HSIZE).
+ * No padding done. Last line may have less data. Varying
+ * number of lines per frame, depending on amount of data.
+ */
+ ret = ov5640_mod_reg(sensor, OV5640_REG_JPG_MODE_SELECT, 0x7, 0x3);
+ if (ret < 0)
+ return ret;
+
+ ret = ov5640_write_reg16(sensor, OV5640_REG_VFIFO_HSIZE, mode->hact);
+ if (ret < 0)
+ return ret;
+
+ return ov5640_write_reg16(sensor, OV5640_REG_VFIFO_VSIZE, mode->vact);
+}
+
+/* download ov5640 settings to sensor through i2c */
+static int ov5640_set_timings(struct ov5640_dev *sensor,
+ const struct ov5640_mode_info *mode)
+{
+ int ret;
+
+ if (sensor->fmt.code == MEDIA_BUS_FMT_JPEG_1X8) {
+ ret = ov5640_set_jpeg_timings(sensor, mode);
+ if (ret < 0)
+ return ret;
+ }
+
+ ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPHO, mode->hact);
+ if (ret < 0)
+ return ret;
+
+ ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPVO, mode->vact);
+ if (ret < 0)
+ return ret;
+
+ ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HTS, mode->htot);
+ if (ret < 0)
+ return ret;
+
+ return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, mode->vtot);
+}
+
+static int ov5640_load_regs(struct ov5640_dev *sensor,
+ const struct ov5640_mode_info *mode)
+{
+ const struct reg_value *regs = mode->reg_data;
+ unsigned int i;
+ u32 delay_ms;
+ u16 reg_addr;
+ u8 mask, val;
+ int ret = 0;
+
+ st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
+ for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+ delay_ms = regs->delay_ms;
+ reg_addr = regs->reg_addr;
+ val = regs->val;
+ mask = regs->mask;
+
+ /* remain in power down mode for DVP */
+ if (regs->reg_addr == OV5640_REG_SYS_CTRL0 &&
+ val == OV5640_REG_SYS_CTRL0_SW_PWUP &&
+ sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY)
+ continue;
+
+ if (mask)
+ ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
+ else
+ ret = ov5640_write_reg(sensor, reg_addr, val);
+ if (ret)
+ break;
+
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+
+ return ov5640_set_timings(sensor, mode);
+}
+
+static int ov5640_set_autoexposure(struct ov5640_dev *sensor, bool on)
+{
+ return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+ BIT(0), on ? 0 : BIT(0));
+}
+
+/* read exposure, in number of line periods */
+static int ov5640_get_exposure(struct ov5640_dev *sensor)
+{
+ int exp, ret;
+ u8 temp;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
+ if (ret)
+ return ret;
+ exp = ((int)temp & 0x0f) << 16;
+ ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
+ if (ret)
+ return ret;
+ exp |= ((int)temp << 8);
+ ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
+ if (ret)
+ return ret;
+ exp |= (int)temp;
+
+ return exp >> 4;
+}
+
+/* write exposure, given number of line periods */
+static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
+{
+ int ret;
+
+ exposure <<= 4;
+
+ ret = ov5640_write_reg(sensor,
+ OV5640_REG_AEC_PK_EXPOSURE_LO,
+ exposure & 0xff);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor,
+ OV5640_REG_AEC_PK_EXPOSURE_MED,
+ (exposure >> 8) & 0xff);
+ if (ret)
+ return ret;
+ return ov5640_write_reg(sensor,
+ OV5640_REG_AEC_PK_EXPOSURE_HI,
+ (exposure >> 16) & 0x0f);
+}
+
+static int ov5640_get_gain(struct ov5640_dev *sensor)
+{
+ u16 gain;
+ int ret;
+
+ ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
+ if (ret)
+ return ret;
+
+ return gain & 0x3ff;
+}
+
+static int ov5640_set_gain(struct ov5640_dev *sensor, int gain)
+{
+ return ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
+ (u16)gain & 0x3ff);
+}
+
+static int ov5640_set_autogain(struct ov5640_dev *sensor, bool on)
+{
+ return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
+ BIT(1), on ? 0 : BIT(1));
+}
+
+static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
+{
+ return ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0, on ?
+ OV5640_REG_SYS_CTRL0_SW_PWUP :
+ OV5640_REG_SYS_CTRL0_SW_PWDN);
+}
+
+static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
+{
+ int ret;
+
+ /*
+ * Enable/disable the MIPI interface
+ *
+ * 0x300e = on ? 0x45 : 0x40
+ *
+ * FIXME: the sensor manual (version 2.03) reports
+ * [7:5] = 000 : 1 data lane mode
+ * [7:5] = 001 : 2 data lanes mode
+ * But this settings do not work, while the following ones
+ * have been validated for 2 data lanes mode.
+ *
+ * [7:5] = 010 : 2 data lanes mode
+ * [4] = 0 : Power up MIPI HS Tx
+ * [3] = 0 : Power up MIPI LS Rx
+ * [2] = 1/0 : MIPI interface enable/disable
+ * [1:0] = 01/00: FIXME: 'debug'
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00,
+ on ? 0x45 : 0x40);
+ if (ret)
+ return ret;
+
+ return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
+ on ? 0x00 : 0x0f);
+}
+
+static int ov5640_get_sysclk(struct ov5640_dev *sensor)
+{
+ /* calculate sysclk */
+ u32 xvclk = sensor->xclk_freq / 10000;
+ u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
+ u32 sclk_rdiv_map[] = {1, 2, 4, 8};
+ u32 bit_div2x = 1, sclk_rdiv, sysclk;
+ u8 temp1, temp2;
+ int ret;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
+ if (ret)
+ return ret;
+ temp2 = temp1 & 0x0f;
+ if (temp2 == 8 || temp2 == 10)
+ bit_div2x = temp2 / 2;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
+ if (ret)
+ return ret;
+ sysdiv = temp1 >> 4;
+ if (sysdiv == 0)
+ sysdiv = 16;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
+ if (ret)
+ return ret;
+ multiplier = temp1;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
+ if (ret)
+ return ret;
+ prediv = temp1 & 0x0f;
+ pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
+ if (ret)
+ return ret;
+ temp2 = temp1 & 0x03;
+ sclk_rdiv = sclk_rdiv_map[temp2];
+
+ if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
+ return -EINVAL;
+
+ VCO = xvclk * multiplier / prediv;
+
+ sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
+
+ return sysclk;
+}
+
+static int ov5640_set_night_mode(struct ov5640_dev *sensor)
+{
+ /* read HTS from register settings */
+ u8 mode;
+ int ret;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
+ if (ret)
+ return ret;
+ mode &= 0xfb;
+ return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
+}
+
+static int ov5640_get_hts(struct ov5640_dev *sensor)
+{
+ /* read HTS from register settings */
+ u16 hts;
+ int ret;
+
+ ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
+ if (ret)
+ return ret;
+ return hts;
+}
+
+static int ov5640_get_vts(struct ov5640_dev *sensor)
+{
+ u16 vts;
+ int ret;
+
+ ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
+ if (ret)
+ return ret;
+ return vts;
+}
+
+static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
+{
+ return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
+}
+
+static int ov5640_get_light_freq(struct ov5640_dev *sensor)
+{
+ /* get banding filter value */
+ int ret, light_freq = 0;
+ u8 temp, temp1;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
+ if (ret)
+ return ret;
+
+ if (temp & 0x80) {
+ /* manual */
+ ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
+ &temp1);
+ if (ret)
+ return ret;
+ if (temp1 & 0x04) {
+ /* 50Hz */
+ light_freq = 50;
+ } else {
+ /* 60Hz */
+ light_freq = 60;
+ }
+ } else {
+ /* auto */
+ ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
+ &temp1);
+ if (ret)
+ return ret;
+
+ if (temp1 & 0x01) {
+ /* 50Hz */
+ light_freq = 50;
+ } else {
+ /* 60Hz */
+ }
+ }
+
+ return light_freq;
+}
+
+static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
+{
+ u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
+ int ret;
+
+ /* read preview PCLK */
+ ret = ov5640_get_sysclk(sensor);
+ if (ret < 0)
+ return ret;
+ if (ret == 0)
+ return -EINVAL;
+ sensor->prev_sysclk = ret;
+ /* read preview HTS */
+ ret = ov5640_get_hts(sensor);
+ if (ret < 0)
+ return ret;
+ if (ret == 0)
+ return -EINVAL;
+ sensor->prev_hts = ret;
+
+ /* read preview VTS */
+ ret = ov5640_get_vts(sensor);
+ if (ret < 0)
+ return ret;
+ prev_vts = ret;
+
+ /* calculate banding filter */
+ /* 60Hz */
+ band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
+ ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
+ if (ret)
+ return ret;
+ if (!band_step60)
+ return -EINVAL;
+ max_band60 = (int)((prev_vts - 4) / band_step60);
+ ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
+ if (ret)
+ return ret;
+
+ /* 50Hz */
+ band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
+ ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
+ if (ret)
+ return ret;
+ if (!band_step50)
+ return -EINVAL;
+ max_band50 = (int)((prev_vts - 4) / band_step50);
+ return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
+}
+
+static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
+{
+ /* stable in high */
+ u32 fast_high, fast_low;
+ int ret;
+
+ sensor->ae_low = target * 23 / 25; /* 0.92 */
+ sensor->ae_high = target * 27 / 25; /* 1.08 */
+
+ fast_high = sensor->ae_high << 1;
+ if (fast_high > 255)
+ fast_high = 255;
+
+ fast_low = sensor->ae_low >> 1;
+
+ ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
+ if (ret)
+ return ret;
+ return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
+}
+
+static int ov5640_get_binning(struct ov5640_dev *sensor)
+{
+ u8 temp;
+ int ret;
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
+ if (ret)
+ return ret;
+
+ return temp & BIT(0);
+}
+
+static int ov5640_set_binning(struct ov5640_dev *sensor, bool enable)
+{
+ int ret;
+
+ /*
+ * TIMING TC REG21:
+ * - [0]: Horizontal binning enable
+ */
+ ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+ BIT(0), enable ? BIT(0) : 0);
+ if (ret)
+ return ret;
+ /*
+ * TIMING TC REG20:
+ * - [0]: Undocumented, but hardcoded init sequences
+ * are always setting REG21/REG20 bit 0 to same value...
+ */
+ return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
+ BIT(0), enable ? BIT(0) : 0);
+}
+
+static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ u8 temp, channel = virtual_channel;
+ int ret;
+
+ if (channel > 3) {
+ dev_err(&client->dev,
+ "%s: wrong virtual_channel parameter, expected (0..3), got %d\n",
+ __func__, channel);
+ return -EINVAL;
+ }
+
+ ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
+ if (ret)
+ return ret;
+ temp &= ~(3 << 6);
+ temp |= (channel << 6);
+ return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
+}
+
+static const struct ov5640_mode_info *
+ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
+ int width, int height, bool nearest)
+{
+ const struct ov5640_mode_info *mode;
+
+ mode = v4l2_find_nearest_size(ov5640_mode_data,
+ ARRAY_SIZE(ov5640_mode_data),
+ hact, vact,
+ width, height);
+
+ if (!mode ||
+ (!nearest && (mode->hact != width || mode->vact != height)))
+ return NULL;
+
+ /* Check to see if the current mode exceeds the max frame rate */
+ if (ov5640_framerates[fr] > ov5640_framerates[mode->max_fps])
+ return NULL;
+
+ return mode;
+}
+
+static u64 ov5640_calc_pixel_rate(struct ov5640_dev *sensor)
+{
+ u64 rate;
+
+ rate = sensor->current_mode->vtot * sensor->current_mode->htot;
+ rate *= ov5640_framerates[sensor->current_fr];
+
+ return rate;
+}
+
+/*
+ * sensor changes between scaling and subsampling, go through
+ * exposure calculation
+ */
+static int ov5640_set_mode_exposure_calc(struct ov5640_dev *sensor,
+ const struct ov5640_mode_info *mode)
+{
+ u32 prev_shutter, prev_gain16;
+ u32 cap_shutter, cap_gain16;
+ u32 cap_sysclk, cap_hts, cap_vts;
+ u32 light_freq, cap_bandfilt, cap_maxband;
+ u32 cap_gain16_shutter;
+ u8 average;
+ int ret;
+
+ if (!mode->reg_data)
+ return -EINVAL;
+
+ /* read preview shutter */
+ ret = ov5640_get_exposure(sensor);
+ if (ret < 0)
+ return ret;
+ prev_shutter = ret;
+ ret = ov5640_get_binning(sensor);
+ if (ret < 0)
+ return ret;
+ if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
+ mode->id != OV5640_MODE_1080P_1920_1080)
+ prev_shutter *= 2;
+
+ /* read preview gain */
+ ret = ov5640_get_gain(sensor);
+ if (ret < 0)
+ return ret;
+ prev_gain16 = ret;
+
+ /* get average */
+ ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
+ if (ret)
+ return ret;
+
+ /* turn off night mode for capture */
+ ret = ov5640_set_night_mode(sensor);
+ if (ret < 0)
+ return ret;
+
+ /* Write capture setting */
+ ret = ov5640_load_regs(sensor, mode);
+ if (ret < 0)
+ return ret;
+
+ /* read capture VTS */
+ ret = ov5640_get_vts(sensor);
+ if (ret < 0)
+ return ret;
+ cap_vts = ret;
+ ret = ov5640_get_hts(sensor);
+ if (ret < 0)
+ return ret;
+ if (ret == 0)
+ return -EINVAL;
+ cap_hts = ret;
+
+ ret = ov5640_get_sysclk(sensor);
+ if (ret < 0)
+ return ret;
+ if (ret == 0)
+ return -EINVAL;
+ cap_sysclk = ret;
+
+ /* calculate capture banding filter */
+ ret = ov5640_get_light_freq(sensor);
+ if (ret < 0)
+ return ret;
+ light_freq = ret;
+
+ if (light_freq == 60) {
+ /* 60Hz */
+ cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
+ } else {
+ /* 50Hz */
+ cap_bandfilt = cap_sysclk * 100 / cap_hts;
+ }
+
+ if (!sensor->prev_sysclk) {
+ ret = ov5640_get_sysclk(sensor);
+ if (ret < 0)
+ return ret;
+ if (ret == 0)
+ return -EINVAL;
+ sensor->prev_sysclk = ret;
+ }
+
+ if (!cap_bandfilt)
+ return -EINVAL;
+
+ cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
+
+ /* calculate capture shutter/gain16 */
+ if (average > sensor->ae_low && average < sensor->ae_high) {
+ /* in stable range */
+ cap_gain16_shutter =
+ prev_gain16 * prev_shutter *
+ cap_sysclk / sensor->prev_sysclk *
+ sensor->prev_hts / cap_hts *
+ sensor->ae_target / average;
+ } else {
+ cap_gain16_shutter =
+ prev_gain16 * prev_shutter *
+ cap_sysclk / sensor->prev_sysclk *
+ sensor->prev_hts / cap_hts;
+ }
+
+ /* gain to shutter */
+ if (cap_gain16_shutter < (cap_bandfilt * 16)) {
+ /* shutter < 1/100 */
+ cap_shutter = cap_gain16_shutter / 16;
+ if (cap_shutter < 1)
+ cap_shutter = 1;
+
+ cap_gain16 = cap_gain16_shutter / cap_shutter;
+ if (cap_gain16 < 16)
+ cap_gain16 = 16;
+ } else {
+ if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
+ /* exposure reach max */
+ cap_shutter = cap_bandfilt * cap_maxband;
+ if (!cap_shutter)
+ return -EINVAL;
+
+ cap_gain16 = cap_gain16_shutter / cap_shutter;
+ } else {
+ /* 1/100 < (cap_shutter = n/100) =< max */
+ cap_shutter =
+ ((int)(cap_gain16_shutter / 16 / cap_bandfilt))
+ * cap_bandfilt;
+ if (!cap_shutter)
+ return -EINVAL;
+
+ cap_gain16 = cap_gain16_shutter / cap_shutter;
+ }
+ }
+
+ /* set capture gain */
+ ret = ov5640_set_gain(sensor, cap_gain16);
+ if (ret)
+ return ret;
+
+ /* write capture shutter */
+ if (cap_shutter > (cap_vts - 4)) {
+ cap_vts = cap_shutter + 4;
+ ret = ov5640_set_vts(sensor, cap_vts);
+ if (ret < 0)
+ return ret;
+ }
+
+ /* set exposure */
+ return ov5640_set_exposure(sensor, cap_shutter);
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
+ const struct ov5640_mode_info *mode)
+{
+ if (!mode->reg_data)
+ return -EINVAL;
+
+ /* Write capture setting */
+ return ov5640_load_regs(sensor, mode);
+}
+
+static int ov5640_set_mode(struct ov5640_dev *sensor)
+{
+ const struct ov5640_mode_info *mode = sensor->current_mode;
+ const struct ov5640_mode_info *orig_mode = sensor->last_mode;
+ enum ov5640_downsize_mode dn_mode, orig_dn_mode;
+ bool auto_gain = sensor->ctrls.auto_gain->val == 1;
+ bool auto_exp = sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
+ unsigned long rate;
+ int ret;
+
+ dn_mode = mode->dn_mode;
+ orig_dn_mode = orig_mode->dn_mode;
+
+ /* auto gain and exposure must be turned off when changing modes */
+ if (auto_gain) {
+ ret = ov5640_set_autogain(sensor, false);
+ if (ret)
+ return ret;
+ }
+
+ if (auto_exp) {
+ ret = ov5640_set_autoexposure(sensor, false);
+ if (ret)
+ goto restore_auto_gain;
+ }
+
+ /*
+ * All the formats we support have 16 bits per pixel, seems to require
+ * the same rate than YUV, so we can just use 16 bpp all the time.
+ */
+ rate = ov5640_calc_pixel_rate(sensor) * 16;
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY) {
+ rate = rate / sensor->ep.bus.mipi_csi2.num_data_lanes;
+ ret = ov5640_set_mipi_pclk(sensor, rate);
+ } else {
+ rate = rate / sensor->ep.bus.parallel.bus_width;
+ ret = ov5640_set_dvp_pclk(sensor, rate);
+ }
+
+ if (ret < 0)
+ return 0;
+
+ if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
+ (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
+ /*
+ * change between subsampling and scaling
+ * go through exposure calculation
+ */
+ ret = ov5640_set_mode_exposure_calc(sensor, mode);
+ } else {
+ /*
+ * change inside subsampling or scaling
+ * download firmware directly
+ */
+ ret = ov5640_set_mode_direct(sensor, mode);
+ }
+ if (ret < 0)
+ goto restore_auto_exp_gain;
+
+ /* restore auto gain and exposure */
+ if (auto_gain)
+ ov5640_set_autogain(sensor, true);
+ if (auto_exp)
+ ov5640_set_autoexposure(sensor, true);
+
+ ret = ov5640_set_binning(sensor, dn_mode != SCALING);
+ if (ret < 0)
+ return ret;
+ ret = ov5640_set_ae_target(sensor, sensor->ae_target);
+ if (ret < 0)
+ return ret;
+ ret = ov5640_get_light_freq(sensor);
+ if (ret < 0)
+ return ret;
+ ret = ov5640_set_bandingfilter(sensor);
+ if (ret < 0)
+ return ret;
+ ret = ov5640_set_virtual_channel(sensor);
+ if (ret < 0)
+ return ret;
+
+ sensor->pending_mode_change = false;
+ sensor->last_mode = mode;
+
+ return 0;
+
+restore_auto_exp_gain:
+ if (auto_exp)
+ ov5640_set_autoexposure(sensor, true);
+restore_auto_gain:
+ if (auto_gain)
+ ov5640_set_autogain(sensor, true);
+
+ return ret;
+}
+
+static int ov5640_set_framefmt(struct ov5640_dev *sensor,
+ struct v4l2_mbus_framefmt *format);
+
+/* restore the last set video mode after chip power-on */
+static int ov5640_restore_mode(struct ov5640_dev *sensor)
+{
+ int ret;
+
+ /* first load the initial register values */
+ ret = ov5640_load_regs(sensor, &ov5640_mode_init_data);
+ if (ret < 0)
+ return ret;
+ sensor->last_mode = &ov5640_mode_init_data;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
+ (ilog2(OV5640_SCLK2X_ROOT_DIV) << 2) |
+ ilog2(OV5640_SCLK_ROOT_DIV));
+ if (ret)
+ return ret;
+
+ /* now restore the last capture mode */
+ ret = ov5640_set_mode(sensor);
+ if (ret < 0)
+ return ret;
+
+ return ov5640_set_framefmt(sensor, &sensor->fmt);
+}
+
+static void ov5640_power(struct ov5640_dev *sensor, bool enable)
+{
+ if (!sensor->pwdn_gpio)
+ return;
+ gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void ov5640_reset(struct ov5640_dev *sensor)
+{
+ if (!sensor->reset_gpio)
+ return;
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+
+ /* camera power cycle */
+ ov5640_power(sensor, false);
+ usleep_range(5000, 10000);
+ ov5640_power(sensor, true);
+ usleep_range(5000, 10000);
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+ usleep_range(1000, 2000);
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+ usleep_range(20000, 25000);
+}
+
+static int ov5640_set_power_on(struct ov5640_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret;
+
+ ret = clk_prepare_enable(sensor->xclk);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable clock\n",
+ __func__);
+ return ret;
+ }
+
+ ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
+ sensor->supplies);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable regulators\n",
+ __func__);
+ goto xclk_off;
+ }
+
+ ov5640_reset(sensor);
+ ov5640_power(sensor, true);
+
+ ret = ov5640_init_slave_id(sensor);
+ if (ret)
+ goto power_off;
+
+ return 0;
+
+power_off:
+ ov5640_power(sensor, false);
+ regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+xclk_off:
+ clk_disable_unprepare(sensor->xclk);
+ return ret;
+}
+
+static void ov5640_set_power_off(struct ov5640_dev *sensor)
+{
+ ov5640_power(sensor, false);
+ regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
+ clk_disable_unprepare(sensor->xclk);
+}
+
+static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
+{
+ int ret;
+
+ if (!on) {
+ /* Reset MIPI bus settings to their default values. */
+ ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+ ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x04);
+ ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x00);
+ return 0;
+ }
+
+ /*
+ * Power up MIPI HS Tx and LS Rx; 2 data lanes mode
+ *
+ * 0x300e = 0x40
+ * [7:5] = 010 : 2 data lanes mode (see FIXME note in
+ * "ov5640_set_stream_mipi()")
+ * [4] = 0 : Power up MIPI HS Tx
+ * [3] = 0 : Power up MIPI LS Rx
+ * [2] = 0 : MIPI interface disabled
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x40);
+ if (ret)
+ return ret;
+
+ /*
+ * Gate clock and set LP11 in 'no packets mode' (idle)
+ *
+ * 0x4800 = 0x24
+ * [5] = 1 : Gate clock when 'no packets'
+ * [2] = 1 : MIPI bus in LP11 when 'no packets'
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x24);
+ if (ret)
+ return ret;
+
+ /*
+ * Set data lanes and clock in LP11 when 'sleeping'
+ *
+ * 0x3019 = 0x70
+ * [6] = 1 : MIPI data lane 2 in LP11 when 'sleeping'
+ * [5] = 1 : MIPI data lane 1 in LP11 when 'sleeping'
+ * [4] = 1 : MIPI clock lane in LP11 when 'sleeping'
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x70);
+ if (ret)
+ return ret;
+
+ /* Give lanes some time to coax into LP11 state. */
+ usleep_range(500, 1000);
+
+ return 0;
+}
+
+static int ov5640_set_power_dvp(struct ov5640_dev *sensor, bool on)
+{
+ unsigned int flags = sensor->ep.bus.parallel.flags;
+ bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
+ u8 polarities = 0;
+ int ret;
+
+ if (!on) {
+ /* Reset settings to their default values. */
+ ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00, 0x00);
+ ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+ ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, 0x20);
+ ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01, 0x00);
+ ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0x00);
+ return 0;
+ }
+
+ /*
+ * Note about parallel port configuration.
+ *
+ * When configured in parallel mode, the OV5640 will
+ * output 10 bits data on DVP data lines [9:0].
+ * If only 8 bits data are wanted, the 8 bits data lines
+ * of the camera interface must be physically connected
+ * on the DVP data lines [9:2].
+ *
+ * Control lines polarity can be configured through
+ * devicetree endpoint control lines properties.
+ * If no endpoint control lines properties are set,
+ * polarity will be as below:
+ * - VSYNC: active high
+ * - HREF: active low
+ * - PCLK: active low
+ *
+ * VSYNC & HREF are not configured if BT656 bus mode is selected
+ */
+
+ /*
+ * BT656 embedded synchronization configuration
+ *
+ * CCIR656 CTRL00
+ * - [7]: SYNC code selection (0: auto generate sync code,
+ * 1: sync code from regs 0x4732-0x4735)
+ * - [6]: f value in CCIR656 SYNC code when fixed f value
+ * - [5]: Fixed f value
+ * - [4:3]: Blank toggle data options (00: data=1'h040/1'h200,
+ * 01: data from regs 0x4736-0x4738, 10: always keep 0)
+ * - [1]: Clip data disable
+ * - [0]: CCIR656 mode enable
+ *
+ * Default CCIR656 SAV/EAV mode with default codes
+ * SAV=0xff000080 & EAV=0xff00009d is enabled here with settings:
+ * - CCIR656 mode enable
+ * - auto generation of sync codes
+ * - blank toggle data 1'h040/1'h200
+ * - clip reserved data (0x00 & 0xff changed to 0x01 & 0xfe)
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00,
+ bt656 ? 0x01 : 0x00);
+ if (ret)
+ return ret;
+
+ /*
+ * configure parallel port control lines polarity
+ *
+ * POLARITY CTRL0
+ * - [5]: PCLK polarity (0: active low, 1: active high)
+ * - [1]: HREF polarity (0: active low, 1: active high)
+ * - [0]: VSYNC polarity (mismatch here between
+ * datasheet and hardware, 0 is active high
+ * and 1 is active low...)
+ */
+ if (!bt656) {
+ if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+ polarities |= BIT(1);
+ if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+ polarities |= BIT(0);
+ }
+ if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+ polarities |= BIT(5);
+
+ ret = ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, polarities);
+ if (ret)
+ return ret;
+
+ /*
+ * powerdown MIPI TX/RX PHY & enable DVP
+ *
+ * MIPI CONTROL 00
+ * [4] = 1 : Power down MIPI HS Tx
+ * [3] = 1 : Power down MIPI LS Rx
+ * [2] = 0 : DVP enable (MIPI disable)
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
+ if (ret)
+ return ret;
+
+ /*
+ * enable VSYNC/HREF/PCLK DVP control lines
+ * & D[9:6] DVP data lines
+ *
+ * PAD OUTPUT ENABLE 01
+ * - 6: VSYNC output enable
+ * - 5: HREF output enable
+ * - 4: PCLK output enable
+ * - [3:0]: D[9:6] output enable
+ */
+ ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01,
+ bt656 ? 0x1f : 0x7f);
+ if (ret)
+ return ret;
+
+ /*
+ * enable D[5:0] DVP data lines
+ *
+ * PAD OUTPUT ENABLE 02
+ * - [7:2]: D[5:0] output enable
+ */
+ return ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0xfc);
+}
+
+static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
+{
+ int ret = 0;
+
+ if (on) {
+ ret = ov5640_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+ ret = ov5640_restore_mode(sensor);
+ if (ret)
+ goto power_off;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov5640_set_power_mipi(sensor, on);
+ else
+ ret = ov5640_set_power_dvp(sensor, on);
+ if (ret)
+ goto power_off;
+
+ if (!on)
+ ov5640_set_power_off(sensor);
+
+ return 0;
+
+power_off:
+ ov5640_set_power_off(sensor);
+ return ret;
+}
+
+/* --------------- Subdev Operations --------------- */
+
+static int ov5640_s_power(struct v4l2_subdev *sd, int on)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ /*
+ * If the power count is modified from 0 to != 0 or from != 0 to 0,
+ * update the power state.
+ */
+ if (sensor->power_count == !on) {
+ ret = ov5640_set_power(sensor, !!on);
+ if (ret)
+ goto out;
+ }
+
+ /* Update the power count. */
+ sensor->power_count += on ? 1 : -1;
+ WARN_ON(sensor->power_count < 0);
+out:
+ mutex_unlock(&sensor->lock);
+
+ if (on && !ret && sensor->power_count == 1) {
+ /* restore controls */
+ ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+ }
+
+ return ret;
+}
+
+static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
+ struct v4l2_fract *fi,
+ u32 width, u32 height)
+{
+ const struct ov5640_mode_info *mode;
+ enum ov5640_frame_rate rate = OV5640_15_FPS;
+ int minfps, maxfps, best_fps, fps;
+ int i;
+
+ minfps = ov5640_framerates[OV5640_15_FPS];
+ maxfps = ov5640_framerates[OV5640_60_FPS];
+
+ if (fi->numerator == 0) {
+ fi->denominator = maxfps;
+ fi->numerator = 1;
+ rate = OV5640_60_FPS;
+ goto find_mode;
+ }
+
+ fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+ minfps, maxfps);
+
+ best_fps = minfps;
+ for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
+ int curr_fps = ov5640_framerates[i];
+
+ if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+ best_fps = curr_fps;
+ rate = i;
+ }
+ }
+
+ fi->numerator = 1;
+ fi->denominator = best_fps;
+
+find_mode:
+ mode = ov5640_find_mode(sensor, rate, width, height, false);
+ return mode ? rate : -EINVAL;
+}
+
+static int ov5640_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *format)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ struct v4l2_mbus_framefmt *fmt;
+
+ if (format->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+ fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+ format->pad);
+ else
+ fmt = &sensor->fmt;
+
+ format->format = *fmt;
+
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
+ struct v4l2_mbus_framefmt *fmt,
+ enum ov5640_frame_rate fr,
+ const struct ov5640_mode_info **new_mode)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ const struct ov5640_mode_info *mode;
+ int i;
+
+ mode = ov5640_find_mode(sensor, fr, fmt->width, fmt->height, true);
+ if (!mode)
+ return -EINVAL;
+ fmt->width = mode->hact;
+ fmt->height = mode->vact;
+
+ if (new_mode)
+ *new_mode = mode;
+
+ for (i = 0; i < ARRAY_SIZE(ov5640_formats); i++)
+ if (ov5640_formats[i].code == fmt->code)
+ break;
+ if (i >= ARRAY_SIZE(ov5640_formats))
+ i = 0;
+
+ fmt->code = ov5640_formats[i].code;
+ fmt->colorspace = ov5640_formats[i].colorspace;
+ 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);
+
+ return 0;
+}
+
+static int ov5640_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *format)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ const struct ov5640_mode_info *new_mode;
+ struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+ struct v4l2_mbus_framefmt *fmt;
+ int ret;
+
+ if (format->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ ret = ov5640_try_fmt_internal(sd, mbus_fmt, 0, &new_mode);
+ if (ret)
+ goto out;
+
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+ fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+ else
+ fmt = &sensor->fmt;
+
+ if (mbus_fmt->code != sensor->fmt.code)
+ sensor->pending_fmt_change = true;
+
+ *fmt = *mbus_fmt;
+
+ if (new_mode != sensor->current_mode) {
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+ if (new_mode->max_fps < sensor->current_fr) {
+ sensor->current_fr = new_mode->max_fps;
+ sensor->frame_interval.numerator = 1;
+ sensor->frame_interval.denominator =
+ ov5640_framerates[sensor->current_fr];
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ ov5640_calc_pixel_rate(sensor));
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int ov5640_set_framefmt(struct ov5640_dev *sensor,
+ struct v4l2_mbus_framefmt *format)
+{
+ int ret = 0;
+ bool is_jpeg = false;
+ u8 fmt, mux;
+
+ switch (format->code) {
+ case MEDIA_BUS_FMT_UYVY8_2X8:
+ /* YUV422, UYVY */
+ fmt = 0x3f;
+ mux = OV5640_FMT_MUX_YUV422;
+ break;
+ case MEDIA_BUS_FMT_YUYV8_2X8:
+ /* YUV422, YUYV */
+ fmt = 0x30;
+ mux = OV5640_FMT_MUX_YUV422;
+ break;
+ case MEDIA_BUS_FMT_RGB565_2X8_LE:
+ /* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
+ fmt = 0x6F;
+ mux = OV5640_FMT_MUX_RGB;
+ break;
+ case MEDIA_BUS_FMT_RGB565_2X8_BE:
+ /* RGB565 {r[4:0],g[5:3]},{g[2:0],b[4:0]} */
+ fmt = 0x61;
+ mux = OV5640_FMT_MUX_RGB;
+ break;
+ case MEDIA_BUS_FMT_JPEG_1X8:
+ /* YUV422, YUYV */
+ fmt = 0x30;
+ mux = OV5640_FMT_MUX_YUV422;
+ is_jpeg = true;
+ break;
+ case MEDIA_BUS_FMT_SBGGR8_1X8:
+ /* Raw, BGBG... / GRGR... */
+ fmt = 0x00;
+ mux = OV5640_FMT_MUX_RAW_DPC;
+ break;
+ case MEDIA_BUS_FMT_SGBRG8_1X8:
+ /* Raw bayer, GBGB... / RGRG... */
+ fmt = 0x01;
+ mux = OV5640_FMT_MUX_RAW_DPC;
+ break;
+ case MEDIA_BUS_FMT_SGRBG8_1X8:
+ /* Raw bayer, GRGR... / BGBG... */
+ fmt = 0x02;
+ mux = OV5640_FMT_MUX_RAW_DPC;
+ break;
+ case MEDIA_BUS_FMT_SRGGB8_1X8:
+ /* Raw bayer, RGRG... / GBGB... */
+ fmt = 0x03;
+ mux = OV5640_FMT_MUX_RAW_DPC;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* FORMAT CONTROL00: YUV and RGB formatting */
+ ret = ov5640_write_reg(sensor, OV5640_REG_FORMAT_CONTROL00, fmt);
+ if (ret)
+ return ret;
+
+ /* FORMAT MUX CONTROL: ISP YUV or RGB */
+ ret = ov5640_write_reg(sensor, OV5640_REG_ISP_FORMAT_MUX_CTRL, mux);
+ if (ret)
+ return ret;
+
+ /*
+ * TIMING TC REG21:
+ * - [5]: JPEG enable
+ */
+ ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+ BIT(5), is_jpeg ? BIT(5) : 0);
+ if (ret)
+ return ret;
+
+ /*
+ * SYSTEM RESET02:
+ * - [4]: Reset JFIFO
+ * - [3]: Reset SFIFO
+ * - [2]: Reset JPEG
+ */
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_RESET02,
+ BIT(4) | BIT(3) | BIT(2),
+ is_jpeg ? 0 : (BIT(4) | BIT(3) | BIT(2)));
+ if (ret)
+ return ret;
+
+ /*
+ * CLOCK ENABLE02:
+ * - [5]: Enable JPEG 2x clock
+ * - [3]: Enable JPEG clock
+ */
+ return ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE02,
+ BIT(5) | BIT(3),
+ is_jpeg ? (BIT(5) | BIT(3)) : 0);
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
+{
+ int ret;
+
+ if (value) {
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+ BIT(0), BIT(0));
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
+ } else {
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
+ }
+
+ return ret;
+}
+
+static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
+{
+ int ret;
+
+ if (value) {
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+ BIT(2), BIT(2));
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
+ value & 0xff);
+ } else {
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
+ }
+
+ return ret;
+}
+
+static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
+{
+ int ret;
+
+ if (value) {
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
+ BIT(1), BIT(1));
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
+ value & 0xff);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
+ value & 0xff);
+ } else {
+ ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
+ }
+
+ return ret;
+}
+
+static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
+{
+ int ret;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
+ BIT(0), awb ? 0 : 1);
+ if (ret)
+ return ret;
+
+ if (!awb) {
+ u16 red = (u16)sensor->ctrls.red_balance->val;
+ u16 blue = (u16)sensor->ctrls.blue_balance->val;
+
+ ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
+ if (ret)
+ return ret;
+ ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
+ }
+
+ return ret;
+}
+
+static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor,
+ enum v4l2_exposure_auto_type auto_exposure)
+{
+ struct ov5640_ctrls *ctrls = &sensor->ctrls;
+ bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+ int ret = 0;
+
+ if (ctrls->auto_exp->is_new) {
+ ret = ov5640_set_autoexposure(sensor, auto_exp);
+ if (ret)
+ return ret;
+ }
+
+ if (!auto_exp && ctrls->exposure->is_new) {
+ u16 max_exp;
+
+ ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
+ &max_exp);
+ if (ret)
+ return ret;
+ ret = ov5640_get_vts(sensor);
+ if (ret < 0)
+ return ret;
+ max_exp += ret;
+ ret = 0;
+
+ if (ctrls->exposure->val < max_exp)
+ ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
+ }
+
+ return ret;
+}
+
+static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, bool auto_gain)
+{
+ struct ov5640_ctrls *ctrls = &sensor->ctrls;
+ int ret = 0;
+
+ if (ctrls->auto_gain->is_new) {
+ ret = ov5640_set_autogain(sensor, auto_gain);
+ if (ret)
+ return ret;
+ }
+
+ if (!auto_gain && ctrls->gain->is_new)
+ ret = ov5640_set_gain(sensor, ctrls->gain->val);
+
+ return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+ "Disabled",
+ "Color bars",
+ "Color bars w/ rolling bar",
+ "Color squares",
+ "Color squares w/ rolling bar",
+};
+
+#define OV5640_TEST_ENABLE BIT(7)
+#define OV5640_TEST_ROLLING BIT(6) /* rolling horizontal bar */
+#define OV5640_TEST_TRANSPARENT BIT(5)
+#define OV5640_TEST_SQUARE_BW BIT(4) /* black & white squares */
+#define OV5640_TEST_BAR_STANDARD (0 << 2)
+#define OV5640_TEST_BAR_VERT_CHANGE_1 (1 << 2)
+#define OV5640_TEST_BAR_HOR_CHANGE (2 << 2)
+#define OV5640_TEST_BAR_VERT_CHANGE_2 (3 << 2)
+#define OV5640_TEST_BAR (0 << 0)
+#define OV5640_TEST_RANDOM (1 << 0)
+#define OV5640_TEST_SQUARE (2 << 0)
+#define OV5640_TEST_BLACK (3 << 0)
+
+static const u8 test_pattern_val[] = {
+ 0,
+ OV5640_TEST_ENABLE | OV5640_TEST_BAR_VERT_CHANGE_1 |
+ OV5640_TEST_BAR,
+ OV5640_TEST_ENABLE | OV5640_TEST_ROLLING |
+ OV5640_TEST_BAR_VERT_CHANGE_1 | OV5640_TEST_BAR,
+ OV5640_TEST_ENABLE | OV5640_TEST_SQUARE,
+ OV5640_TEST_ENABLE | OV5640_TEST_ROLLING | OV5640_TEST_SQUARE,
+};
+
+static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
+{
+ return ov5640_write_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
+ test_pattern_val[value]);
+}
+
+static int ov5640_set_ctrl_light_freq(struct ov5640_dev *sensor, int value)
+{
+ int ret;
+
+ ret = ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL01, BIT(7),
+ (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) ?
+ 0 : BIT(7));
+ if (ret)
+ return ret;
+
+ return ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL00, BIT(2),
+ (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ) ?
+ BIT(2) : 0);
+}
+
+static int ov5640_set_ctrl_hflip(struct ov5640_dev *sensor, int value)
+{
+ /*
+ * If sensor is mounted upside down, mirror logic is inversed.
+ *
+ * Sensor is a BSI (Back Side Illuminated) one,
+ * so image captured is physically mirrored.
+ * This is why mirror logic is inversed in
+ * order to cancel this mirror effect.
+ */
+
+ /*
+ * TIMING TC REG21:
+ * - [2]: ISP mirror
+ * - [1]: Sensor mirror
+ */
+ return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
+ BIT(2) | BIT(1),
+ (!(value ^ sensor->upside_down)) ?
+ (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
+{
+ /* If sensor is mounted upside down, flip logic is inversed */
+
+ /*
+ * TIMING TC REG20:
+ * - [2]: ISP vflip
+ * - [1]: Sensor vflip
+ */
+ return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
+ BIT(2) | BIT(1),
+ (value ^ sensor->upside_down) ?
+ (BIT(2) | BIT(1)) : 0);
+}
+
+static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ int val;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ val = ov5640_get_gain(sensor);
+ if (val < 0)
+ return val;
+ sensor->ctrls.gain->val = val;
+ break;
+ case V4L2_CID_EXPOSURE_AUTO:
+ val = ov5640_get_exposure(sensor);
+ if (val < 0)
+ return val;
+ sensor->ctrls.exposure->val = val;
+ break;
+ }
+
+ return 0;
+}
+
+static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ int ret;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ /*
+ * If the device is not powered up by the host driver do
+ * not apply any controls to H/W at this time. Instead
+ * the controls will be restored right after power-up.
+ */
+ if (sensor->power_count == 0)
+ return 0;
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
+ break;
+ case V4L2_CID_EXPOSURE_AUTO:
+ ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
+ break;
+ case V4L2_CID_AUTO_WHITE_BALANCE:
+ ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
+ break;
+ case V4L2_CID_CONTRAST:
+ ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN:
+ ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
+ break;
+ case V4L2_CID_POWER_LINE_FREQUENCY:
+ ret = ov5640_set_ctrl_light_freq(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ ret = ov5640_set_ctrl_hflip(sensor, ctrl->val);
+ break;
+ case V4L2_CID_VFLIP:
+ ret = ov5640_set_ctrl_vflip(sensor, ctrl->val);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+ return ret;
+}
+
+static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
+ .g_volatile_ctrl = ov5640_g_volatile_ctrl,
+ .s_ctrl = ov5640_s_ctrl,
+};
+
+static int ov5640_init_controls(struct ov5640_dev *sensor)
+{
+ const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
+ struct ov5640_ctrls *ctrls = &sensor->ctrls;
+ struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+ int ret;
+
+ v4l2_ctrl_handler_init(hdl, 32);
+
+ /* we can use our own mutex for the ctrl lock */
+ hdl->lock = &sensor->lock;
+
+ /* Clock related controls */
+ ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+ 0, INT_MAX, 1,
+ ov5640_calc_pixel_rate(sensor));
+
+ /* Auto/manual white balance */
+ ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+ V4L2_CID_AUTO_WHITE_BALANCE,
+ 0, 1, 1, 1);
+ ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+ 0, 4095, 1, 0);
+ ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+ 0, 4095, 1, 0);
+ /* Auto/manual exposure */
+ ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_EXPOSURE_AUTO,
+ V4L2_EXPOSURE_MANUAL, 0,
+ V4L2_EXPOSURE_AUTO);
+ ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+ 0, 65535, 1, 0);
+ /* Auto/manual gain */
+ ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+ 0, 1, 1, 1);
+ ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+ 0, 1023, 1, 0);
+
+ ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+ 0, 255, 1, 64);
+ ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+ 0, 359, 1, 0);
+ ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+ 0, 255, 1, 0);
+ ctrls->test_pattern =
+ v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+ ARRAY_SIZE(test_pattern_menu) - 1,
+ 0, 0, test_pattern_menu);
+ ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+ 0, 1, 1, 0);
+ ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+ 0, 1, 1, 0);
+
+ ctrls->light_freq =
+ v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_POWER_LINE_FREQUENCY,
+ V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+ V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+ if (hdl->error) {
+ ret = hdl->error;
+ goto free_ctrls;
+ }
+
+ ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+ ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+ ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+ v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+ v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+ v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+ sensor->sd.ctrl_handler = hdl;
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(hdl);
+ return ret;
+}
+
+static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ if (fse->pad != 0)
+ return -EINVAL;
+ if (fse->index >= OV5640_NUM_MODES)
+ return -EINVAL;
+
+ fse->min_width =
+ ov5640_mode_data[fse->index].hact;
+ fse->max_width = fse->min_width;
+ fse->min_height =
+ ov5640_mode_data[fse->index].vact;
+ fse->max_height = fse->min_height;
+
+ return 0;
+}
+
+static int ov5640_enum_frame_interval(
+ struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_interval_enum *fie)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ struct v4l2_fract tpf;
+ int ret;
+
+ if (fie->pad != 0)
+ return -EINVAL;
+ if (fie->index >= OV5640_NUM_FRAMERATES)
+ return -EINVAL;
+
+ tpf.numerator = 1;
+ tpf.denominator = ov5640_framerates[fie->index];
+
+ ret = ov5640_try_frame_interval(sensor, &tpf,
+ fie->width, fie->height);
+ if (ret < 0)
+ return -EINVAL;
+
+ fie->interval = tpf;
+ return 0;
+}
+
+static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+ mutex_lock(&sensor->lock);
+ fi->interval = sensor->frame_interval;
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ const struct ov5640_mode_info *mode;
+ int frame_rate, ret = 0;
+
+ if (fi->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ mode = sensor->current_mode;
+
+ frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
+ mode->hact, mode->vact);
+ if (frame_rate < 0) {
+ /* Always return a valid frame interval value */
+ fi->interval = sensor->frame_interval;
+ goto out;
+ }
+
+ mode = ov5640_find_mode(sensor, frame_rate, mode->hact,
+ mode->vact, true);
+ if (!mode) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ if (mode != sensor->current_mode ||
+ frame_rate != sensor->current_fr) {
+ sensor->current_fr = frame_rate;
+ sensor->frame_interval = fi->interval;
+ sensor->current_mode = mode;
+ sensor->pending_mode_change = true;
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ ov5640_calc_pixel_rate(sensor));
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ if (code->pad != 0)
+ return -EINVAL;
+ if (code->index >= ARRAY_SIZE(ov5640_formats))
+ return -EINVAL;
+
+ code->code = ov5640_formats[code->index].code;
+ return 0;
+}
+
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming == !enable) {
+ if (enable && sensor->pending_mode_change) {
+ ret = ov5640_set_mode(sensor);
+ if (ret)
+ goto out;
+ }
+
+ if (enable && sensor->pending_fmt_change) {
+ ret = ov5640_set_framefmt(sensor, &sensor->fmt);
+ if (ret)
+ goto out;
+ sensor->pending_fmt_change = false;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov5640_set_stream_mipi(sensor, enable);
+ else
+ ret = ov5640_set_stream_dvp(sensor, enable);
+
+ if (!ret)
+ sensor->streaming = enable;
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+int ov5640_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+ *frames = OV5640_SKIP_FRAMES;
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops ov5640_core_ops = {
+ .s_power = ov5640_s_power,
+ .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 ov5640_video_ops = {
+ .g_frame_interval = ov5640_g_frame_interval,
+ .s_frame_interval = ov5640_s_frame_interval,
+ .s_stream = ov5640_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
+ .enum_mbus_code = ov5640_enum_mbus_code,
+ .get_fmt = ov5640_get_fmt,
+ .set_fmt = ov5640_set_fmt,
+ .enum_frame_size = ov5640_enum_frame_size,
+ .enum_frame_interval = ov5640_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_sensor_ops ov5640_sensor_ops = {
+ .g_skip_frames = ov5640_skip_frames,
+};
+
+static const struct v4l2_subdev_ops ov5640_subdev_ops = {
+ .core = &ov5640_core_ops,
+ .video = &ov5640_video_ops,
+ .pad = &ov5640_pad_ops,
+ .sensor = &ov5640_sensor_ops,
+};
+
+static int ov5640_get_regulators(struct ov5640_dev *sensor)
+{
+ int i;
+
+ for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
+ sensor->supplies[i].supply = ov5640_supply_name[i];
+
+ return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+ OV5640_NUM_SUPPLIES,
+ sensor->supplies);
+}
+
+static int ov5640_check_chip_id(struct ov5640_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret = 0;
+ u16 chip_id;
+
+ ret = ov5640_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+ ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to read chip identifier\n",
+ __func__);
+ goto power_off;
+ }
+
+ if (chip_id != OV5640_CHIP_ID) {
+ dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+ __func__, OV5640_CHIP_ID, chip_id);
+ ret = -ENXIO;
+ }
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+
+power_off:
+ ov5640_set_power_off(sensor);
+ return ret;
+}
+
+static int ov5640_probe(struct i2c_client *client)
+{
+ struct device *dev = &client->dev;
+ struct fwnode_handle *endpoint;
+ struct ov5640_dev *sensor;
+ struct v4l2_mbus_framefmt *fmt;
+ u32 rotation;
+ int ret;
+
+ sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+ if (!sensor)
+ return -ENOMEM;
+
+ sensor->i2c_client = client;
+
+ /*
+ * default init sequence initialize sensor to
+ * YUV422 UYVY VGA@30fps
+ */
+ fmt = &sensor->fmt;
+ fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+ 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 = 640;
+ fmt->height = 480;
+ fmt->field = V4L2_FIELD_NONE;
+ sensor->frame_interval.numerator = 1;
+ sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
+ sensor->current_fr = OV5640_30_FPS;
+ sensor->current_mode =
+ &ov5640_mode_data[OV5640_MODE_VGA_640_480];
+ sensor->last_mode = sensor->current_mode;
+
+ sensor->ae_target = 52;
+
+ /* optional indication of physical rotation of sensor */
+ ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
+ &rotation);
+ if (!ret) {
+ switch (rotation) {
+ case 180:
+ sensor->upside_down = true;
+ fallthrough;
+ case 0:
+ break;
+ default:
+ dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
+ rotation);
+ }
+ }
+
+ endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+ NULL);
+ if (!endpoint) {
+ dev_err(dev, "endpoint node not found\n");
+ return -EINVAL;
+ }
+
+ ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+ fwnode_handle_put(endpoint);
+ if (ret) {
+ dev_err(dev, "Could not parse endpoint\n");
+ return ret;
+ }
+
+ if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+ sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+ sensor->ep.bus_type != V4L2_MBUS_BT656) {
+ dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+ return -EINVAL;
+ }
+
+ /* get system clock (xclk) */
+ sensor->xclk = devm_clk_get(dev, "xclk");
+ if (IS_ERR(sensor->xclk)) {
+ dev_err(dev, "failed to get xclk\n");
+ return PTR_ERR(sensor->xclk);
+ }
+
+ sensor->xclk_freq = clk_get_rate(sensor->xclk);
+ if (sensor->xclk_freq < OV5640_XCLK_MIN ||
+ sensor->xclk_freq > OV5640_XCLK_MAX) {
+ dev_err(dev, "xclk frequency out of range: %d Hz\n",
+ sensor->xclk_freq);
+ return -EINVAL;
+ }
+
+ /* request optional power down pin */
+ sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->pwdn_gpio))
+ return PTR_ERR(sensor->pwdn_gpio);
+
+ /* request optional reset pin */
+ sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->reset_gpio))
+ return PTR_ERR(sensor->reset_gpio);
+
+ v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
+
+ sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+ V4L2_SUBDEV_FL_HAS_EVENTS;
+ sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+ sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+ ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+ if (ret)
+ return ret;
+
+ ret = ov5640_get_regulators(sensor);
+ if (ret)
+ return ret;
+
+ mutex_init(&sensor->lock);
+
+ ret = ov5640_check_chip_id(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = ov5640_init_controls(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = v4l2_async_register_subdev_sensor_common(&sensor->sd);
+ if (ret)
+ goto free_ctrls;
+
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+ media_entity_cleanup(&sensor->sd.entity);
+ mutex_destroy(&sensor->lock);
+ return ret;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct ov5640_dev *sensor = to_ov5640_dev(sd);
+
+ v4l2_async_unregister_subdev(&sensor->sd);
+ media_entity_cleanup(&sensor->sd.entity);
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+ mutex_destroy(&sensor->lock);
+
+ return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+ {"ov5640", 0},
+ {},
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static const struct of_device_id ov5640_dt_ids[] = {
+ { .compatible = "ovti,ov5640" },
+ { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
+
+static struct i2c_driver ov5640_i2c_driver = {
+ .driver = {
+ .name = "ov5640",
+ .of_match_table = ov5640_dt_ids,
+ },
+ .id_table = ov5640_id,
+ .probe_new = ov5640_probe,
+ .remove = ov5640_remove,
+};
+
+module_i2c_driver(ov5640_i2c_driver);
+
+MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-async.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 "stfcamss.h"
+
+/* min/typical/max system clock (xclk) frequencies */
+#define SC2235_XCLK_MIN 6000000
+#define SC2235_XCLK_MAX 54000000
+
+#define SC2235_CHIP_ID (0x2235)
+
+#define SC2235_REG_CHIP_ID 0x3107
+#define SC2235_REG_AEC_PK_MANUAL 0x3e03
+#define SC2235_REG_AEC_PK_EXPOSURE_HI 0x3e01
+#define SC2235_REG_AEC_PK_EXPOSURE_LO 0x3e02
+#define SC2235_REG_AEC_PK_REAL_GAIN 0x3e08
+#define SC2235_REG_TIMING_HTS 0x320c
+#define SC2235_REG_TIMING_VTS 0x320e
+#define SC2235_REG_TEST_SET0 0x4501
+#define SC2235_REG_TEST_SET1 0x3902
+#define SC2235_REG_TIMING_TC_REG21 0x3221
+#define SC2235_REG_SC_PLL_CTRL0 0x3039
+#define SC2235_REG_SC_PLL_CTRL1 0x303a
+
+enum sc2235_mode_id {
+ SC2235_MODE_1080P_1920_1080 = 0,
+ SC2235_NUM_MODES,
+};
+
+enum sc2235_frame_rate {
+ SC2235_15_FPS = 0,
+ SC2235_30_FPS,
+ SC2235_60_FPS,
+ SC2235_NUM_FRAMERATES,
+};
+
+struct sc2235_pixfmt {
+ u32 code;
+ u32 colorspace;
+};
+
+static const struct sc2235_pixfmt sc2235_formats[] = {
+ //{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB, },
+};
+
+static const int sc2235_framerates[] = {
+ [SC2235_15_FPS] = 15,
+ [SC2235_30_FPS] = 30,
+ [SC2235_60_FPS] = 60,
+};
+
+/* regulator supplies */
+static const char * const sc2235_supply_name[] = {
+ "DOVDD", /* Digital I/O (1.8V) supply */
+ "AVDD", /* Analog (2.8V) supply */
+ "DVDD", /* Digital Core (1.5V) supply */
+};
+
+#define SC2235_NUM_SUPPLIES ARRAY_SIZE(sc2235_supply_name)
+
+struct reg_value {
+ u16 reg_addr;
+ u8 val;
+ u8 mask;
+ u32 delay_ms;
+};
+
+struct sc2235_mode_info {
+ enum sc2235_mode_id id;
+ u32 hact;
+ u32 htot;
+ u32 vact;
+ u32 vtot;
+ const struct reg_value *reg_data;
+ u32 reg_data_size;
+ u32 max_fps;
+};
+
+struct sc2235_ctrls {
+ struct v4l2_ctrl_handler handler;
+ struct v4l2_ctrl *pixel_rate;
+ struct {
+ struct v4l2_ctrl *auto_exp;
+ struct v4l2_ctrl *exposure;
+ };
+ struct {
+ struct v4l2_ctrl *auto_wb;
+ struct v4l2_ctrl *blue_balance;
+ struct v4l2_ctrl *red_balance;
+ };
+ struct {
+ struct v4l2_ctrl *auto_gain;
+ struct v4l2_ctrl *gain;
+ };
+ struct v4l2_ctrl *brightness;
+ struct v4l2_ctrl *light_freq;
+ struct v4l2_ctrl *saturation;
+ struct v4l2_ctrl *contrast;
+ struct v4l2_ctrl *hue;
+ struct v4l2_ctrl *test_pattern;
+ struct v4l2_ctrl *hflip;
+ struct v4l2_ctrl *vflip;
+};
+
+struct sc2235_dev {
+ struct i2c_client *i2c_client;
+ struct v4l2_subdev sd;
+ struct media_pad pad;
+ struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+ struct clk *xclk; /* system clock to SC2235 */
+ u32 xclk_freq;
+
+ struct regulator_bulk_data supplies[SC2235_NUM_SUPPLIES];
+ struct gpio_desc *reset_gpio;
+ struct gpio_desc *pwdn_gpio;
+ bool upside_down;
+
+ /* lock to protect all members below */
+ struct mutex lock;
+
+ int power_count;
+
+ struct v4l2_mbus_framefmt fmt;
+ bool pending_fmt_change;
+
+ const struct sc2235_mode_info *current_mode;
+ const struct sc2235_mode_info *last_mode;
+ enum sc2235_frame_rate current_fr;
+ struct v4l2_fract frame_interval;
+
+ struct sc2235_ctrls ctrls;
+
+ u32 prev_sysclk, prev_hts;
+ u32 ae_low, ae_high, ae_target;
+
+ bool pending_mode_change;
+ bool streaming;
+};
+
+static inline struct sc2235_dev *to_sc2235_dev(struct v4l2_subdev *sd)
+{
+ return container_of(sd, struct sc2235_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+ return &container_of(ctrl->handler, struct sc2235_dev,
+ ctrls.handler)->sd;
+}
+
+/* sc2235 initial register */
+static struct reg_value sc2235_init_tbl_1080p_7fps[] = {
+ {0x0103,0x01,0,0},
+ {0x0100,0x00,0,0},
+ {0x3039,0x75,0,0},
+ {0x320c,0x08,0,0},
+ {0x320d,0x98,0,0},
+ {0x3222,0x29,0,0},
+ {0x3235,0x04,0,0},
+ {0x3236,0x63,0,0},
+ {0x3237,0x08,0,0},
+ {0x3238,0x68,0,0},
+ {0x3301,0x04,0,0},
+ {0x3303,0x20,0,0},
+ {0x3306,0x1a,0,0},
+ {0x3309,0xa0,0,0},
+ {0x330b,0x54,0,0},
+ {0x330e,0x30,0,0},
+ {0x3313,0x05,0,0},
+ {0x331e,0x0d,0,0},
+ {0x331f,0x8d,0,0},
+ {0x3320,0x0f,0,0},
+ {0x3321,0x8f,0,0},
+ {0x3340,0x06,0,0},
+ {0x3341,0x50,0,0},
+ {0x3342,0x04,0,0},
+ {0x3343,0x20,0,0},
+ {0x3348,0x07,0,0},
+ {0x3349,0x80,0,0},
+ {0x334a,0x04,0,0},
+ {0x334b,0x20,0,0},
+ {0x335e,0x01,0,0},
+ {0x335f,0x03,0,0},
+ {0x3364,0x05,0,0},
+ {0x3366,0x7c,0,0},
+ {0x3367,0x08,0,0},
+ {0x3368,0x02,0,0},
+ {0x3369,0x00,0,0},
+ {0x336a,0x00,0,0},
+ {0x336b,0x00,0,0},
+ {0x337c,0x04,0,0},
+ {0x337d,0x06,0,0},
+ {0x337f,0x03,0,0},
+ {0x3380,0x04,0,0},
+ {0x3381,0x0a,0,0},
+ {0x33a0,0x05,0,0},
+ {0x33b5,0x10,0,0},
+ {0x3621,0x28,0,0},
+ {0x3622,0x06,0,0},
+ {0x3625,0x02,0,0},
+ {0x3630,0x48,0,0},
+ {0x3631,0x84,0,0},
+ {0x3632,0x88,0,0},
+ {0x3633,0x42,0,0},
+ {0x3634,0x42,0,0},
+ {0x3636,0x24,0,0},
+ {0x3635,0xc1,0,0},
+ {0x3637,0x14,0,0},
+ {0x3638,0x1f,0,0},
+ {0x363b,0x09,0,0},
+ {0x3639,0x09,0,0},
+ {0x363c,0x07,0,0},
+ {0x366e,0x08,0,0},
+ {0x3670,0x08,0,0},
+ {0x366f,0x2f,0,0},
+ {0x3677,0x1f,0,0},
+ {0x3678,0x42,0,0},
+ {0x3679,0x43,0,0},
+ {0x367e,0x07,0,0},
+ {0x367f,0x0f,0,0},
+ {0x3802,0x01,0,0},
+ {0x3901,0x02,0,0},
+ {0x3908,0x11,0,0},
+ {0x391b,0x4d,0,0},
+ {0x391e,0x00,0,0},
+ {0x3d08,0x02,0,0},
+ {0x3e01,0x46,0,0},
+ {0x3e03,0x0b,0,0},
+ {0x3f00,0x07,0,0},
+ {0x3f04,0x08,0,0},
+ {0x3f05,0x74,0,0},
+ {0x4500,0x59,0,0},
+ {0x5780,0xff,0,0},
+ {0x5781,0x04,0,0},
+ {0x5785,0x18,0,0},
+ {0x0100,0x01,0,0},
+ {0x330b,0x5d,0,0},
+ {0x3301,0x0a,0,0},
+ {0x3631,0x88,0,0},
+ {0x366f,0x2f,0,0},
+
+ {0x3d08,0x02,0,0},//hs-vs polity
+};
+
+static struct reg_value sc2235_setting_1080P_1920_1080[] = {
+
+};
+
+/* power-on sensor init reg table */
+static const struct sc2235_mode_info sc2235_mode_init_data = {
+ SC2235_MODE_1080P_1920_1080,
+ 1920, 0x8ca, 1080, 0x4b0,
+
+ sc2235_init_tbl_1080p_7fps,
+ ARRAY_SIZE(sc2235_init_tbl_1080p_7fps),
+ SC2235_60_FPS,
+};
+
+static const struct sc2235_mode_info
+sc2235_mode_data[SC2235_NUM_MODES] = {
+ {SC2235_MODE_1080P_1920_1080,
+ 1920, 0x8ca, 1080, 0x4b0,
+ sc2235_setting_1080P_1920_1080,
+ ARRAY_SIZE(sc2235_setting_1080P_1920_1080),
+ SC2235_60_FPS},
+};
+
+static int sc2235_write_reg(struct sc2235_dev *sensor, u16 reg, u8 val)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg;
+ u8 buf[3];
+ int ret;
+
+ buf[0] = reg >> 8;
+ buf[1] = reg & 0xff;
+ buf[2] = val;
+
+ msg.addr = client->addr;
+ msg.flags = client->flags;
+ msg.buf = buf;
+ msg.len = sizeof(buf);
+
+ ret = i2c_transfer(client->adapter, &msg, 1);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
+ __func__, reg, val);
+ return ret;
+ }
+
+ return 0;
+}
+
+static int sc2235_read_reg(struct sc2235_dev *sensor, u16 reg, u8 *val)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ struct i2c_msg msg[2];
+ u8 buf[2];
+ int ret;
+
+ buf[0] = reg >> 8;
+ buf[1] = reg & 0xff;
+
+ msg[0].addr = client->addr;
+ msg[0].flags = client->flags;
+ msg[0].buf = buf;
+ msg[0].len = sizeof(buf);
+
+ msg[1].addr = client->addr;
+ msg[1].flags = client->flags | I2C_M_RD;
+ msg[1].buf = buf;
+ msg[1].len = 1;
+
+ ret = i2c_transfer(client->adapter, msg, 2);
+ if (ret < 0) {
+ dev_err(&client->dev, "%s: error: reg=%x\n",
+ __func__, reg);
+ return ret;
+ }
+
+ *val = buf[0];
+ return 0;
+}
+
+static int sc2235_read_reg16(struct sc2235_dev *sensor, u16 reg, u16 *val)
+{
+ u8 hi, lo;
+ int ret;
+
+ ret = sc2235_read_reg(sensor, reg, &hi);
+ if (ret)
+ return ret;
+ ret = sc2235_read_reg(sensor, reg + 1, &lo);
+ if (ret)
+ return ret;
+
+ *val = ((u16)hi << 8) | (u16)lo;
+ return 0;
+}
+
+static int sc2235_write_reg16(struct sc2235_dev *sensor, u16 reg, u16 val)
+{
+ int ret;
+
+ ret = sc2235_write_reg(sensor, reg, val >> 8);
+ if (ret)
+ return ret;
+
+ return sc2235_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int sc2235_mod_reg(struct sc2235_dev *sensor, u16 reg,
+ u8 mask, u8 val)
+{
+ u8 readval;
+ int ret;
+
+ ret = sc2235_read_reg(sensor, reg, &readval);
+ if (ret)
+ return ret;
+
+ readval &= ~mask;
+ val &= mask;
+ val |= readval;
+
+ return sc2235_write_reg(sensor, reg, val);
+}
+
+#define SC2235_PLL_PREDIV 3
+
+#define SC2235_SYSDIV_MIN 0
+#define SC2235_SYSDIV_MAX 7
+
+#define SC2235_PLL_MULT_MIN 0
+#define SC2235_PLL_MULT_MAX 63
+
+static unsigned long sc2235_compute_sys_clk(struct sc2235_dev *sensor,
+ u8 pll_pre, u8 pll_mult,
+ u8 sysdiv)
+{
+ unsigned long sysclk =
+ sensor->xclk_freq * (64 - pll_mult) / (pll_pre * (sysdiv + 1));
+
+ /* PLL1 output cannot exceed 1GHz. */
+ if (sysclk / 1000000 > 1000)
+ return 0;
+
+ return sysclk;
+}
+
+static unsigned long sc2235_calc_sys_clk(struct sc2235_dev *sensor,
+ unsigned long rate,
+ u8 *pll_prediv, u8 *pll_mult,
+ u8 *sysdiv)
+{
+ unsigned long best = ~0;
+ u8 best_sysdiv = 1, best_mult = 1;
+ u8 _sysdiv, _pll_mult;
+
+ for (_sysdiv = SC2235_SYSDIV_MIN;
+ _sysdiv <= SC2235_SYSDIV_MAX;
+ _sysdiv++) {
+ for (_pll_mult = SC2235_PLL_MULT_MIN;
+ _pll_mult <= SC2235_PLL_MULT_MAX;
+ _pll_mult++) {
+ unsigned long _rate;
+
+ _rate = sc2235_compute_sys_clk(sensor,
+ SC2235_PLL_PREDIV,
+ _pll_mult, _sysdiv);
+
+ /*
+ * We have reached the maximum allowed PLL1 output,
+ * increase sysdiv.
+ */
+ if (!_rate)
+ break;
+
+ /*
+ * Prefer rates above the expected clock rate than
+ * below, even if that means being less precise.
+ */
+ if (_rate < rate)
+ continue;
+
+ if (abs(rate - _rate) < abs(rate - best)) {
+ best = _rate;
+ best_sysdiv = _sysdiv;
+ best_mult = _pll_mult;
+ }
+
+ if (_rate == rate)
+ goto out;
+ }
+ }
+
+out:
+ *sysdiv = best_sysdiv;
+ *pll_prediv = SC2235_PLL_PREDIV;
+ *pll_mult = best_mult;
+
+ return best;
+}
+
+static int sc2235_set_timings(struct sc2235_dev *sensor,
+ const struct sc2235_mode_info *mode)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int sc2235_load_regs(struct sc2235_dev *sensor,
+ const struct sc2235_mode_info *mode)
+{
+ const struct reg_value *regs = mode->reg_data;
+ unsigned int i;
+ u32 delay_ms;
+ u16 reg_addr;
+ u8 mask, val;
+ int ret = 0;
+
+ st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
+ for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
+ delay_ms = regs->delay_ms;
+ reg_addr = regs->reg_addr;
+ val = regs->val;
+ mask = regs->mask;
+
+ if (mask)
+ ret = sc2235_mod_reg(sensor, reg_addr, mask, val);
+ else
+ ret = sc2235_write_reg(sensor, reg_addr, val);
+ if (ret)
+ break;
+
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+
+ return sc2235_set_timings(sensor, mode);
+}
+
+static int sc2235_set_autoexposure(struct sc2235_dev *sensor, bool on)
+{
+ return sc2235_mod_reg(sensor, SC2235_REG_AEC_PK_MANUAL,
+ BIT(0), on ? 0 : BIT(0));
+}
+
+static int sc2235_get_exposure(struct sc2235_dev *sensor)
+{
+ int exp = 0, ret = 0;
+ u8 temp;
+
+ ret = sc2235_read_reg(sensor, SC2235_REG_AEC_PK_EXPOSURE_HI, &temp);
+ if (ret)
+ return ret;
+ exp |= (int)temp << 8;
+ ret = sc2235_read_reg(sensor, SC2235_REG_AEC_PK_EXPOSURE_LO, &temp);
+ if (ret)
+ return ret;
+ exp |= (int)temp;
+
+ return exp >> 4;
+}
+
+static int sc2235_set_exposure(struct sc2235_dev *sensor, u32 exposure)
+{
+ int ret;
+
+ exposure <<= 4;
+
+ ret = sc2235_write_reg(sensor,
+ SC2235_REG_AEC_PK_EXPOSURE_LO,
+ exposure & 0xff);
+ if (ret)
+ return ret;
+ return sc2235_write_reg(sensor,
+ SC2235_REG_AEC_PK_EXPOSURE_HI,
+ (exposure >> 8) & 0xff);
+}
+
+static int sc2235_get_gain(struct sc2235_dev *sensor)
+{
+ u16 gain;
+ int ret;
+
+ ret = sc2235_read_reg16(sensor, SC2235_REG_AEC_PK_REAL_GAIN, &gain);
+ if (ret)
+ return ret;
+
+ return gain & 0x1fff;
+}
+
+static int sc2235_set_gain(struct sc2235_dev *sensor, int gain)
+{
+ return sc2235_write_reg16(sensor, SC2235_REG_AEC_PK_REAL_GAIN,
+ (u16)gain & 0x1fff);
+}
+
+static int sc2235_set_autogain(struct sc2235_dev *sensor, bool on)
+{
+ return sc2235_mod_reg(sensor, SC2235_REG_AEC_PK_MANUAL,
+ BIT(1), on ? 0 : BIT(1));
+}
+
+static int sc2235_set_stream_dvp(struct sc2235_dev *sensor, bool on)
+{
+ return 0;
+}
+
+static int sc2235_get_sysclk(struct sc2235_dev *sensor)
+{
+ return 0;
+}
+
+static int sc2235_set_night_mode(struct sc2235_dev *sensor)
+{
+ return 0;
+}
+
+static int sc2235_get_hts(struct sc2235_dev *sensor)
+{
+ u16 hts;
+ int ret;
+
+ ret = sc2235_read_reg16(sensor, SC2235_REG_TIMING_HTS, &hts);
+ if (ret)
+ return ret;
+ return hts;
+}
+
+static int sc2235_get_vts(struct sc2235_dev *sensor)
+{
+ u16 vts;
+ int ret;
+
+ ret = sc2235_read_reg16(sensor, SC2235_REG_TIMING_VTS, &vts);
+ if (ret)
+ return ret;
+ return vts;
+}
+
+static int sc2235_set_vts(struct sc2235_dev *sensor, int vts)
+{
+ return sc2235_write_reg16(sensor, SC2235_REG_TIMING_VTS, vts);
+}
+
+static int sc2235_get_light_freq(struct sc2235_dev *sensor)
+{
+ return 0;
+}
+
+static int sc2235_set_bandingfilter(struct sc2235_dev *sensor)
+{
+ return 0;
+}
+
+static int sc2235_set_ae_target(struct sc2235_dev *sensor, int target)
+{
+ return 0;
+}
+
+static int sc2235_get_binning(struct sc2235_dev *sensor)
+{
+ return 0;
+}
+
+static int sc2235_set_binning(struct sc2235_dev *sensor, bool enable)
+{
+ return 0;
+}
+
+static const struct sc2235_mode_info *
+sc2235_find_mode(struct sc2235_dev *sensor, enum sc2235_frame_rate fr,
+ int width, int height, bool nearest)
+{
+ const struct sc2235_mode_info *mode;
+
+ mode = v4l2_find_nearest_size(sc2235_mode_data,
+ ARRAY_SIZE(sc2235_mode_data),
+ hact, vact,
+ width, height);
+
+ if (!mode ||
+ (!nearest && (mode->hact != width || mode->vact != height)))
+ return NULL;
+
+ /* Check to see if the current mode exceeds the max frame rate */
+ if (sc2235_framerates[fr] > sc2235_framerates[mode->max_fps])
+ return NULL;
+
+ return mode;
+}
+
+static u64 sc2235_calc_pixel_rate(struct sc2235_dev *sensor)
+{
+ u64 rate;
+
+ rate = sensor->current_mode->vtot * sensor->current_mode->htot;
+ rate *= sc2235_framerates[sensor->current_fr];
+
+ return rate;
+}
+
+/*
+ * sc2235_set_dvp_pclk() - Calculate the clock tree configuration values
+ * for the dvp output.
+ *
+ * @rate: The requested bandwidth per lane in bytes per second.
+ * 'Bandwidth Per Lane' is calculated as:
+ * rate = HTOT * VTOT * FPS;
+ *
+ * This function use the requested bandwidth to calculate:
+ * - rate = xclk * (64 - M) / (N * (S + 1));
+ *
+ */
+
+#define PLL_PREDIV 1
+#define PLL_SYSEL 0
+
+static int sc2235_set_dvp_pclk(struct sc2235_dev *sensor,
+ unsigned long rate)
+{
+ const struct sc2235_mode_info *mode = sensor->current_mode;
+ const struct sc2235_mode_info *orig_mode = sensor->last_mode;
+ u8 prediv, mult, sysdiv;
+ int ret = 0;
+
+ sc2235_calc_sys_clk(sensor, rate, &prediv, &mult,
+ &sysdiv);
+
+ st_info(ST_SENSOR, "%s, prediv = %d, mult = %d, sysdiv = %d\n",
+ __func__, prediv, mult, sysdiv);
+
+ ret = sc2235_mod_reg(sensor, SC2235_REG_SC_PLL_CTRL0, 0x7f,
+ (sysdiv << 4) | (prediv << 1) | ((mult & 0x20) >> 5));
+ if (ret)
+ return ret;
+
+ return sc2235_mod_reg(sensor, SC2235_REG_SC_PLL_CTRL1,
+ 0xf8, mult << 3);
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int sc2235_set_mode_direct(struct sc2235_dev *sensor,
+ const struct sc2235_mode_info *mode)
+{
+ if (!mode->reg_data)
+ return -EINVAL;
+
+ /* Write capture setting */
+ return sc2235_load_regs(sensor, mode);
+}
+
+static int sc2235_set_mode(struct sc2235_dev *sensor)
+{
+ const struct sc2235_mode_info *mode = sensor->current_mode;
+ const struct sc2235_mode_info *orig_mode = sensor->last_mode;
+ bool auto_gain = sensor->ctrls.auto_gain->val == 1;
+ bool auto_exp = sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
+ unsigned long rate;
+ int ret = 0;
+
+ /* auto gain and exposure must be turned off when changing modes */
+ if (auto_gain) {
+ ret = sc2235_set_autogain(sensor, false);
+ if (ret)
+ return ret;
+ }
+#if 0
+ /* This issue will be addressed in the EVB board*/
+ /* This action will result in poor image display 2021 1111*/
+ if (auto_exp) {
+ ret = sc2235_set_autoexposure(sensor, false);
+ if (ret)
+ goto restore_auto_gain;
+ }
+
+ rate = sc2235_calc_pixel_rate(sensor);
+ if (sensor->ep.bus_type == V4L2_MBUS_PARALLEL)
+ ret = sc2235_set_dvp_pclk(sensor, rate);
+
+
+ if (ret < 0)
+ return 0;
+
+ ret = sc2235_set_mode_direct(sensor, mode);
+ if (ret < 0)
+ goto restore_auto_exp_gain;
+
+
+ /* restore auto gain and exposure */
+ if (auto_gain)
+ sc2235_set_autogain(sensor, true);
+ if (auto_exp)
+ sc2235_set_autoexposure(sensor, true);
+
+
+ sensor->pending_mode_change = false;
+ sensor->last_mode = mode;
+ return 0;
+
+restore_auto_exp_gain:
+ if (auto_exp)
+ sc2235_set_autoexposure(sensor, true);
+restore_auto_gain:
+ if (auto_gain)
+ sc2235_set_autogain(sensor, true);
+#endif
+ return ret;
+}
+
+static int sc2235_set_framefmt(struct sc2235_dev *sensor,
+ struct v4l2_mbus_framefmt *format);
+
+/* restore the last set video mode after chip power-on */
+static int sc2235_restore_mode(struct sc2235_dev *sensor)
+{
+ int ret;
+ unsigned int hs_polity = 1; /* 1: valid when high; 0: valid when low */
+ unsigned int vs_polity = 1; /* 1: valid when high; 0: valid when low */
+ unsigned char val;
+
+ /* first load the initial register values */
+ ret = sc2235_load_regs(sensor, &sc2235_mode_init_data);
+ if (ret < 0)
+ return ret;
+ sensor->last_mode = &sc2235_mode_init_data;
+ val = (vs_polity<<1)|((!hs_polity)<<2);
+ sc2235_write_reg16(sensor, 0x3d08, val);
+
+ /* now restore the last capture mode */
+ ret = sc2235_set_mode(sensor);
+ if (ret < 0)
+ return ret;
+
+ return sc2235_set_framefmt(sensor, &sensor->fmt);
+}
+
+static void sc2235_power(struct sc2235_dev *sensor, bool enable)
+{
+ if (!sensor->pwdn_gpio)
+ return;
+ gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
+}
+
+static void sc2235_reset(struct sc2235_dev *sensor)
+{
+ if (!sensor->reset_gpio)
+ return;
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+
+ /* camera power cycle */
+ sc2235_power(sensor, false);
+ usleep_range(5000, 10000);
+ sc2235_power(sensor, true);
+ usleep_range(5000, 10000);
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+ usleep_range(1000, 2000);
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+ usleep_range(20000, 25000);
+}
+
+static int sc2235_set_power_on(struct sc2235_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret;
+
+ ret = clk_prepare_enable(sensor->xclk);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable clock\n",
+ __func__);
+ return ret;
+ }
+
+ ret = regulator_bulk_enable(SC2235_NUM_SUPPLIES,
+ sensor->supplies);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable regulators\n",
+ __func__);
+ goto xclk_off;
+ }
+
+ sc2235_reset(sensor);
+ sc2235_power(sensor, true);
+
+ return 0;
+
+xclk_off:
+ clk_disable_unprepare(sensor->xclk);
+ return ret;
+}
+
+static void sc2235_set_power_off(struct sc2235_dev *sensor)
+{
+ sc2235_power(sensor, false);
+ regulator_bulk_disable(SC2235_NUM_SUPPLIES, sensor->supplies);
+ clk_disable_unprepare(sensor->xclk);
+}
+
+static int sc2235_set_power_dvp(struct sc2235_dev *sensor, bool on)
+{
+ unsigned int flags = sensor->ep.bus.parallel.flags;
+ bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
+ u8 polarities = 0;
+ int ret;
+
+ /*
+ * configure parallel port control lines polarity
+ *
+ * POLARITY CTRL0
+ * - [5]: PCLK polarity (0: active low, 1: active high)
+ * - [1]: HREF polarity (0: active low, 1: active high)
+ * - [0]: VSYNC polarity (mismatch here between
+ * datasheet and hardware, 0 is active high
+ * and 1 is active low...)
+ */
+ if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+ polarities |= BIT(1);
+ if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+ polarities |= BIT(0);
+ if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+ polarities |= BIT(5);
+
+ // ret = sc2235_write_reg(sensor,
+ // SC2235_REG_POLARITY_CTRL00,
+ // polarities);
+ // if (ret)
+ // return ret;
+
+ return 0;
+}
+
+static int sc2235_set_power(struct sc2235_dev *sensor, bool on)
+{
+ int ret = 0;
+
+ if (on) {
+ ret = sc2235_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+ ret = sc2235_restore_mode(sensor);
+ if (ret)
+ goto power_off;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_PARALLEL)
+ ret = sc2235_set_power_dvp(sensor, on);
+ if (ret)
+ goto power_off;
+
+ if (!on)
+ sc2235_set_power_off(sensor);
+
+ return 0;
+
+power_off:
+ sc2235_set_power_off(sensor);
+
+ return ret;
+}
+
+static int sc2235_s_power(struct v4l2_subdev *sd, int on)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ /*
+ * If the power count is modified from 0 to != 0 or from != 0 to 0,
+ * update the power state.
+ */
+ if (sensor->power_count == !on) {
+ ret = sc2235_set_power(sensor, !!on);
+ if (ret)
+ goto out;
+ }
+
+ /* Update the power count. */
+ sensor->power_count += on ? 1 : -1;
+ WARN_ON(sensor->power_count < 0);
+out:
+ mutex_unlock(&sensor->lock);
+
+ if (on && !ret && sensor->power_count == 1) {
+ /* restore controls */
+ ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+ }
+
+ return ret;
+}
+
+static int sc2235_try_frame_interval(struct sc2235_dev *sensor,
+ struct v4l2_fract *fi,
+ u32 width, u32 height)
+{
+ const struct sc2235_mode_info *mode;
+ enum sc2235_frame_rate rate = SC2235_15_FPS;
+ int minfps, maxfps, best_fps, fps;
+ int i;
+
+ minfps = sc2235_framerates[SC2235_15_FPS];
+ maxfps = sc2235_framerates[SC2235_60_FPS];
+
+ if (fi->numerator == 0) {
+ fi->denominator = maxfps;
+ fi->numerator = 1;
+ rate = SC2235_60_FPS;
+ goto find_mode;
+ }
+
+ fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
+ minfps, maxfps);
+
+ best_fps = minfps;
+ for (i = 0; i < ARRAY_SIZE(sc2235_framerates); i++) {
+ int curr_fps = sc2235_framerates[i];
+
+ if (abs(curr_fps - fps) < abs(best_fps - fps)) {
+ best_fps = curr_fps;
+ rate = i;
+ }
+ }
+
+ fi->numerator = 1;
+ fi->denominator = best_fps;
+
+find_mode:
+ mode = sc2235_find_mode(sensor, rate, width, height, false);
+ return mode ? rate : -EINVAL;
+}
+
+static int sc2235_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ struct v4l2_mbus_framefmt *fmt;
+
+ if (format->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+ fmt = v4l2_subdev_get_try_format(&sensor->sd, state,
+ format->pad);
+ else
+ fmt = &sensor->fmt;
+
+ format->format = *fmt;
+
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int sc2235_try_fmt_internal(struct v4l2_subdev *sd,
+ struct v4l2_mbus_framefmt *fmt,
+ enum sc2235_frame_rate fr,
+ const struct sc2235_mode_info **new_mode)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ const struct sc2235_mode_info *mode;
+ int i;
+
+ mode = sc2235_find_mode(sensor, fr, fmt->width, fmt->height, true);
+ if (!mode)
+ return -EINVAL;
+ fmt->width = mode->hact;
+ fmt->height = mode->vact;
+
+ if (new_mode)
+ *new_mode = mode;
+
+ for (i = 0; i < ARRAY_SIZE(sc2235_formats); i++)
+ if (sc2235_formats[i].code == fmt->code)
+ break;
+ if (i >= ARRAY_SIZE(sc2235_formats))
+ i = 0;
+
+ fmt->code = sc2235_formats[i].code;
+ fmt->colorspace = sc2235_formats[i].colorspace;
+ 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);
+
+ return 0;
+}
+
+static int sc2235_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ const struct sc2235_mode_info *new_mode;
+ struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
+ struct v4l2_mbus_framefmt *fmt;
+ int ret;
+
+ if (format->pad != 0)
+ return -EINVAL;
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ ret = sc2235_try_fmt_internal(sd, mbus_fmt, 0, &new_mode);
+ if (ret)
+ goto out;
+
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+ fmt = v4l2_subdev_get_try_format(sd, state, 0);
+ else
+ fmt = &sensor->fmt;
+
+ if (mbus_fmt->code != sensor->fmt.code)
+ sensor->pending_fmt_change = true;
+
+ *fmt = *mbus_fmt;
+
+ if (new_mode != sensor->current_mode) {
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+ if (new_mode->max_fps < sensor->current_fr) {
+ sensor->current_fr = new_mode->max_fps;
+ sensor->frame_interval.numerator = 1;
+ sensor->frame_interval.denominator =
+ sc2235_framerates[sensor->current_fr];
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ sc2235_calc_pixel_rate(sensor));
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int sc2235_set_framefmt(struct sc2235_dev *sensor,
+ struct v4l2_mbus_framefmt *format)
+{
+ int ret = 0;
+
+ switch (format->code) {
+ default:
+ return ret;
+ }
+ return ret;
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int sc2235_set_ctrl_hue(struct sc2235_dev *sensor, int value)
+{
+ int ret = 0;
+ return ret;
+}
+
+static int sc2235_set_ctrl_contrast(struct sc2235_dev *sensor, int value)
+{
+ int ret = 0;
+ return ret;
+}
+
+static int sc2235_set_ctrl_saturation(struct sc2235_dev *sensor, int value)
+{
+ int ret = 0;
+ return ret;
+}
+
+static int sc2235_set_ctrl_white_balance(struct sc2235_dev *sensor, int awb)
+{
+ int ret = 0;
+ return ret;
+}
+
+static int sc2235_set_ctrl_exposure(struct sc2235_dev *sensor,
+ enum v4l2_exposure_auto_type auto_exposure)
+{
+ struct sc2235_ctrls *ctrls = &sensor->ctrls;
+ bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
+ int ret = 0;
+
+ if (ctrls->auto_exp->is_new) {
+ ret = sc2235_set_autoexposure(sensor, auto_exp);
+ if (ret)
+ return ret;
+ }
+
+ if (!auto_exp && ctrls->exposure->is_new) {
+ u16 max_exp = 0;
+
+ ret = sc2235_get_vts(sensor);
+ if (ret < 0)
+ return ret;
+ max_exp += ret - 4;
+ ret = 0;
+
+ if (ctrls->exposure->val < max_exp)
+ ret = sc2235_set_exposure(sensor, ctrls->exposure->val);
+ }
+
+ return ret;
+}
+
+static int sc2235_set_ctrl_gain(struct sc2235_dev *sensor, bool auto_gain)
+{
+ struct sc2235_ctrls *ctrls = &sensor->ctrls;
+ int ret = 0;
+
+ if (ctrls->auto_gain->is_new) {
+ ret = sc2235_set_autogain(sensor, auto_gain);
+ if (ret)
+ return ret;
+ }
+
+ if (!auto_gain && ctrls->gain->is_new)
+ ret = sc2235_set_gain(sensor, ctrls->gain->val);
+
+ return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+ "Disabled",
+ "Black bars",
+ "Auto Black bars",
+};
+
+#define SC2235_TEST_ENABLE BIT(3)
+#define SC2235_TEST_BLACK (3 << 0)
+
+static int sc2235_set_ctrl_test_pattern(struct sc2235_dev *sensor, int value)
+{
+ int ret = 0;
+ /*
+ *For 7110 platform, refer to 1125 FW code configuration. This operation will cause the image to be white.
+ */
+#if 0
+ ret = sc2235_mod_reg(sensor, SC2235_REG_TEST_SET0, BIT(3),
+ !!value << 3);
+
+ ret |= sc2235_mod_reg(sensor, SC2235_REG_TEST_SET1, BIT(6),
+ (value >> 1) << 6);
+#endif
+ return ret;
+}
+
+static int sc2235_set_ctrl_light_freq(struct sc2235_dev *sensor, int value)
+{
+ return 0;
+}
+
+static int sc2235_set_ctrl_hflip(struct sc2235_dev *sensor, int value)
+{
+ return sc2235_mod_reg(sensor, SC2235_REG_TIMING_TC_REG21,
+ BIT(2) | BIT(1),
+ (!(value ^ sensor->upside_down)) ?
+ (BIT(2) | BIT(1)) : 0);
+}
+
+static int sc2235_set_ctrl_vflip(struct sc2235_dev *sensor, int value)
+{
+ return sc2235_mod_reg(sensor, SC2235_REG_TIMING_TC_REG21,
+ BIT(6) | BIT(5),
+ (value ^ sensor->upside_down) ?
+ (BIT(6) | BIT(5)) : 0);
+}
+
+static int sc2235_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ int val;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ val = sc2235_get_gain(sensor);
+ if (val < 0)
+ return val;
+ sensor->ctrls.gain->val = val;
+ break;
+ case V4L2_CID_EXPOSURE_AUTO:
+ val = sc2235_get_exposure(sensor);
+ if (val < 0)
+ return val;
+ sensor->ctrls.exposure->val = val;
+ break;
+ }
+
+ return 0;
+}
+
+static int sc2235_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ int ret;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ /*
+ * If the device is not powered up by the host driver do
+ * not apply any controls to H/W at this time. Instead
+ * the controls will be restored right after power-up.
+ */
+ if (sensor->power_count == 0)
+ return 0;
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ ret = sc2235_set_ctrl_gain(sensor, ctrl->val);
+ break;
+ case V4L2_CID_EXPOSURE_AUTO:
+ ret = sc2235_set_ctrl_exposure(sensor, ctrl->val);
+ break;
+ case V4L2_CID_AUTO_WHITE_BALANCE:
+ ret = sc2235_set_ctrl_white_balance(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ ret = sc2235_set_ctrl_hue(sensor, ctrl->val);
+ break;
+ case V4L2_CID_CONTRAST:
+ ret = sc2235_set_ctrl_contrast(sensor, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ ret = sc2235_set_ctrl_saturation(sensor, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN:
+ ret = sc2235_set_ctrl_test_pattern(sensor, ctrl->val);
+ break;
+ case V4L2_CID_POWER_LINE_FREQUENCY:
+ ret = sc2235_set_ctrl_light_freq(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ ret = sc2235_set_ctrl_hflip(sensor, ctrl->val);
+ break;
+ case V4L2_CID_VFLIP:
+ ret = sc2235_set_ctrl_vflip(sensor, ctrl->val);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+ return ret;
+}
+
+static const struct v4l2_ctrl_ops sc2235_ctrl_ops = {
+ .g_volatile_ctrl = sc2235_g_volatile_ctrl,
+ .s_ctrl = sc2235_s_ctrl,
+};
+
+static int sc2235_init_controls(struct sc2235_dev *sensor)
+{
+ const struct v4l2_ctrl_ops *ops = &sc2235_ctrl_ops;
+ struct sc2235_ctrls *ctrls = &sensor->ctrls;
+ struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+ int ret;
+
+ v4l2_ctrl_handler_init(hdl, 32);
+
+ /* we can use our own mutex for the ctrl lock */
+ hdl->lock = &sensor->lock;
+
+ /* Clock related controls */
+ ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+ 0, INT_MAX, 1,
+ sc2235_calc_pixel_rate(sensor));
+
+ /* Auto/manual white balance */
+ ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+ V4L2_CID_AUTO_WHITE_BALANCE,
+ 0, 1, 1, 1);
+ ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+ 0, 4095, 1, 0);
+ ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+ 0, 4095, 1, 0);
+ /* Auto/manual exposure */
+#if 0
+ /*
+ *For 7110 platform, This operation will cause the image to be white.
+ */
+ ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_EXPOSURE_AUTO,
+ V4L2_EXPOSURE_MANUAL, 0,
+ V4L2_EXPOSURE_AUTO);
+ ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+ 0, 65535, 1, 0);
+ /* Auto/manual gain */
+ ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+ 0, 1, 1, 1);
+ ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+ 0, 1023, 1, 0);
+#else
+ ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_EXPOSURE_AUTO,
+ V4L2_EXPOSURE_MANUAL, 0,
+ 1);
+ ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+ 0, 65535, 1, 0x4600);
+ /* Auto/manual gain */
+ ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+ 0, 1, 1, 0);
+ ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+ 0, 1023, 1, 0x10);
+#endif
+ ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+ 0, 255, 1, 64);
+ ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+ 0, 359, 1, 0);
+ ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+ 0, 255, 1, 0);
+ ctrls->test_pattern =
+ v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+ ARRAY_SIZE(test_pattern_menu) - 1,
+ 0, 0, test_pattern_menu); //0x02
+ ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+ 0, 1, 1, 1);
+ ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+ 0, 1, 1, 0);
+
+ ctrls->light_freq =
+ v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_POWER_LINE_FREQUENCY,
+ V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+ V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+ if (hdl->error) {
+ ret = hdl->error;
+ goto free_ctrls;
+ }
+
+ ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+ ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+ ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+ v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+ v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+ v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+ sensor->sd.ctrl_handler = hdl;
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(hdl);
+ return ret;
+}
+
+static int sc2235_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ if (fse->pad != 0)
+ return -EINVAL;
+ if (fse->index >= SC2235_NUM_MODES)
+ return -EINVAL;
+
+ fse->min_width =
+ sc2235_mode_data[fse->index].hact;
+ fse->max_width = fse->min_width;
+ fse->min_height =
+ sc2235_mode_data[fse->index].vact;
+ fse->max_height = fse->min_height;
+
+ return 0;
+}
+
+static int sc2235_enum_frame_interval(
+ struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_interval_enum *fie)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ struct v4l2_fract tpf;
+ int ret;
+
+ if (fie->pad != 0)
+ return -EINVAL;
+ if (fie->index >= SC2235_NUM_FRAMERATES)
+ return -EINVAL;
+
+ tpf.numerator = 1;
+ tpf.denominator = sc2235_framerates[fie->index];
+
+ ret = sc2235_try_frame_interval(sensor, &tpf,
+ fie->width, fie->height);
+ if (ret < 0)
+ return -EINVAL;
+
+ fie->interval = tpf;
+ return 0;
+}
+
+static int sc2235_g_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+
+ mutex_lock(&sensor->lock);
+ fi->interval = sensor->frame_interval;
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int sc2235_s_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ const struct sc2235_mode_info *mode;
+ int frame_rate, ret = 0;
+
+ if (fi->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ mode = sensor->current_mode;
+
+ frame_rate = sc2235_try_frame_interval(sensor, &fi->interval,
+ mode->hact, mode->vact);
+ if (frame_rate < 0) {
+ /* Always return a valid frame interval value */
+ fi->interval = sensor->frame_interval;
+ goto out;
+ }
+
+ mode = sc2235_find_mode(sensor, frame_rate, mode->hact,
+ mode->vact, true);
+ if (!mode) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ if (mode != sensor->current_mode ||
+ frame_rate != sensor->current_fr) {
+ sensor->current_fr = frame_rate;
+ sensor->frame_interval = fi->interval;
+ sensor->current_mode = mode;
+ sensor->pending_mode_change = true;
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ sc2235_calc_pixel_rate(sensor));
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int sc2235_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ if (code->pad != 0)
+ return -EINVAL;
+ if (code->index >= ARRAY_SIZE(sc2235_formats))
+ return -EINVAL;
+
+ code->code = sc2235_formats[code->index].code;
+ return 0;
+}
+
+static int sc2235_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming == !enable) {
+ if (enable && sensor->pending_mode_change) {
+ ret = sc2235_set_mode(sensor);
+ if (ret)
+ goto out;
+ }
+
+ if (enable && sensor->pending_fmt_change) {
+ ret = sc2235_set_framefmt(sensor, &sensor->fmt);
+ if (ret)
+ goto out;
+ sensor->pending_fmt_change = false;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_PARALLEL)
+ ret = sc2235_set_stream_dvp(sensor, enable);
+
+ if (!ret)
+ sensor->streaming = enable;
+ }
+out:
+ mutex_unlock(&sensor->lock);
+
+ return ret;
+}
+
+static const struct v4l2_subdev_core_ops sc2235_core_ops = {
+ .s_power = sc2235_s_power,
+ .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 sc2235_video_ops = {
+ .g_frame_interval = sc2235_g_frame_interval,
+ .s_frame_interval = sc2235_s_frame_interval,
+ .s_stream = sc2235_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops sc2235_pad_ops = {
+ .enum_mbus_code = sc2235_enum_mbus_code,
+ .get_fmt = sc2235_get_fmt,
+ .set_fmt = sc2235_set_fmt,
+ .enum_frame_size = sc2235_enum_frame_size,
+ .enum_frame_interval = sc2235_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops sc2235_subdev_ops = {
+ .core = &sc2235_core_ops,
+ .video = &sc2235_video_ops,
+ .pad = &sc2235_pad_ops,
+};
+
+static int sc2235_get_regulators(struct sc2235_dev *sensor)
+{
+ int i;
+
+ for (i = 0; i < SC2235_NUM_SUPPLIES; i++)
+ sensor->supplies[i].supply = sc2235_supply_name[i];
+
+ return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+ SC2235_NUM_SUPPLIES,
+ sensor->supplies);
+}
+
+static int sc2235_check_chip_id(struct sc2235_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret = 0;
+ u16 chip_id;
+
+ ret = sc2235_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+ ret = sc2235_read_reg16(sensor, SC2235_REG_CHIP_ID, &chip_id);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to read chip identifier\n",
+ __func__);
+ goto power_off;
+ }
+
+ if (chip_id != SC2235_CHIP_ID) {
+ dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+ __func__, SC2235_CHIP_ID, chip_id);
+ ret = -ENXIO;
+ }
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+
+power_off:
+ sc2235_set_power_off(sensor);
+ return ret;
+}
+
+static int sc2235_probe(struct i2c_client *client)
+{
+ struct device *dev = &client->dev;
+ struct fwnode_handle *endpoint;
+ struct sc2235_dev *sensor;
+ struct v4l2_mbus_framefmt *fmt;
+ u32 rotation;
+ int ret;
+ u8 chip_id_high, chip_id_low;
+
+ sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+ if (!sensor)
+ return -ENOMEM;
+
+ sensor->i2c_client = client;
+
+ fmt = &sensor->fmt;
+ fmt->code = MEDIA_BUS_FMT_SGBRG10_1X10;
+ 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 = 1920;
+ fmt->height = 1080;
+ fmt->field = V4L2_FIELD_NONE;
+ sensor->frame_interval.numerator = 1;
+ sensor->frame_interval.denominator = sc2235_framerates[SC2235_30_FPS];
+ sensor->current_fr = SC2235_30_FPS;
+ sensor->current_mode =
+ &sc2235_mode_data[SC2235_MODE_1080P_1920_1080];
+ sensor->last_mode = sensor->current_mode;
+
+ sensor->ae_target = 52;
+
+ /* optional indication of physical rotation of sensor */
+ ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
+ &rotation);
+ if (!ret) {
+ switch (rotation) {
+ case 180:
+ sensor->upside_down = true;
+ fallthrough;
+ case 0:
+ break;
+ default:
+ dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
+ rotation);
+ }
+ }
+
+ endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+ NULL);
+ if (!endpoint) {
+ dev_err(dev, "endpoint node not found\n");
+ return -EINVAL;
+ }
+
+ ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
+ fwnode_handle_put(endpoint);
+ if (ret) {
+ dev_err(dev, "Could not parse endpoint\n");
+ return ret;
+ }
+
+ if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
+ sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+ sensor->ep.bus_type != V4L2_MBUS_BT656) {
+ dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
+ return -EINVAL;
+ }
+
+ /* get system clock (xclk) */
+ sensor->xclk = devm_clk_get(dev, "xclk");
+ if (IS_ERR(sensor->xclk)) {
+ dev_err(dev, "failed to get xclk\n");
+ return PTR_ERR(sensor->xclk);
+ }
+
+ sensor->xclk_freq = clk_get_rate(sensor->xclk);
+ if (sensor->xclk_freq < SC2235_XCLK_MIN ||
+ sensor->xclk_freq > SC2235_XCLK_MAX) {
+ dev_err(dev, "xclk frequency out of range: %d Hz\n",
+ sensor->xclk_freq);
+ return -EINVAL;
+ }
+#if 0
+ /*At present, the GPIO of the sensor is configured in the Uboot system,
+ and these GPIOs are controlled by the kernel GPIO subsystem API after
+ it is ready 2021 1110 */
+ /* request optional power down pin */
+ sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->pwdn_gpio))
+ return PTR_ERR(sensor->pwdn_gpio);
+
+ /* request optional reset pin */
+ sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->reset_gpio))
+ return PTR_ERR(sensor->reset_gpio);
+#endif
+ v4l2_i2c_subdev_init(&sensor->sd, client, &sc2235_subdev_ops);
+
+ sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+ V4L2_SUBDEV_FL_HAS_EVENTS;
+ sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+ sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+ ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+ if (ret)
+ return ret;
+
+ ret = sc2235_get_regulators(sensor);
+ if (ret)
+ return ret;
+ mutex_init(&sensor->lock);
+
+ ret = sc2235_check_chip_id(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = sc2235_init_controls(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+ if (ret)
+ goto free_ctrls;
+
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_cleanup:
+ media_entity_cleanup(&sensor->sd.entity);
+ mutex_destroy(&sensor->lock);
+ return ret;
+}
+
+static int sc2235_remove(struct i2c_client *client)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+
+ v4l2_async_unregister_subdev(&sensor->sd);
+ media_entity_cleanup(&sensor->sd.entity);
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+ mutex_destroy(&sensor->lock);
+
+ return 0;
+}
+
+static const struct i2c_device_id sc2235_id[] = {
+ {"sc2235", 0},
+ {},
+};
+MODULE_DEVICE_TABLE(i2c, sc2235_id);
+
+static const struct of_device_id sc2235_dt_ids[] = {
+ { .compatible = "sc2235" },
+ { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sc2235_dt_ids);
+
+static struct i2c_driver sc2235_i2c_driver = {
+ .driver = {
+ .name = "sc2235",
+ .of_match_table = sc2235_dt_ids,
+ },
+ .id_table = sc2235_id,
+ .probe_new = sc2235_probe,
+ .remove = sc2235_remove,
+};
+
+module_i2c_driver(sc2235_i2c_driver);
+
+MODULE_DESCRIPTION("SC2235 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_COMMON_H
+#define STF_COMMON_H
+
+#include <linux/kern_levels.h>
+
+// #define STF_DEBUG
+
+// #define USE_CSIDPHY_ONE_CLK_MODE 1
+
+enum {
+ ST_DVP = 0x0001,
+ ST_CSIPHY = 0x0002,
+ ST_CSI = 0x0004,
+ ST_ISP = 0x0008,
+ ST_VIN = 0x0010,
+ ST_VIDEO = 0x0020,
+ ST_CAMSS = 0x0040,
+ ST_SENSOR= 0x0080,
+};
+
+enum {
+ ST_NONE = 0x00,
+ ST_ERR = 0x01,
+ ST_WARN = 0x02,
+ ST_INFO = 0x03,
+ ST_DEBUG = 0x04,
+};
+
+extern unsigned int stdbg_level;
+extern unsigned int stdbg_mask;
+
+#define ST_MODULE2STRING(__module) ({ \
+ char *__str; \
+ \
+ switch (__module) { \
+ case ST_DVP: \
+ __str = "st_dvp"; \
+ break; \
+ case ST_CSIPHY: \
+ __str = "st_csiphy"; \
+ break; \
+ case ST_CSI: \
+ __str = "st_csi"; \
+ break; \
+ case ST_ISP: \
+ __str = "st_isp"; \
+ break; \
+ case ST_VIN: \
+ __str = "st_vin"; \
+ break; \
+ case ST_VIDEO: \
+ __str = "st_video"; \
+ break; \
+ case ST_CAMSS: \
+ __str = "st_camss"; \
+ break; \
+ case ST_SENSOR: \
+ __str = "st_sensor"; \
+ break; \
+ default: \
+ __str = "unknow"; \
+ break; \
+ } \
+ \
+ __str; \
+ })
+
+#define st_debug(module, __fmt, arg...) \
+ do { \
+ if (stdbg_level > ST_INFO) { \
+ if (stdbg_mask & module) \
+ pr_err("[%s] debug: " __fmt, \
+ ST_MODULE2STRING(module), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_info(module, __fmt, arg...) \
+ do { \
+ if (stdbg_level > ST_WARN) { \
+ if (stdbg_mask & module) \
+ pr_err("[%s] info: " __fmt, \
+ ST_MODULE2STRING(module), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_warn(module, __fmt, arg...) \
+ do { \
+ if (stdbg_level > ST_ERR) { \
+ if (stdbg_mask & module) \
+ pr_err("[%s] warn: " __fmt, \
+ ST_MODULE2STRING(module), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_err(module, __fmt, arg...) \
+ do { \
+ if (stdbg_level > ST_NONE) { \
+ if (stdbg_mask & module) \
+ pr_err("[%s] error: " __fmt, \
+ ST_MODULE2STRING(module), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_err_ratelimited(module, fmt, ...) \
+ do { \
+ static DEFINE_RATELIMIT_STATE(_rs, \
+ DEFAULT_RATELIMIT_INTERVAL, \
+ DEFAULT_RATELIMIT_BURST); \
+ if (__ratelimit(&_rs) && (stdbg_level > ST_NONE)) { \
+ if (stdbg_mask & module) \
+ pr_err("[%s] error: " fmt, \
+ ST_MODULE2STRING(module), \
+ ##__VA_ARGS__); \
+ } \
+ } while (0)
+
+#define set_bits(p, v, b, m) (((p) & ~(m)) | ((v) << (b)))
+
+static inline u32 reg_read(void __iomem * base, u32 reg)
+{
+ return ioread32(base + reg);
+}
+
+static inline void reg_write(void __iomem * base, u32 reg, u32 val)
+{
+ iowrite32(val, base + reg);
+}
+
+static inline void reg_set_bit(void __iomem * base, u32 reg, u32 mask, u32 val)
+{
+ u32 value;
+
+ value = ioread32(base + reg) & ~mask;
+ val &= mask;
+ val |= value;
+ iowrite32(val, base + reg);
+}
+
+static inline void reg_set(void __iomem * base, u32 reg, u32 mask)
+{
+ iowrite32(ioread32(base + reg) | mask, base + reg);
+}
+
+static inline void reg_clear(void __iomem * base, u32 reg, u32 mask)
+{
+ iowrite32(ioread32(base + reg) & ~mask, base + reg);
+}
+
+static inline void reg_clear_rst(void __iomem * base, u32 reg1, u32 reg2, uint32_t mask)
+{
+ u32 val;
+
+ val = ioread32(base + reg1);
+ val &= ~mask;
+ iowrite32(val, base + reg1);
+ do{
+ val = ioread32(base + reg2);
+ }while((val&mask)!=mask);
+
+}
+
+static void reg_assert_rst(void __iomem * base, u32 reg1, u32 reg2, uint32_t mask)
+{
+ u32 val;
+
+ val = ioread32(base + reg1);
+ val |= mask;
+ iowrite32(val, base + reg1);
+ do{
+ val = ioread32(base + reg2);
+ }while((val&mask)!=0);
+}
+
+static inline void reg_set_highest_bit(void __iomem * base, u32 reg)
+{
+ u32 val;
+
+ val = ioread32(base + reg);
+ val &= ~(0x1 << 31);
+ val |= (0x1 & 0x1) << 31;
+ iowrite32(val, base + reg);
+}
+
+static inline void reg_clr_highest_bit(void __iomem * base, u32 reg)
+{
+ u32 val;
+
+ val = ioread32(base + reg);
+ val &= ~(0x1 << 31);
+ val |= (0x0 & 0x1) << 31;
+ iowrite32(val, base + reg);
+}
+
+static inline void print_reg(unsigned int module, void __iomem * base, u32 reg)
+{
+ st_debug(module, "REG 0x%x = 0x%x\n",
+ base + reg, ioread32(base + reg));
+}
+
+#endif /* STF_COMMON_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.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>
+
+#define STF_CSI_NAME "stf_csi"
+
+static const struct csi_format csi_formats_st7110[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, 16},
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+static int csi_find_format(u32 code,
+ const struct csi_format *formats,
+ unsigned int nformats)
+{
+ int i;
+
+ for (i = 0; i < nformats; i++)
+ if (formats[i].code == code)
+ return i;
+ return -EINVAL;
+}
+
+int stf_csi_subdev_init(struct stfcamss *stfcamss, int id)
+{
+ struct stf_csi_dev *csi_dev = &stfcamss->csi_dev[id];
+
+ csi_dev->id = id;
+ csi_dev->csiphy_id = id;
+ csi_dev->s_type = SENSOR_VIN;
+ csi_dev->hw_ops = &csi_ops;
+ csi_dev->stfcamss = stfcamss;
+ csi_dev->formats = csi_formats_st7110;
+ csi_dev->nformats = ARRAY_SIZE(csi_formats_st7110);
+ mutex_init(&csi_dev->stream_lock);
+ return 0;
+}
+
+static int csi_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__csi_get_format(struct stf_csi_dev *csi_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(&csi_dev->subdev, state, pad);
+
+ return &csi_dev->fmt[pad];
+}
+
+static int csi_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+ struct stf_csi_dev *csi0_dev = &csi_dev->stfcamss->csi_dev[0];
+ struct v4l2_mbus_framefmt *format;
+ int ret = 0, is_raw10 = 0;
+ u32 code;
+
+ if (csi_dev->id == 1)
+ csi_set_stream(&csi0_dev->subdev, enable);
+
+ format = __csi_get_format(csi_dev, NULL, STF_CSI_PAD_SRC,
+ V4L2_SUBDEV_FORMAT_ACTIVE);
+ if (format == NULL)
+ return -EINVAL;
+ ret = csi_find_format(format->code,
+ csi_dev->formats,
+ csi_dev->nformats);
+ if (ret < 0)
+ return ret;
+
+ code = csi_dev->formats[ret].code;
+ if (code == MEDIA_BUS_FMT_SBGGR10_1X10 ||
+ code == MEDIA_BUS_FMT_SGBRG10_1X10 ||
+ code == MEDIA_BUS_FMT_SGRBG10_1X10 ||
+ code == MEDIA_BUS_FMT_SRGGB10_1X10)
+ is_raw10 = 1;
+
+ mutex_lock(&csi_dev->stream_lock);
+ if (enable) {
+ if (csi_dev->stream_count == 0) {
+ csi_dev->hw_ops->csi_config_set(csi_dev);
+ csi_dev->hw_ops->csi_clk_enable(csi_dev);
+ csi_dev->hw_ops->csi_set_format(csi_dev,
+ format->height,
+ csi_dev->formats[ret].bpp, is_raw10);
+ csi_dev->hw_ops->csi_stream_set(csi_dev, enable);
+ }
+ csi_dev->stream_count++;
+ } else {
+ if (csi_dev->stream_count == 0)
+ goto exit;
+ if (csi_dev->stream_count == 1) {
+ csi_dev->hw_ops->csi_stream_set(csi_dev, enable);
+ csi_dev->hw_ops->csi_clk_disable(csi_dev);
+ }
+ csi_dev->stream_count--;
+ }
+exit:
+ mutex_unlock(&csi_dev->stream_lock);
+ return 0;
+}
+
+static void csi_try_format(struct stf_csi_dev *csi_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ unsigned int i;
+
+ switch (pad) {
+ case STF_CSI_PAD_SINK:
+ /* Set format on sink pad */
+
+ for (i = 0; i < csi_dev->nformats; i++)
+ if (fmt->code == csi_dev->formats[i].code)
+ break;
+
+ if (i >= csi_dev->nformats)
+ fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+ fmt->width = clamp_t(u32,
+ fmt->width,
+ 1,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height,
+ 1,
+ STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ break;
+
+ case STF_CSI_PAD_SRC:
+
+ *fmt = *__csi_get_format(csi_dev, state, STF_CSI_PAD_SINK, which);
+
+ break;
+ }
+}
+
+static int csi_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+
+ if (code->index >= csi_dev->nformats)
+ return -EINVAL;
+ if (code->pad == STF_CSI_PAD_SINK) {
+ code->code = csi_dev->formats[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __csi_get_format(csi_dev, state, STF_CSI_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+
+ return 0;
+}
+
+static int csi_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ csi_try_format(csi_dev, state, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ csi_try_format(csi_dev, state, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int csi_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __csi_get_format(csi_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int csi_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+ int ret;
+
+ format = __csi_get_format(csi_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ csi_try_format(csi_dev, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_CSI_PAD_SINK) {
+ format = __csi_get_format(csi_dev, state, STF_CSI_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ csi_try_format(csi_dev, state, STF_CSI_PAD_SRC, format,
+ fmt->which);
+ }
+
+ return 0;
+}
+
+static int csi_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_CSI_PAD_SINK,
+ .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
+ V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return csi_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int csi_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+ (flags & MEDIA_LNK_FL_ENABLED)) {
+ struct v4l2_subdev *sd;
+ struct stf_csi_dev *csi_dev;
+ struct vin_line *line;
+
+ if (media_entity_remote_pad(local))
+ return -EBUSY;
+
+ sd = media_entity_to_v4l2_subdev(entity);
+ csi_dev = v4l2_get_subdevdata(sd);
+
+ sd = media_entity_to_v4l2_subdev(remote->entity);
+ line = v4l2_get_subdevdata(sd);
+ if (line->sdev_type == VIN_DEV_TYPE)
+ csi_dev->s_type = SENSOR_VIN;
+ if (line->sdev_type == ISP0_DEV_TYPE)
+ csi_dev->s_type = SENSOR_ISP0;
+ if (line->sdev_type == ISP1_DEV_TYPE)
+ csi_dev->s_type = SENSOR_ISP1;
+ st_info(ST_CSI, "CSI%d device sensor type: %d\n",
+ csi_dev->id, csi_dev->s_type);
+ }
+
+ if ((local->flags & MEDIA_PAD_FL_SINK) &&
+ (flags & MEDIA_LNK_FL_ENABLED)) {
+ struct v4l2_subdev *sd;
+ struct stf_csi_dev *csi_dev;
+ struct stf_csiphy_dev *csiphy_dev;
+
+ if (media_entity_remote_pad(local))
+ return -EBUSY;
+
+ sd = media_entity_to_v4l2_subdev(entity);
+ csi_dev = v4l2_get_subdevdata(sd);
+
+ sd = media_entity_to_v4l2_subdev(remote->entity);
+ csiphy_dev = v4l2_get_subdevdata(sd);
+
+ csi_dev->csiphy_id = csiphy_dev->id;
+ st_info(ST_SENSOR, "CSI%d link to csiphy%d\n",
+ csi_dev->id, csi_dev->csiphy_id);
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops csi_core_ops = {
+ .s_power = csi_set_power,
+};
+
+static const struct v4l2_subdev_video_ops csi_video_ops = {
+ .s_stream = csi_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csi_pad_ops = {
+ .enum_mbus_code = csi_enum_mbus_code,
+ .enum_frame_size = csi_enum_frame_size,
+ .get_fmt = csi_get_format,
+ .set_fmt = csi_set_format,
+};
+
+static const struct v4l2_subdev_ops csi_v4l2_ops = {
+ .core = &csi_core_ops,
+ .video = &csi_video_ops,
+ .pad = &csi_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi_v4l2_internal_ops = {
+ .open = csi_init_formats,
+};
+
+static const struct media_entity_operations csi_media_ops = {
+ .link_setup = csi_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_csi_register(struct stf_csi_dev *csi_dev, struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd = &csi_dev->subdev;
+ struct device *dev = csi_dev->stfcamss->dev;
+ struct media_pad *pads = csi_dev->pads;
+ int ret;
+
+ v4l2_subdev_init(sd, &csi_v4l2_ops);
+ sd->internal_ops = &csi_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+ STF_CSI_NAME, csi_dev->id);
+ v4l2_set_subdevdata(sd, csi_dev);
+
+ ret = csi_init_formats(sd, NULL);
+ if (ret < 0) {
+ dev_err(dev, "Failed to init format: %d\n", ret);
+ return ret;
+ }
+
+ pads[STF_CSI_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_CSI_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &csi_media_ops;
+ ret = media_entity_pads_init(&sd->entity, STF_CSI_PADS_NUM, pads);
+ if (ret < 0) {
+ dev_err(dev, "Failed to init media entity: %d\n", ret);
+ return ret;
+ }
+
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ dev_err(dev, "Failed to register subdev: %d\n", ret);
+ goto err_sreg;
+ }
+
+ return 0;
+
+err_sreg:
+ media_entity_cleanup(&sd->entity);
+ return ret;
+}
+
+int stf_csi_unregister(struct stf_csi_dev *csi_dev)
+{
+ v4l2_device_unregister_subdev(&csi_dev->subdev);
+ media_entity_cleanup(&csi_dev->subdev.entity);
+ mutex_destroy(&csi_dev->stream_lock);
+ return 0;
+}
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_CSI_H
+#define STF_CSI_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_CSI_PAD_SINK 0
+#define STF_CSI_PAD_SRC 1
+#define STF_CSI_PADS_NUM 2
+
+struct csi_format {
+ u32 code;
+ u8 bpp;
+};
+
+struct stf_csi_dev;
+
+struct csi_hw_ops {
+ int (*csi_clk_enable)(struct stf_csi_dev *csi_dev);
+ int (*csi_clk_disable)(struct stf_csi_dev *csi_dev);
+ int (*csi_config_set)(struct stf_csi_dev *csi_dev);
+ int (*csi_set_format)(struct stf_csi_dev *csi_dev,
+ u32 vsize, u8 bpp, int is_raw10);
+ int (*csi_stream_set)(struct stf_csi_dev *csi_dev, int on);
+};
+
+struct stf_csi_dev {
+ struct stfcamss *stfcamss;
+ u8 id;
+ u8 csiphy_id;
+ enum sensor_type s_type;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_CSI_PADS_NUM];
+ struct v4l2_mbus_framefmt fmt[STF_CSI_PADS_NUM];
+ const struct csi_format *formats;
+ unsigned int nformats;
+ struct csi_hw_ops *hw_ops;
+ struct mutex stream_lock;
+ int stream_count;
+};
+
+extern int stf_csi_subdev_init(struct stfcamss *stfcamss, int id);
+extern int stf_csi_register(struct stf_csi_dev *csi_dev,
+ struct v4l2_device *v4l2_dev);
+extern int stf_csi_unregister(struct stf_csi_dev *csi_dev);
+extern int raw_csi2rx_dphy_config(struct stf_vin_dev *vin,
+ const csi2rx_dphy_cfg_t *cfg);
+extern int raw_csi2rx_config(struct stf_vin_dev *vin,
+ int id,
+ const csi2rx_cfg_t *cfg);
+extern struct csi_hw_ops csi_ops;
+extern void dump_csi_reg(void *__iomem csibase, int id);
+
+#endif /* STF_CSI_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * linux/drivers/media/platform/starfive/stf_csi.c
+ *
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ *
+ */
+#include "stfcamss.h"
+
+#define CSI2RX_DEVICE_CFG_REG 0x000
+
+#define CSI2RX_SOFT_RESET_REG 0x004
+#define CSI2RX_SOFT_RESET_PROTOCOL BIT(1)
+#define CSI2RX_SOFT_RESET_FRONT BIT(0)
+
+#define CSI2RX_DPHY_LANE_CONTROL 0x040
+
+#define CSI2RX_STATIC_CFG_REG 0x008
+#define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane) \
+ ((plane) << (16 + (llane) * 4))
+#define CSI2RX_STATIC_CFG_LANES_MASK GENMASK(11, 8)
+
+#define CSI2RX_STREAM_BASE(n) (((n) + 1) * 0x100)
+
+#define CSI2RX_STREAM_CTRL_REG(n) (CSI2RX_STREAM_BASE(n) + 0x000)
+#define CSI2RX_STREAM_CTRL_START BIT(0)
+
+#define CSI2RX_STREAM_DATA_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x008)
+#define CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT BIT(31)
+#define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n) BIT((n) + 16)
+
+#define CSI2RX_STREAM_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x00c)
+#define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF (1 << 8)
+
+#define CSI2RX_LANES_MAX 4
+#define CSI2RX_STREAMS_MAX 4
+
+static int apb_clk_set(struct stf_vin_dev *vin, int on)
+{
+ static int init_flag;
+ static struct mutex count_lock;
+ static int count;
+
+ if (!init_flag) {
+ init_flag = 1;
+ mutex_init(&count_lock);
+ }
+ mutex_lock(&count_lock);
+ if (on) {
+ if (count == 0)
+ reg_set_highest_bit(vin->clkgen_base,
+ CLK_CSI2RX0_APB_CTRL);
+ count++;
+ } else {
+ if (count == 0)
+ goto exit;
+ if (count == 1)
+ reg_clr_highest_bit(vin->clkgen_base,
+ CLK_CSI2RX0_APB_CTRL);
+ count--;
+ }
+exit:
+ mutex_unlock(&count_lock);
+ return 0;
+
+}
+static int stf_csi_clk_enable(struct stf_csi_dev *csi_dev)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+
+ // reg_set_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+ apb_clk_set(vin, 1);
+
+ if (csi_dev->id == 0) {
+ reg_set_bit(vin->clkgen_base,
+ CLK_MIPI_RX0_PXL_CTRL,
+ 0x1F, 0x3);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+ } else {
+ reg_set_bit(vin->clkgen_base,
+ CLK_MIPI_RX1_PXL_CTRL,
+ 0x1F, 0x3);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+ }
+
+ return 0;
+}
+
+static int stf_csi_clk_disable(struct stf_csi_dev *csi_dev)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+
+ // reg_clr_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+ apb_clk_set(vin, 0);
+
+ if (csi_dev->id == 0) {
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+ } else {
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+ }
+
+ return 0;
+}
+
+static int stf_csi_config_set(struct stf_csi_dev *csi_dev)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+ u32 mipi_channel_sel, mipi_vc = 0;
+
+ st_info(ST_CSI, "%s, csi_id = %d\n", __func__, csi_dev->id);
+
+ switch (csi_dev->s_type) {
+ case SENSOR_VIN:
+ st_err(ST_CSI, "%s, %d: need todo\n", __func__, __LINE__);
+ break;
+ case SENSOR_ISP0:
+ reg_set_bit(vin->clkgen_base,
+ CLK_ISP0_MIPI_CTRL,
+ BIT(24), csi_dev->id << 24);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_C_ISP0_CTRL,
+ BIT(25) | BIT(24),
+ csi_dev->id << 24);
+
+ mipi_channel_sel = csi_dev->id * 4 + mipi_vc;
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_CHAN_SEL,
+ 0xF, mipi_channel_sel);
+ break;
+ case SENSOR_ISP1:
+ reg_set_bit(vin->clkgen_base,
+ CLK_ISP1_MIPI_CTRL,
+ BIT(24), csi_dev->id << 24);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_C_ISP1_CTRL,
+ BIT(25) | BIT(24),
+ csi_dev->id << 24);
+
+ mipi_channel_sel = csi_dev->id * 4 + mipi_vc;
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_CHAN_SEL,
+ 0xF << 4, mipi_channel_sel << 4);
+ default:
+ break;
+ }
+
+ return 0;
+}
+
+static int stf_csi_set_format(struct stf_csi_dev *csi_dev,
+ u32 vsize, u8 bpp, int is_raw10)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+ void *reg_base = NULL;
+
+ if (csi_dev->id == 0)
+ reg_base = vin->mipi0_base;
+ else if (csi_dev->id == 1)
+ reg_base = vin->mipi1_base;
+ else
+ return 0;
+
+ switch (csi_dev->s_type) {
+ case SENSOR_VIN:
+ st_err(ST_CSI, "%s, %d: need todo\n", __func__, __LINE__);
+ break;
+ case SENSOR_ISP0:
+ if (is_raw10)
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_DW_SEL,
+ BIT(4), 1 << 4);
+ break;
+ case SENSOR_ISP1:
+ if (is_raw10)
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_DW_SEL,
+ BIT(5), 1 << 5);
+ default:
+ break;
+ }
+
+ return 0;
+}
+
+static void csi2rx_reset(void *reg_base)
+{
+ writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT,
+ reg_base + CSI2RX_SOFT_RESET_REG);
+
+ udelay(10);
+
+ writel(0, reg_base + CSI2RX_SOFT_RESET_REG);
+}
+
+static void csi2rx_debug_config(void *reg_base, u32 frame_lines)
+{
+ // data_id, ecc, crc error to irq
+ union error_bypass_cfg err_bypass_cfg = {
+ .data_id = 0,
+ .ecc = 0,
+ .crc = 0,
+ };
+ union stream_monitor_ctrl stream0_monitor_ctrl = {
+ .frame_length = frame_lines,
+ .frame_mon_en = 1,
+ .frame_mon_vc = 0,
+ .lb_en = 1,
+ .lb_vc = 0,
+ };
+ reg_write(reg_base, ERROR_BYPASS_CFG, err_bypass_cfg.value);
+ reg_write(reg_base, MONITOR_IRQS_MASK_CFG, 0xffffffff);
+ reg_write(reg_base, INFO_IRQS_MASK_CFG, 0xffffffff);
+ reg_write(reg_base, ERROR_IRQS_MASK_CFG, 0xffffffff);
+ reg_write(reg_base, DPHY_ERR_IRQ_MASK_CFG, 0xffffffff);
+
+ reg_write(reg_base, STREAM0_MONITOR_CTRL, stream0_monitor_ctrl.value);
+ reg_write(reg_base, STREAM0_FCC_CTRL, (0x0 << 1) | 0x1);
+}
+
+static int csi2rx_start(struct stf_csi_dev *csi_dev)
+{
+ struct stfcamss *stfcamss = csi_dev->stfcamss;
+ struct stf_vin_dev *vin = stfcamss->vin;
+ struct csi2phy_cfg *csiphy =
+ stfcamss->csiphy_dev[csi_dev->csiphy_id].csiphy;
+ unsigned int i;
+ unsigned long lanes_used = 0;
+ u32 reg;
+ int ret;
+ void *reg_base = NULL;
+
+ if (!csiphy) {
+ st_err(ST_CSI, "csiphy%d sensor not exist use csiphy%d init.\n",
+ csi_dev->csiphy_id, !csi_dev->csiphy_id);
+ csiphy = stfcamss->csiphy_dev[!csi_dev->csiphy_id].csiphy;
+ if (!csiphy) {
+ st_err(ST_CSI, "csiphy%d sensor not exist\n",
+ !csi_dev->csiphy_id);
+ return -EINVAL;
+ }
+ }
+
+ if (csi_dev->id == 0)
+ reg_base = vin->mipi0_base;
+ else if (csi_dev->id == 1)
+ reg_base = vin->mipi1_base;
+ else
+ return 0;
+
+ csi2rx_reset(reg_base);
+
+ reg = csiphy->num_data_lanes << 8;
+ for (i = 0; i < csiphy->num_data_lanes; i++) {
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+ reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csiphy->data_lanes[i]);
+ set_bit(csiphy->data_lanes[i] - 1, &lanes_used);
+#else
+ reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1);
+ set_bit(i, &lanes_used);
+#endif
+ }
+
+ /*
+ * Even the unused lanes need to be mapped. In order to avoid
+ * to map twice to the same physical lane, keep the lanes used
+ * in the previous loop, and only map unused physical lanes to
+ * the rest of our logical lanes.
+ */
+ for (i = csiphy->num_data_lanes; i < CSI2RX_LANES_MAX; i++) {
+ unsigned int idx = find_first_zero_bit(&lanes_used,
+ CSI2RX_LANES_MAX);
+
+ set_bit(idx, &lanes_used);
+ reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, idx + 1);
+ }
+
+ writel(reg, reg_base + CSI2RX_STATIC_CFG_REG);
+
+ // 0x40 DPHY_LANE_CONTROL
+ reg = 0;
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+ for (i = 0; i < csiphy->num_data_lanes; i++)
+ reg |= 1 << (csiphy->data_lanes[i] - 1)
+ | 1 << (csiphy->data_lanes[i] + 11);
+#else
+ for (i = 0; i < csiphy->num_data_lanes; i++)
+ reg |= 1 << i | 1 << (i + 12);
+#endif
+
+ reg |= 1 << 4 | 1 << 16;
+ writel(reg, reg_base + CSI2RX_DPHY_LANE_CONTROL);
+
+ // csi2rx_debug_config(reg_base, 1080);
+
+ /*
+ * Create a static mapping between the CSI virtual channels
+ * and the output stream.
+ *
+ * This should be enhanced, but v4l2 lacks the support for
+ * changing that mapping dynamically.
+ *
+ * We also cannot enable and disable independent streams here,
+ * hence the reference counting.
+ */
+ for (i = 0; i < CSI2RX_STREAMS_MAX; i++) {
+ writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
+ reg_base + CSI2RX_STREAM_CFG_REG(i));
+
+ writel(CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT |
+ CSI2RX_STREAM_DATA_CFG_VC_SELECT(i),
+ reg_base + CSI2RX_STREAM_DATA_CFG_REG(i));
+
+ writel(CSI2RX_STREAM_CTRL_START,
+ reg_base + CSI2RX_STREAM_CTRL_REG(i));
+ }
+
+ return 0;
+}
+
+static void csi2rx_stop(void *reg_base)
+{
+ unsigned int i;
+
+ for (i = 0; i < CSI2RX_STREAMS_MAX; i++)
+ writel(0, reg_base + CSI2RX_STREAM_CTRL_REG(i));
+}
+
+static int stf_csi_stream_set(struct stf_csi_dev *csi_dev, int on)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+ void *reg_base = NULL;
+
+ if (csi_dev->id == 0)
+ reg_base = vin->mipi0_base;
+ else if (csi_dev->id == 1)
+ reg_base = vin->mipi1_base;
+ else
+ return 0;
+
+ if (on)
+ csi2rx_start(csi_dev);
+ else
+ csi2rx_stop(reg_base);
+
+ return 0;
+}
+
+void dump_csi_reg(void *__iomem csibase, int id)
+{
+ st_info(ST_CSI, "DUMP CSI%d register:\n", id);
+ print_reg(ST_CSI, csibase, 0x00);
+ print_reg(ST_CSI, csibase, 0x04);
+ print_reg(ST_CSI, csibase, 0x08);
+ print_reg(ST_CSI, csibase, 0x10);
+
+ print_reg(ST_CSI, csibase, 0x40);
+ print_reg(ST_CSI, csibase, 0x48);
+ print_reg(ST_CSI, csibase, 0x4c);
+ print_reg(ST_CSI, csibase, 0x50);
+}
+
+struct csi_hw_ops csi_ops = {
+ .csi_clk_enable = stf_csi_clk_enable,
+ .csi_clk_disable = stf_csi_clk_disable,
+ .csi_config_set = stf_csi_config_set,
+ .csi_set_format = stf_csi_set_format,
+ .csi_stream_set = stf_csi_stream_set,
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.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>
+
+#define STF_CSIPHY_NAME "stf_csiphy"
+
+static const struct csiphy_format csiphy_formats_st7110[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, 16},
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+int stf_csiphy_subdev_init(struct stfcamss *stfcamss, int id)
+{
+ struct stf_csiphy_dev *csiphy_dev = &stfcamss->csiphy_dev[id];
+
+ csiphy_dev->id = id;
+ csiphy_dev->csi_id = id;
+ csiphy_dev->hw_ops = &csiphy_ops;
+ csiphy_dev->stfcamss = stfcamss;
+ csiphy_dev->formats = csiphy_formats_st7110;
+ csiphy_dev->nformats = ARRAY_SIZE(csiphy_formats_st7110);
+ mutex_init(&csiphy_dev->stream_lock);
+ return 0;
+}
+
+static int csiphy_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+ if (on)
+ csiphy_dev->hw_ops->cdns_csi_power(csiphy_dev, on);
+#endif
+ return 0;
+}
+
+static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+
+ mutex_lock(&csiphy_dev->stream_lock);
+ if (enable) {
+ if (csiphy_dev->stream_count == 0) {
+ csiphy_dev->hw_ops->csiphy_clk_enable(csiphy_dev);
+ csiphy_dev->hw_ops->csiphy_config_set(csiphy_dev);
+ csiphy_dev->hw_ops->csiphy_stream_set(csiphy_dev, 1);
+ }
+ csiphy_dev->stream_count++;
+ } else {
+ if (csiphy_dev->stream_count == 0)
+ goto exit;
+ if (csiphy_dev->stream_count == 1) {
+ csiphy_dev->hw_ops->csiphy_clk_disable(csiphy_dev);
+ csiphy_dev->hw_ops->csiphy_stream_set(csiphy_dev, 0);
+ }
+ csiphy_dev->stream_count--;
+ }
+exit:
+ mutex_unlock(&csiphy_dev->stream_lock);
+
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+ if (!enable)
+ csiphy_dev->hw_ops->cdns_csi_power(csiphy_dev, enable);
+#endif
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__csiphy_get_format(struct stf_csiphy_dev *csiphy_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(
+ &csiphy_dev->subdev,
+ state,
+ pad);
+
+ return &csiphy_dev->fmt[pad];
+}
+
+static void csiphy_try_format(struct stf_csiphy_dev *csiphy_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ unsigned int i;
+
+ switch (pad) {
+ case STF_CSIPHY_PAD_SINK:
+ /* Set format on sink pad */
+
+ for (i = 0; i < csiphy_dev->nformats; i++)
+ if (fmt->code == csiphy_dev->formats[i].code)
+ break;
+
+ if (i >= csiphy_dev->nformats)
+ fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+ fmt->width = clamp_t(u32,
+ fmt->width,
+ 1,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height,
+ 1,
+ STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ break;
+
+ case STF_CSIPHY_PAD_SRC:
+
+ *fmt = *__csiphy_get_format(csiphy_dev,
+ state,
+ STF_CSIPHY_PAD_SINK, which);
+
+ break;
+ }
+}
+
+static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+
+ if (code->index >= csiphy_dev->nformats)
+ return -EINVAL;
+
+ if (code->pad == STF_CSIPHY_PAD_SINK) {
+ code->code = csiphy_dev->formats[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __csiphy_get_format(csiphy_dev, state,
+ STF_CSIPHY_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+ return 0;
+}
+
+static int csiphy_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ csiphy_try_format(csiphy_dev, state, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ csiphy_try_format(csiphy_dev, state, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int csiphy_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __csiphy_get_format(csiphy_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int csiphy_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_csiphy_dev *csiphy_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __csiphy_get_format(csiphy_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ csiphy_try_format(csiphy_dev, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_CSIPHY_PAD_SINK) {
+ format = __csiphy_get_format(csiphy_dev,
+ state,
+ STF_CSIPHY_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ csiphy_try_format(csiphy_dev, state, STF_CSIPHY_PAD_SRC, format,
+ fmt->which);
+ }
+
+ return 0;
+}
+
+static int csiphy_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_CSIPHY_PAD_SINK,
+ .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
+ V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return csiphy_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int csiphy_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+ (flags & MEDIA_LNK_FL_ENABLED)) {
+ struct v4l2_subdev *sd;
+ struct stf_csiphy_dev *csiphy_dev;
+ struct stf_csi_dev *csi_dev;
+
+ if (media_entity_remote_pad(local))
+ return -EBUSY;
+
+ sd = media_entity_to_v4l2_subdev(entity);
+ csiphy_dev = v4l2_get_subdevdata(sd);
+
+ sd = media_entity_to_v4l2_subdev(remote->entity);
+ csi_dev = v4l2_get_subdevdata(sd);
+ csiphy_dev->csi_id = csi_dev->id;
+ st_info(ST_CSIPHY, "CSIPHY%d link to CSI%d\n",
+ csiphy_dev->id, csiphy_dev->csi_id);
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops csiphy_core_ops = {
+ .s_power = csiphy_set_power,
+};
+
+static const struct v4l2_subdev_video_ops csiphy_video_ops = {
+ .s_stream = csiphy_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops csiphy_pad_ops = {
+ .enum_mbus_code = csiphy_enum_mbus_code,
+ .enum_frame_size = csiphy_enum_frame_size,
+ .get_fmt = csiphy_get_format,
+ .set_fmt = csiphy_set_format,
+};
+
+static const struct v4l2_subdev_ops csiphy_v4l2_ops = {
+ .core = &csiphy_core_ops,
+ .video = &csiphy_video_ops,
+ .pad = &csiphy_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = {
+ .open = csiphy_init_formats,
+};
+
+static const struct media_entity_operations csiphy_media_ops = {
+ .link_setup = csiphy_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_csiphy_register(struct stf_csiphy_dev *csiphy_dev,
+ struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd = &csiphy_dev->subdev;
+ struct device *dev = csiphy_dev->stfcamss->dev;
+ struct media_pad *pads = csiphy_dev->pads;
+ int ret;
+
+ v4l2_subdev_init(sd, &csiphy_v4l2_ops);
+ sd->internal_ops = &csiphy_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+ STF_CSIPHY_NAME, csiphy_dev->id);
+ v4l2_set_subdevdata(sd, csiphy_dev);
+
+ ret = csiphy_init_formats(sd, NULL);
+ if (ret < 0) {
+ dev_err(dev, "Failed to init format: %d\n", ret);
+ return ret;
+ }
+
+ pads[STF_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &csiphy_media_ops;
+ ret = media_entity_pads_init(&sd->entity, STF_CSIPHY_PADS_NUM, pads);
+ if (ret < 0) {
+ dev_err(dev, "Failed to init media entity: %d\n", ret);
+ return ret;
+ }
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ dev_err(dev, "Failed to register subdev: %d\n", ret);
+ goto err_sreg;
+ }
+#else
+ sd->dev = dev;
+ ret = v4l2_async_register_subdev(sd);
+ if (ret < 0) {
+ dev_err(dev, "Failed to async register subdev: %d\n", ret);
+ goto err_sreg;
+ }
+#endif
+
+ return 0;
+
+err_sreg:
+ media_entity_cleanup(&sd->entity);
+ return ret;
+}
+
+int stf_csiphy_unregister(struct stf_csiphy_dev *csiphy_dev)
+{
+ v4l2_device_unregister_subdev(&csiphy_dev->subdev);
+ media_entity_cleanup(&csiphy_dev->subdev.entity);
+ mutex_destroy(&csiphy_dev->stream_lock);
+ return 0;
+}
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_CSIPHY_H
+#define STF_CSIPHY_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_CSIPHY_PAD_SINK 0
+#define STF_CSIPHY_PAD_SRC 1
+#define STF_CSIPHY_PADS_NUM 2
+
+#define STF_CSI2_MAX_DATA_LANES 4
+
+union static_config {
+ u32 raw;
+ struct {
+ u32 sel : 2;
+ u32 rsvd_6 : 2;
+ u32 v2p0_support_enable : 1;
+ u32 rsvd_5 : 3;
+ u32 lane_nb : 3;
+ u32 rsvd_4 : 5;
+ u32 dl0_map : 3;
+ u32 rsvd_3 : 1;
+ u32 dl1_map : 3;
+ u32 rsvd_2 : 1;
+ u32 dl2_map : 3;
+ u32 rsvd_1 : 1;
+ u32 dl3_map : 3;
+ u32 rsvd_0 : 1;
+ } bits;
+};
+
+union error_bypass_cfg {
+ u32 value;
+ struct {
+ u32 crc : 1;
+ u32 ecc : 1;
+ u32 data_id : 1;
+ u32 rsvd_0 : 29;
+ };
+};
+
+union stream_monitor_ctrl {
+ u32 value;
+ struct {
+ u32 lb_vc : 4;
+ u32 lb_en : 1;
+ u32 timer_vc : 4;
+ u32 timer_en : 1;
+ u32 timer_eof : 1;
+ u32 frame_mon_vc : 4;
+ u32 frame_mon_en : 1;
+ u32 frame_length : 16;
+ };
+};
+
+union stream_cfg {
+ u32 value;
+ struct {
+ u32 interface_mode : 1;
+ u32 ls_le_mode : 1;
+ u32 rsvd_3 : 2;
+ u32 num_pixels : 2;
+ u32 rsvd_2 : 2;
+ u32 fifo_mode : 2;
+ u32 rsvd_1 : 2;
+ u32 bpp_bypass : 3;
+ u32 rsvd_0 : 1;
+ u32 fifo_fill : 16;
+ };
+};
+
+union dphy_lane_ctrl {
+ u32 raw;
+ struct {
+ u32 dl0_en : 1;
+ u32 dl1_en : 1;
+ u32 dl2_en : 1;
+ u32 dl3_en : 1;
+ u32 cl_en : 1;
+ u32 rsvd_1 : 7;
+ u32 dl0_reset : 1;
+ u32 dl1_reset : 1;
+ u32 dl2_reset : 1;
+ u32 dl3_reset : 1;
+ u32 cl_reset : 1;
+ u32 rsvd_0 : 15;
+ } bits;
+};
+
+union dphy_lane_swap {
+ u32 raw;
+ struct {
+ u32 rx_1c2c_sel : 1;
+ u32 lane_swap_clk : 3;
+ u32 lane_swap_clk1 : 3;
+ u32 lane_swap_lan0 : 3;
+ u32 lane_swap_lan1 : 3;
+ u32 lane_swap_lan2 : 3;
+ u32 lane_swap_lan3 : 3;
+ u32 dpdn_swap_clk : 1;
+ u32 dpdn_swap_clk1 : 1;
+ u32 dpdn_swap_lan0 : 1;
+ u32 dpdn_swap_lan1 : 1;
+ u32 dpdn_swap_lan2 : 1;
+ u32 dpdn_swap_lan3 : 1;
+ u32 hs_freq_chang_clk0 : 1;
+ u32 hs_freq_chang_clk1 : 1;
+ u32 reserved : 5;
+ } bits;
+};
+
+union dphy_lane_en {
+ u32 raw;
+ struct {
+ u32 gpio_en : 6;
+ u32 mp_test_mode_sel : 5;
+ u32 mp_test_en : 1;
+ u32 dphy_enable_lan0 : 1;
+ u32 dphy_enable_lan1 : 1;
+ u32 dphy_enable_lan2 : 1;
+ u32 dphy_enable_lan3 : 1;
+ u32 rsvd_0 : 16;
+ } bits;
+};
+
+struct csiphy_format {
+ u32 code;
+ u8 bpp;
+};
+
+struct csi2phy_cfg {
+ unsigned int flags;
+ unsigned char data_lanes[STF_CSI2_MAX_DATA_LANES];
+ unsigned char clock_lane;
+ unsigned char num_data_lanes;
+ bool lane_polarities[1 + STF_CSI2_MAX_DATA_LANES];
+};
+
+struct csi2phy_cfg2 {
+ unsigned char data_lanes[STF_CSI2_MAX_DATA_LANES];
+ unsigned char num_data_lanes;
+ unsigned char num_clks;
+ unsigned char clock_lane;
+ unsigned char clock1_lane;
+ bool lane_polarities[2 + STF_CSI2_MAX_DATA_LANES];
+};
+
+struct stf_csiphy_dev;
+
+struct csiphy_hw_ops {
+ int (*csiphy_clk_enable)(struct stf_csiphy_dev *csiphy_dev);
+ int (*csiphy_clk_disable)(struct stf_csiphy_dev *csiphy_dev);
+ int (*csiphy_config_set)(struct stf_csiphy_dev *csiphy_dev);
+ int (*csiphy_stream_set)(struct stf_csiphy_dev *csiphy_dev, int on);
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+ int (*cdns_csi_power)(struct stf_csiphy_dev *csiphy_dev, int on);
+#endif
+};
+
+struct stf_csiphy_dev {
+ struct stfcamss *stfcamss;
+ struct csi2phy_cfg *csiphy;
+ u8 id;
+ u8 csi_id;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_CSIPHY_PADS_NUM];
+ struct v4l2_mbus_framefmt fmt[STF_CSIPHY_PADS_NUM];
+ const struct csiphy_format *formats;
+ unsigned int nformats;
+ struct csiphy_hw_ops *hw_ops;
+ struct mutex stream_lock;
+ int stream_count;
+};
+
+extern int stf_csiphy_subdev_init(struct stfcamss *stfcamss, int id);
+extern int stf_csiphy_register(struct stf_csiphy_dev *csiphy_dev,
+ struct v4l2_device *v4l2_dev);
+extern int stf_csiphy_unregister(struct stf_csiphy_dev *csiphy_dev);
+
+extern struct csiphy_hw_ops csiphy_ops;
+
+#endif /* STF_CSIPHY_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <linux/sort.h>
+
+static int stf_csiphy_clk_set(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+ struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+ static int init_flag;
+ static struct mutex count_lock;
+ static int count;
+
+ if (!init_flag) {
+ init_flag = 1;
+ mutex_init(&count_lock);
+ }
+ mutex_lock(&count_lock);
+ if (on) {
+ if (count == 0) {
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL,
+ 0x1F, 0x08);
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL,
+ 1 << 31, 1 << 31);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_REFCLK_ISPCORE_2X_CTRL,
+ 0x1F, 0x10);
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_REFCLK_ISPCORE_2X_CTRL,
+ 1 << 31, 1 << 31);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_TXCLKESC_IN_CTRL,
+ 0x3F, 0x28);
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_TXCLKESC_IN_CTRL,
+ 1 << 31, 1 << 31);
+ }
+ count++;
+ } else {
+ if (count == 0)
+ goto exit;
+ if (count == 1) {
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL,
+ 1 << 31, 0 << 31);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_REFCLK_ISPCORE_2X_CTRL,
+ 1 << 31, 0 << 31);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_DPHY_TXCLKESC_IN_CTRL,
+ 1 << 31, 0 << 31);
+ }
+ count--;
+ }
+exit:
+ mutex_unlock(&count_lock);
+ return 0;
+}
+
+static int stf_csiphy_clk_enable(struct stf_csiphy_dev *csiphy_dev)
+{
+ return stf_csiphy_clk_set(csiphy_dev, 1);
+}
+
+static int stf_csiphy_clk_disable(struct stf_csiphy_dev *csiphy_dev)
+{
+ return stf_csiphy_clk_set(csiphy_dev, 0);
+}
+
+static int cmp_func(const void *x1, const void *x2)
+{
+ return *((unsigned char *)x1) - *((unsigned char *)x2);
+}
+
+int try_cfg(struct csi2phy_cfg2 *cfg, struct csi2phy_cfg *cfg0,
+ struct csi2phy_cfg *cfg1)
+{
+ int i = 0;
+
+ if (cfg0 && cfg1) {
+ st_debug(ST_CSIPHY, "CSIPHY use 2 clk mode\n");
+ cfg->num_clks = 2;
+ cfg->num_data_lanes =
+ cfg1->num_data_lanes + cfg0->num_data_lanes;
+ if (cfg->num_data_lanes > STF_CSI2_MAX_DATA_LANES)
+ return -EINVAL;
+ cfg->clock_lane = cfg0->clock_lane;
+ cfg->lane_polarities[0] = cfg0->lane_polarities[0];
+ cfg->clock1_lane = cfg1->clock_lane;
+ cfg->lane_polarities[1] = cfg1->lane_polarities[0];
+ for (i = 0; i < cfg0->num_data_lanes; i++) {
+ cfg->data_lanes[i] = cfg0->data_lanes[i];
+ cfg->lane_polarities[i + 2] =
+ cfg0->lane_polarities[i + 1];
+ }
+
+ for (i = cfg0->num_data_lanes; i < cfg->num_data_lanes; i++) {
+ cfg->data_lanes[i] =
+ cfg1->data_lanes[i - cfg0->num_data_lanes];
+ cfg->lane_polarities[i + 2] =
+ cfg1->lane_polarities[i - cfg0->num_data_lanes + 1];
+ }
+ } else if (cfg0 && !cfg1) {
+ st_debug(ST_CSIPHY, "CSIPHY cfg0 use 1 clk mode\n");
+ cfg->num_clks = 1;
+ cfg->num_data_lanes = cfg0->num_data_lanes;
+ cfg->clock_lane = cfg->clock1_lane = cfg0->clock_lane;
+ cfg->lane_polarities[0] = cfg->lane_polarities[1] =
+ cfg0->lane_polarities[0];
+ for (i = 0; i < cfg0->num_data_lanes; i++) {
+ cfg->data_lanes[i] = cfg0->data_lanes[i];
+ cfg->lane_polarities[i + 2] = cfg0->lane_polarities[i + 1];
+ }
+ } else if (!cfg0 && cfg1) {
+ st_debug(ST_CSIPHY, "CSIPHY cfg1 use 1 clk mode\n");
+ cfg->num_clks = 1;
+ cfg->num_data_lanes = cfg1->num_data_lanes;
+ cfg->clock_lane = cfg->clock1_lane = cfg1->clock_lane;
+ cfg->lane_polarities[0] = cfg->lane_polarities[1] =
+ cfg1->lane_polarities[0];
+ for (i = 0; i < cfg1->num_data_lanes; i++) {
+ cfg->data_lanes[i] = cfg1->data_lanes[i];
+ cfg->lane_polarities[i + 2] = cfg1->lane_polarities[i + 1];
+ }
+ } else {
+ return -EINVAL;
+ }
+
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+ sort(cfg->data_lanes, cfg->num_data_lanes,
+ sizeof(cfg->data_lanes[0]), cmp_func, NULL);
+#endif
+ for (i = 0; i < cfg->num_data_lanes; i++)
+ st_debug(ST_CSIPHY, "%d: %d\n", i, cfg->data_lanes[i]);
+ return 0;
+}
+
+static int csi2rx_dphy_config(struct stf_vin_dev *vin,
+ struct stf_csiphy_dev *csiphy_dev)
+{
+ struct csi2phy_cfg2 cfg2 = {0};
+ struct csi2phy_cfg2 *cfg = &cfg2;
+ struct stf_csiphy_dev *csiphy0_dev =
+ &csiphy_dev->stfcamss->csiphy_dev[0];
+ struct stf_csiphy_dev *csiphy1_dev =
+ &csiphy_dev->stfcamss->csiphy_dev[1];
+ struct csi2phy_cfg *phy0cfg = csiphy0_dev->csiphy;
+ struct csi2phy_cfg *phy1cfg = csiphy1_dev->csiphy;
+ int i;
+ int id = csiphy_dev->id;
+ u32 reg = 0;
+
+ if (!phy0cfg && !phy1cfg)
+ return -EINVAL;
+
+#ifdef USE_CSIDPHY_ONE_CLK_MODE
+ if (id == 0) {
+ phy0cfg = csiphy0_dev->csiphy;
+ phy1cfg = NULL;
+ } else {
+ phy0cfg = NULL;
+ phy1cfg = csiphy1_dev->csiphy;
+ }
+#endif
+
+ if (try_cfg(cfg, phy0cfg, phy1cfg))
+ return -EINVAL;
+
+ id = cfg->num_clks == 2 ? 1 : 0;
+
+ reg = reg_read(vin->sysctrl_base, SYSCTRL_REG4);
+
+ st_debug(ST_CSIPHY, "id = %d, clock_lane = %d, SYSCTRL_REG4: 0x%x\n",
+ id, cfg->clock_lane, reg);
+ st_debug(ST_CSIPHY, "csiphy_dev: csi_id = %d, id = %d\n",
+ csiphy_dev->csi_id, csiphy_dev->id);
+
+ reg = set_bits(reg, id, 0, 0x1);
+ reg = set_bits(reg, cfg->clock_lane, 1, 0x7 << 1);
+ reg = set_bits(reg, cfg->lane_polarities[0], 19, 0x1 << 19);
+ reg = set_bits(reg, cfg->clock1_lane, 4, 0x7 << 4);
+ reg = set_bits(reg, cfg->lane_polarities[1], 20, 0x1 << 20);
+
+ for (i = 0; i < cfg->num_data_lanes; i++) {
+ reg = set_bits(reg, cfg->data_lanes[i], (7 + i * 3),
+ 0x7 << (7 + i * 3));
+
+ reg = set_bits(reg, !!cfg->lane_polarities[i + 2],
+ (21 + i), 0x1 << (21 + i));
+ }
+
+ reg_write(vin->sysctrl_base, SYSCTRL_REG4, reg);
+
+ reg = reg_read(vin->sysctrl_base, SYSCTRL_DPHY_CTRL);
+ for (i = 0; i < cfg->num_data_lanes; i++) {
+ reg = set_bits(reg, 1, (11 + cfg->data_lanes[i]),
+ 0x1 << (11 + cfg->data_lanes[i]));
+ }
+
+ reg_write(vin->sysctrl_base, SYSCTRL_DPHY_CTRL, reg);
+
+ print_reg(ST_CSIPHY, vin->sysctrl_base, SYSCTRL_REG4);
+ print_reg(ST_CSIPHY, vin->sysctrl_base, SYSCTRL_DPHY_CTRL);
+
+ return 0;
+}
+
+static int stf_csiphy_config_set(struct stf_csiphy_dev *csiphy_dev)
+{
+ struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+ st_debug(ST_CSIPHY, "%s, csiphy id = %d\n",
+ __func__, csiphy_dev->id);
+
+ csi2rx_dphy_config(vin, csiphy_dev);
+ return 0;
+}
+
+static int stf_csiphy_stream_set(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+ return 0;
+}
+
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+static int stf_csi_clk_enable(struct stf_csiphy_dev *csiphy_dev)
+{
+ struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+ reg_set_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+
+ if (csiphy_dev->id == 0) {
+ reg_set_bit(vin->clkgen_base,
+ CLK_MIPI_RX0_PXL_CTRL,
+ 0x1F, 0x3);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+ } else {
+ reg_set_bit(vin->clkgen_base,
+ CLK_MIPI_RX1_PXL_CTRL,
+ 0x1F, 0x3);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+ reg_set_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+ }
+
+ return 0;
+}
+
+static int stf_csi_clk_disable(struct stf_csiphy_dev *csiphy_dev)
+{
+ struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+ reg_clr_highest_bit(vin->clkgen_base, CLK_CSI2RX0_APB_CTRL);
+
+ if (csiphy_dev->id == 0) {
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_0_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_1_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_2_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_PXL_3_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX0_SYS0_CTRL);
+ } else {
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_0_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_1_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_2_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_PXL_3_CTRL);
+ reg_clr_highest_bit(vin->clkgen_base, CLK_MIPI_RX1_SYS1_CTRL);
+ }
+
+ return 0;
+}
+
+static int stf_csi_config_set(struct stf_csiphy_dev *csiphy_dev, int is_raw10)
+{
+ struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+ u32 mipi_channel_sel, mipi_vc = 0;
+ enum sensor_type s_type = SENSOR_ISP0;
+
+ switch (s_type) {
+ case SENSOR_VIN:
+ break;
+ case SENSOR_ISP0:
+ reg_set_bit(vin->clkgen_base,
+ CLK_ISP0_MIPI_CTRL,
+ BIT(24), csiphy_dev->id << 24);
+
+ reg_set_bit(vin->clkgen_base,
+ CLK_C_ISP0_CTRL,
+ BIT(25) | BIT(24),
+ csiphy_dev->id << 24);
+
+ mipi_channel_sel = csiphy_dev->id * 4 + mipi_vc;
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_CHAN_SEL,
+ 0xF, mipi_channel_sel);
+
+ if (is_raw10)
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_DW_SEL,
+ BIT(4), 1 << 4);
+ break;
+ case SENSOR_ISP1:
+ reg_set_bit(vin->clkgen_base,
+ CLK_ISP1_MIPI_CTRL,
+ BIT(24), csiphy_dev->id << 24);
+ reg_set_bit(vin->clkgen_base,
+ CLK_C_ISP1_CTRL,
+ BIT(25) | BIT(24),
+ csiphy_dev->id << 24);
+
+ mipi_channel_sel = csiphy_dev->id * 4 + mipi_vc;
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_CHAN_SEL,
+ 0xF << 4, mipi_channel_sel << 4);
+
+ if (is_raw10)
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_SRC_DW_SEL,
+ BIT(5), 1 << 5);
+ default:
+ break;
+ }
+
+ return 0;
+}
+
+static int stf_cdns_csi_power(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+ if (on) {
+ stf_csi_config_set(csiphy_dev, 1);
+ stf_csi_clk_enable(csiphy_dev);
+ } else
+ stf_csi_clk_disable(csiphy_dev);
+
+ return 0;
+}
+#endif
+
+struct csiphy_hw_ops csiphy_ops = {
+ .csiphy_clk_enable = stf_csiphy_clk_enable,
+ .csiphy_clk_disable = stf_csiphy_clk_disable,
+ .csiphy_config_set = stf_csiphy_config_set,
+ .csiphy_stream_set = stf_csiphy_stream_set,
+#ifdef CONFIG_VIDEO_CADENCE_CSI2RX
+ .cdns_csi_power = stf_cdns_csi_power,
+#endif
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.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>
+
+#define STF_DVP_NAME "stf_dvp"
+
+static const struct dvp_format dvp_formats_st7110[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, 16},
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+static int dvp_find_format(u32 code,
+ const struct dvp_format *formats,
+ unsigned int nformats)
+{
+ int i;
+
+ for (i = 0; i < nformats; i++)
+ if (formats[i].code == code)
+ return i;
+ return -EINVAL;
+}
+
+int stf_dvp_subdev_init(struct stfcamss *stfcamss)
+{
+ struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+
+ dvp_dev->s_type = SENSOR_VIN;
+ dvp_dev->hw_ops = &dvp_ops;
+ dvp_dev->stfcamss = stfcamss;
+ dvp_dev->formats = dvp_formats_st7110;
+ dvp_dev->nformats = ARRAY_SIZE(dvp_formats_st7110);
+ mutex_init(&dvp_dev->stream_lock);
+ dvp_dev->stream_count = 0;
+ return 0;
+}
+
+static int dvp_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__dvp_get_format(struct stf_dvp_dev *dvp_dev,
+ struct v4l2_subdev_pad_config *cfg,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_subdev_state *state;
+ state = kzalloc(sizeof(*state), GFP_KERNEL);
+ if (!state)
+ return ERR_PTR(-ENOMEM);
+
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(
+ &dvp_dev->subdev, state, pad);
+ return &dvp_dev->fmt[pad];
+}
+
+static int dvp_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+ int ret = 0;
+
+ format = __dvp_get_format(dvp_dev, NULL, STF_DVP_PAD_SRC,
+ V4L2_SUBDEV_FORMAT_ACTIVE);
+ if (format == NULL)
+ return -EINVAL;
+ ret = dvp_find_format(format->code,
+ dvp_dev->formats,
+ dvp_dev->nformats);
+ if (ret < 0)
+ return ret;
+
+ mutex_lock(&dvp_dev->stream_lock);
+ if (enable) {
+ if (dvp_dev->stream_count == 0) {
+ dvp_dev->hw_ops->dvp_clk_init(dvp_dev);
+ dvp_dev->hw_ops->dvp_config_set(dvp_dev);
+ dvp_dev->hw_ops->dvp_set_format(dvp_dev,
+ format->width, dvp_dev->formats[ret].bpp);
+ dvp_dev->hw_ops->dvp_stream_set(dvp_dev, 1);
+ }
+ dvp_dev->stream_count++;
+ } else {
+ if (dvp_dev->stream_count == 0)
+ goto exit;
+ if (dvp_dev->stream_count == 1)
+ dvp_dev->hw_ops->dvp_stream_set(dvp_dev, 0);
+ dvp_dev->stream_count--;
+ }
+exit:
+ mutex_unlock(&dvp_dev->stream_lock);
+ return 0;
+}
+
+static void dvp_try_format(struct stf_dvp_dev *dvp_dev,
+ struct v4l2_subdev_pad_config *cfg,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ unsigned int i;
+
+ switch (pad) {
+ case STF_DVP_PAD_SINK:
+ /* Set format on sink pad */
+
+ for (i = 0; i < dvp_dev->nformats; i++)
+ if (fmt->code == dvp_dev->formats[i].code)
+ break;
+
+ if (i >= dvp_dev->nformats)
+ fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+ fmt->width = clamp_t(u32,
+ fmt->width, 1, STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height, 1, STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ break;
+
+ case STF_DVP_PAD_SRC:
+
+ *fmt = *__dvp_get_format(dvp_dev, cfg, STF_DVP_PAD_SINK, which);
+
+ break;
+ }
+}
+
+static int dvp_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+
+ if (code->index >= dvp_dev->nformats)
+ return -EINVAL;
+
+ if (code->pad == STF_DVP_PAD_SINK) {
+ code->code = dvp_dev->formats[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __dvp_get_format(dvp_dev, state->pads, STF_DVP_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+
+ return 0;
+}
+
+static int dvp_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ dvp_try_format(dvp_dev, state->pads, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ dvp_try_format(dvp_dev, state->pads, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int dvp_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __dvp_get_format(dvp_dev, state->pads, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int dvp_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_dvp_dev *dvp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+ int ret;
+
+ format = __dvp_get_format(dvp_dev, state->pads, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ dvp_try_format(dvp_dev, state->pads, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_DVP_PAD_SINK) {
+ format = __dvp_get_format(dvp_dev, state->pads, STF_DVP_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ dvp_try_format(dvp_dev, state->pads, STF_DVP_PAD_SRC, format,
+ fmt->which);
+ }
+
+ return 0;
+}
+
+static int dvp_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_DVP_PAD_SINK,
+ .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
+ V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return dvp_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int dvp_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
+ (flags & MEDIA_LNK_FL_ENABLED)) {
+ struct v4l2_subdev *sd;
+ struct stf_dvp_dev *dvp_dev;
+ struct vin_line *line;
+
+ if (media_entity_remote_pad(local))
+ return -EBUSY;
+
+ sd = media_entity_to_v4l2_subdev(entity);
+ dvp_dev = v4l2_get_subdevdata(sd);
+
+ sd = media_entity_to_v4l2_subdev(remote->entity);
+ line = v4l2_get_subdevdata(sd);
+ if (line->sdev_type == VIN_DEV_TYPE)
+ dvp_dev->s_type = SENSOR_VIN;
+ if (line->sdev_type == ISP0_DEV_TYPE)
+ dvp_dev->s_type = SENSOR_ISP0;
+ if (line->sdev_type == ISP1_DEV_TYPE)
+ dvp_dev->s_type = SENSOR_ISP1;
+ st_info(ST_DVP, "DVP device sensor type: %d\n", dvp_dev->s_type);
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops dvp_core_ops = {
+ .s_power = dvp_set_power,
+};
+
+static const struct v4l2_subdev_video_ops dvp_video_ops = {
+ .s_stream = dvp_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops dvp_pad_ops = {
+ .enum_mbus_code = dvp_enum_mbus_code,
+ .enum_frame_size = dvp_enum_frame_size,
+ .get_fmt = dvp_get_format,
+ .set_fmt = dvp_set_format,
+};
+
+static const struct v4l2_subdev_ops dvp_v4l2_ops = {
+ .core = &dvp_core_ops,
+ .video = &dvp_video_ops,
+ .pad = &dvp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops dvp_v4l2_internal_ops = {
+ .open = dvp_init_formats,
+};
+
+static const struct media_entity_operations dvp_media_ops = {
+ .link_setup = dvp_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_dvp_register(struct stf_dvp_dev *dvp_dev,
+ struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd = &dvp_dev->subdev;
+ struct device *dev = dvp_dev->stfcamss->dev;
+ struct media_pad *pads = dvp_dev->pads;
+ int ret;
+
+ v4l2_subdev_init(sd, &dvp_v4l2_ops);
+ sd->internal_ops = &dvp_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+ STF_DVP_NAME, dvp_dev->id);
+ v4l2_set_subdevdata(sd, dvp_dev);
+
+ ret = dvp_init_formats(sd, NULL);
+ if (ret < 0) {
+ st_err(ST_DVP, "Failed to init format: %d\n", ret);
+ return ret;
+ }
+
+ pads[STF_DVP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_DVP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &dvp_media_ops;
+ ret = media_entity_pads_init(&sd->entity, STF_DVP_PADS_NUM, pads);
+ if (ret < 0) {
+ st_err(ST_DVP, "Failed to init media entity: %d\n", ret);
+ return ret;
+ }
+
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ st_err(ST_DVP, "Failed to register subdev: %d\n", ret);
+ goto err_sreg;
+ }
+
+ return 0;
+
+err_sreg:
+ media_entity_cleanup(&sd->entity);
+ return ret;
+}
+
+int stf_dvp_unregister(struct stf_dvp_dev *dvp_dev)
+{
+ v4l2_device_unregister_subdev(&dvp_dev->subdev);
+ media_entity_cleanup(&dvp_dev->subdev.entity);
+ mutex_destroy(&dvp_dev->stream_lock);
+ return 0;
+}
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_DVP_H
+#define STF_DVP_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_DVP_PAD_SINK 0
+#define STF_DVP_PAD_SRC 1
+#define STF_DVP_PADS_NUM 2
+
+struct dvp_format {
+ u32 code;
+ u8 bpp;
+};
+
+enum sensor_type;
+enum subdev_type;
+
+struct dvp_cfg {
+ unsigned int flags;
+ unsigned char bus_width;
+ unsigned char data_shift;
+};
+
+struct stf_dvp_dev;
+
+struct dvp_hw_ops {
+ int (*dvp_clk_init)(struct stf_dvp_dev *dvp_dev);
+ int (*dvp_config_set)(struct stf_dvp_dev *dvp_dev);
+ int (*dvp_set_format)(struct stf_dvp_dev *dvp_dev,
+ u32 pix_width, u8 bpp);
+ int (*dvp_stream_set)(struct stf_dvp_dev *dvp_dev, int on);
+};
+
+struct stf_dvp_dev {
+ struct stfcamss *stfcamss;
+ struct dvp_cfg *dvp;
+ u8 id;
+ enum sensor_type s_type;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_DVP_PADS_NUM];
+ struct v4l2_mbus_framefmt fmt[STF_DVP_PADS_NUM];
+ const struct dvp_format *formats;
+ unsigned int nformats;
+ struct dvp_hw_ops *hw_ops;
+ struct mutex stream_lock;
+ int stream_count;
+};
+
+extern int stf_dvp_subdev_init(struct stfcamss *stfcamss);
+extern int stf_dvp_register(struct stf_dvp_dev *dvp_dev,
+ struct v4l2_device *v4l2_dev);
+extern int stf_dvp_unregister(struct stf_dvp_dev *dvp_dev);
+
+extern struct dvp_hw_ops dvp_ops;
+
+#endif /* STF_DVP_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+
+static int stf_dvp_clk_init(struct stf_dvp_dev *dvp_dev)
+{
+ return 0;
+}
+
+#define U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR 0x13040000
+void reg_phy_write(uint32_t addr,uint32_t reg,uint32_t val)
+{
+ uint32_t tmp;
+
+ iowrite32(val, ioremap(addr + reg, 4));
+}
+
+void stf_dvp_io_pad_config(struct stf_vin_dev *vin)
+{
+ /*
+ * pin: 21 ~ 35
+ * offset: 0x144 ~ 0x164
+ * SCFG_funcshare_pad_ctrl
+ */
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0174U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x10);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x200000);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0178U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x90);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x240000);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x017cU, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x490);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x248000);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0180U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x02a0);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x800);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0184U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x12490);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x100800);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0188U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x92490);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x900800);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x018cU, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x492490);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x4900800);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0190U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x2492490);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x24900800);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0194U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a0U, 0x12492490);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x248001);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x0198U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a4U, 0x2);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x248009);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x019cU, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a4U, 0x12);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x248049);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x01a0U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a4U, 0x92);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x248249);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x01a4U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a4U, 0x492);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b4U, 0x249249);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x01a8U, 0x1);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a4U, 0x2492);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x24904800);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x01acU, 0x11);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02a4U, 0x12492);
+ reg_phy_write(U0_SYS_IOMUX__SAIF_BD_APBS__BASE_ADDR, 0x02b0U, 0x24924800);
+
+}
+
+static int stf_dvp_config_set(struct stf_dvp_dev *dvp_dev)
+{
+
+ struct stf_vin_dev *vin = dvp_dev->stfcamss->vin;
+ unsigned int flags = 0;
+ unsigned char data_shift = 0;
+ u32 polarities = 0;
+
+ if (!dvp_dev->dvp)
+ return -EINVAL;
+
+ flags = dvp_dev->dvp->flags;
+ data_shift = dvp_dev->dvp->data_shift;
+ st_info(ST_DVP, "%s, polarities = 0x%x, flags = 0x%x\n",
+ __func__, polarities, flags);
+
+ stf_dvp_io_pad_config(vin);
+
+ if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+ polarities |= BIT(1);
+ if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+ polarities |= BIT(3);
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCONSAIF_SYSCFG_36);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ V4L2_MBUS_HSYNC_ACTIVE_HIGH
+ | V4L2_MBUS_VSYNC_ACTIVE_HIGH,
+ polarities);
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCONSAIF_SYSCFG_36);
+
+ switch (data_shift) {
+ case 0:
+ data_shift = 0;
+ break;
+ case 2:
+ data_shift = 1;
+ break;
+ case 4:
+ data_shift = 2;
+ break;
+ case 6:
+ data_shift = 3;
+ break;
+ default:
+ data_shift = 0;
+ break;
+ };
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ UO_VIN_CNFG_AXIWR0_PIXEL_HEIGH_BIT_SEL, data_shift << 15);
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+
+ return 0;
+}
+
+static int set_vin_axiwr_pix_ct(struct stf_vin_dev *vin, u8 bpp)
+{
+ u32 value = 0;
+ int cnfg_axiwr_pix_ct = 64 / bpp;
+
+ // need check
+ if (cnfg_axiwr_pix_ct == 2)
+ value = 1;
+ else if (cnfg_axiwr_pix_ct == 4)
+ value = 2;
+ else if (cnfg_axiwr_pix_ct == 8)
+ value = 0;
+ else
+ return 0;
+
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_RW_CTRL, BIT(1) | BIT(0), value);
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_RW_CTRL);
+
+ return cnfg_axiwr_pix_ct;
+
+}
+
+static int stf_dvp_set_format(struct stf_dvp_dev *dvp_dev,
+ u32 pix_width, u8 bpp)
+{
+ struct stf_vin_dev *vin = dvp_dev->stfcamss->vin;
+ int val, pix_ct;
+
+ if (dvp_dev->s_type == SENSOR_VIN) {
+ pix_ct = set_vin_axiwr_pix_ct(vin, bpp);
+ val = (pix_width / pix_ct) - 1;
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL);
+ reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL, val);
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCTRL_VIN_WR_PIX_TOTAL);
+
+ }
+
+ return 0;
+}
+
+static int stf_dvp_stream_set(struct stf_dvp_dev *dvp_dev, int on)
+{
+ struct stf_vin_dev *vin = dvp_dev->stfcamss->vin;
+
+ switch (dvp_dev->s_type) {
+ case SENSOR_VIN:
+#if 0
+ /*View segment register (2.7.4.1.ispv2 ) is vin RD WR related (data stream not
+ through ISP, directly to DDR, or take data from DDR) 2021 1110*/
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_0, U0_VIN_CNFG_AXIRD_AXI_CNT_END, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_4, U0_VIN_CNFG_AXIRD_END_ADDR, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_8, U0_VIN_CNFG_AXIRD_LINE_CNT_END, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_8, U0_VIN_CNFG_AXIRD_LINE_CNT_START, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_12, U0_VIN_CNFG_AXIRD_PIX_CNT_END
+ | U0_VIN_CNFG_AXIRD_PIX_CNT_START
+ | U0_VIN_CNFG_AXIRD_PIX_CT, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_16, U0_VIN_CNFG_AXIRD_START_ADDR, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_20, U0_VIN_CNFG_AXIWR0_CHANNEL_SEL
+ | U0_VIN_CNFG_AXIWR0_EN, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_24, U0_VIN_CNFG_AXIWR0_END_ADDR, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28, U0_VIN_CNFG_AXIWR0_PIXEL_HITH_BIT_SEL
+ | U0_VIN_CNFG_AXIWR0_PIX_CNT_CNT_END
+ | U0_VIN_CNFG_AXIWR0_PIX_CNT_CT
+ | SYSCONSAIF_SYSCFG_28, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_32, U0_VIN_CNFG_AXIWR0_START_ADDR, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_8, U0_VIN_CNFG_AXIRD_INTR_MASK, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_0, U0_VIN_CNFG_AXI_DVP_EN, 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36, U0_VIN_CNFG_COLOR_BAR_EN, 0x0);
+#endif
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_AXI_CTRL, BIT(0), on);
+ reg_set_bit(vin->clkgen_base,
+ CLK_VIN_AXI_WR_CTRL,
+ BIT(25) | BIT(24), 2 << 24);
+ break;
+ case SENSOR_ISP0:
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(U0_VIN_CNFG_ISP_DVP_EN0),
+ !!on<<U0_VIN_CNFG_ISP_DVP_EN0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(U0_VIN_CNFG_DVP_SWAP_EN), 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(U0_VIN_CNFG_GEN_EN_AXIRD), 0x0);
+ break;
+ case SENSOR_ISP1:
+ st_err(ST_DVP, "please check dvp_dev s_type:%d\n", dvp_dev->s_type);
+ break;
+ default:
+ break;
+ }
+
+ return 0;
+}
+
+struct dvp_hw_ops dvp_ops = {
+ .dvp_clk_init = stf_dvp_clk_init,
+ .dvp_config_set = stf_dvp_config_set,
+ .dvp_set_format = stf_dvp_set_format,
+ .dvp_stream_set = stf_dvp_stream_set,
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Event for VIC Video In
+ *
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <linux/notifier.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+
+static ATOMIC_NOTIFIER_HEAD(vin_notifier_list);
+
+int vin_notifier_register(struct notifier_block *nb)
+{
+ return atomic_notifier_chain_register(&vin_notifier_list, nb);
+}
+EXPORT_SYMBOL_GPL(vin_notifier_register);
+
+void vin_notifier_unregister(struct notifier_block *nb)
+{
+ atomic_notifier_chain_unregister(&vin_notifier_list, nb);
+}
+EXPORT_SYMBOL_GPL(vin_notifier_unregister);
+
+int vin_notifier_call(unsigned long e, void *v)
+{
+ return atomic_notifier_call_chain(&vin_notifier_list, e, v);
+}
+EXPORT_SYMBOL_GPL(vin_notifier_call);
+
+MODULE_AUTHOR("StarFive Technology Co., Ltd.");
+MODULE_DESCRIPTION("Starfive VIC video in notifier");
+MODULE_LICENSE("GPL");
+//MODULE_SUPPORTED_DEVICE("video");
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <media/v4l2-async.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 <linux/firmware.h>
+
+#define STF_ISP_NAME "stf_isp"
+
+static const struct isp_format isp_formats_st7110[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, 16},
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+int stf_isp_subdev_init(struct stfcamss *stfcamss, int id)
+{
+ struct stf_isp_dev *isp_dev = &stfcamss->isp_dev[id];
+
+ atomic_set(&isp_dev->ref_count, 0);
+ isp_dev->sdev_type = id == 0 ? ISP0_DEV_TYPE : ISP1_DEV_TYPE;
+ isp_dev->id = id;
+ isp_dev->hw_ops = &isp_ops;
+ isp_dev->stfcamss = stfcamss;
+ isp_dev->formats = isp_formats_st7110;
+ isp_dev->nformats = ARRAY_SIZE(isp_formats_st7110);
+ mutex_init(&isp_dev->stream_lock);
+ mutex_init(&isp_dev->setfile_lock);
+ return 0;
+}
+
+/*
+ * ISP Controls.
+ */
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+ return &container_of(ctrl->handler, struct stf_isp_dev,
+ ctrls.handler)->subdev;
+}
+
+static u64 isp_calc_pixel_rate(struct stf_isp_dev *isp_dev)
+{
+ u64 rate = 0;
+
+ return rate;
+}
+
+static int isp_set_ctrl_hue(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_set_ctrl_contrast(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_set_ctrl_saturation(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_set_ctrl_white_balance(struct stf_isp_dev *isp_dev, int awb)
+{
+ struct isp_ctrls *ctrls = &isp_dev->ctrls;
+ int ret = 0;
+
+ if (!awb && (ctrls->red_balance->is_new
+ || ctrls->blue_balance->is_new)) {
+ u16 red = (u16)ctrls->red_balance->val;
+ u16 blue = (u16)ctrls->blue_balance->val;
+
+ st_debug(ST_ISP, "red = 0x%x, blue = 0x%x\n", red, blue);
+ // isp_dev->hw_ops->isp_set_awb_r_gain(isp_dev, red);
+ // if (ret)
+ // return ret;
+ // isp_dev->hw_ops->isp_set_awb_b_gain(isp_dev, blue);
+ }
+
+ return ret;
+}
+
+static int isp_set_ctrl_exposure(struct stf_isp_dev *isp_dev,
+ enum v4l2_exposure_auto_type auto_exposure)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_set_ctrl_gain(struct stf_isp_dev *isp_dev, bool auto_gain)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static const char * const test_pattern_menu[] = {
+ "Disabled",
+ "Color bars",
+ "Color bars w/ rolling bar",
+ "Color squares",
+ "Color squares w/ rolling bar",
+};
+
+#define ISP_TEST_ENABLE BIT(7)
+#define ISP_TEST_ROLLING BIT(6) /* rolling horizontal bar */
+#define ISP_TEST_TRANSPARENT BIT(5)
+#define ISP_TEST_SQUARE_BW BIT(4) /* black & white squares */
+#define ISP_TEST_BAR_STANDARD (0 << 2)
+#define ISP_TEST_BAR_VERT_CHANGE_1 (1 << 2)
+#define ISP_TEST_BAR_HOR_CHANGE (2 << 2)
+#define ISP_TEST_BAR_VERT_CHANGE_2 (3 << 2)
+#define ISP_TEST_BAR (0 << 0)
+#define ISP_TEST_RANDOM (1 << 0)
+#define ISP_TEST_SQUARE (2 << 0)
+#define ISP_TEST_BLACK (3 << 0)
+
+static const u8 test_pattern_val[] = {
+ 0,
+ ISP_TEST_ENABLE | ISP_TEST_BAR_VERT_CHANGE_1 |
+ ISP_TEST_BAR,
+ ISP_TEST_ENABLE | ISP_TEST_ROLLING |
+ ISP_TEST_BAR_VERT_CHANGE_1 | ISP_TEST_BAR,
+ ISP_TEST_ENABLE | ISP_TEST_SQUARE,
+ ISP_TEST_ENABLE | ISP_TEST_ROLLING | ISP_TEST_SQUARE,
+};
+
+static int isp_set_ctrl_test_pattern(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ // return isp_write_reg(isp_dev, ISP_REG_PRE_ISP_TEST_SET1,
+ // test_pattern_val[value]);
+ return ret;
+}
+
+static int isp_set_ctrl_light_freq(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_set_ctrl_hflip(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_set_ctrl_vflip(struct stf_isp_dev *isp_dev, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int isp_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ int val;
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ break;
+ case V4L2_CID_EXPOSURE_AUTO:
+ break;
+ }
+
+ return 0;
+}
+
+static int isp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ int ret;
+
+ /*
+ * If the device is not powered up by the host driver do
+ * not apply any controls to H/W at this time. Instead
+ * the controls will be restored right after power-up.
+ */
+ if (!atomic_read(&isp_dev->ref_count))
+ return 0;
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ ret = isp_set_ctrl_gain(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_EXPOSURE_AUTO:
+ ret = isp_set_ctrl_exposure(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_AUTO_WHITE_BALANCE:
+ ret = isp_set_ctrl_white_balance(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ ret = isp_set_ctrl_hue(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_CONTRAST:
+ ret = isp_set_ctrl_contrast(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ ret = isp_set_ctrl_saturation(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN:
+ ret = isp_set_ctrl_test_pattern(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_POWER_LINE_FREQUENCY:
+ ret = isp_set_ctrl_light_freq(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ ret = isp_set_ctrl_hflip(isp_dev, ctrl->val);
+ break;
+ case V4L2_CID_VFLIP:
+ ret = isp_set_ctrl_vflip(isp_dev, ctrl->val);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+ return ret;
+}
+
+static const struct v4l2_ctrl_ops isp_ctrl_ops = {
+ .g_volatile_ctrl = isp_g_volatile_ctrl,
+ .s_ctrl = isp_s_ctrl,
+};
+
+static int isp_init_controls(struct stf_isp_dev *isp_dev)
+{
+ const struct v4l2_ctrl_ops *ops = &isp_ctrl_ops;
+ struct isp_ctrls *ctrls = &isp_dev->ctrls;
+ struct v4l2_ctrl_handler *hdl = &ctrls->handler;
+ int ret;
+
+ v4l2_ctrl_handler_init(hdl, 32);
+
+ /* Clock related controls */
+ ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+ 0, INT_MAX, 1,
+ isp_calc_pixel_rate(isp_dev));
+
+ /* Auto/manual white balance */
+ ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
+ V4L2_CID_AUTO_WHITE_BALANCE,
+ 0, 1, 1, 1);
+ ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
+ 0, 4095, 1, 0);
+ ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
+ 0, 4095, 1, 0);
+ /* Auto/manual exposure */
+ ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_EXPOSURE_AUTO,
+ V4L2_EXPOSURE_MANUAL, 0,
+ V4L2_EXPOSURE_AUTO);
+ ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+ 0, 65535, 1, 0);
+ /* Auto/manual gain */
+ ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
+ 0, 1, 1, 1);
+ ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
+ 0, 1023, 1, 0);
+
+ ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
+ 0, 255, 1, 64);
+ ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
+ 0, 359, 1, 0);
+ ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
+ 0, 255, 1, 0);
+ ctrls->test_pattern =
+ v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+ ARRAY_SIZE(test_pattern_menu) - 1,
+ 0, 0, test_pattern_menu);
+ ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+ 0, 1, 1, 0);
+ ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+ 0, 1, 1, 0);
+
+ ctrls->light_freq =
+ v4l2_ctrl_new_std_menu(hdl, ops,
+ V4L2_CID_POWER_LINE_FREQUENCY,
+ V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+ V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+ if (hdl->error) {
+ ret = hdl->error;
+ goto free_ctrls;
+ }
+
+ ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+ ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
+ ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
+
+ v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
+ v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
+ v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
+
+ isp_dev->subdev.ctrl_handler = hdl;
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(hdl);
+ return ret;
+}
+
+static int isp_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+ if (on)
+ atomic_inc(&isp_dev->ref_count);
+ else
+ atomic_dec(&isp_dev->ref_count);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__isp_get_format(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_pad_config *cfg,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_subdev_state *state;
+
+ state = kzalloc(sizeof(*state), GFP_KERNEL);
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(&isp_dev->subdev, state, pad);
+
+ return &isp_dev->fmt[pad];
+}
+
+static int isp_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ int ret = 0;
+ struct v4l2_mbus_framefmt *fmt;
+
+ fmt = __isp_get_format(isp_dev, NULL, STF_ISP_PAD_SINK, V4L2_SUBDEV_FORMAT_ACTIVE);
+ mutex_lock(&isp_dev->stream_lock);
+ if (enable) {
+ if (isp_dev->stream_count == 0) {
+ isp_dev->hw_ops->isp_clk_enable(isp_dev);
+ isp_dev->hw_ops->isp_reset(isp_dev);
+ isp_dev->hw_ops->isp_set_format(isp_dev,
+ &isp_dev->crop, fmt->code);
+ // format->width, format->height);
+ isp_dev->hw_ops->isp_config_set(isp_dev);
+ isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+ }
+ isp_dev->stream_count++;
+ } else {
+ if (isp_dev->stream_count == 0)
+ goto exit;
+ if (isp_dev->stream_count == 1) {
+ isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+ isp_dev->hw_ops->isp_clk_disable(isp_dev);
+ }
+ isp_dev->stream_count--;
+ }
+exit:
+ mutex_unlock(&isp_dev->stream_lock);
+
+ if (enable && atomic_read(&isp_dev->ref_count) == 1) {
+ /* restore controls */
+ ret = v4l2_ctrl_handler_setup(&isp_dev->ctrls.handler);
+ }
+
+ return ret;
+}
+
+static void isp_try_format(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_pad_config *cfg,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ unsigned int i;
+
+ switch (pad) {
+ case STF_ISP_PAD_SINK:
+ /* Set format on sink pad */
+
+ for (i = 0; i < isp_dev->nformats; i++)
+ if (fmt->code == isp_dev->formats[i].code)
+ break;
+
+ if (i >= isp_dev->nformats)
+ fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+ fmt->width = clamp_t(u32,
+ fmt->width, 8, STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->width &= ~0x7 ;
+ fmt->height = clamp_t(u32,
+ fmt->height, 1, STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ break;
+
+ case STF_ISP_PAD_SRC:
+
+ *fmt = *__isp_get_format(isp_dev, cfg, STF_ISP_PAD_SINK, which);
+
+ break;
+ }
+}
+
+static int isp_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+ if (code->index >= isp_dev->nformats)
+ return -EINVAL;
+ if (code->pad == STF_ISP_PAD_SINK) {
+ code->code = isp_dev->formats[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __isp_get_format(isp_dev, state->pads, STF_ISP_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+
+ return 0;
+}
+
+static int isp_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ isp_try_format(isp_dev, state->pads, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ isp_try_format(isp_dev, state->pads, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int isp_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __isp_get_format(isp_dev, state->pads, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int isp_set_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel);
+
+static int isp_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __isp_get_format(isp_dev, state->pads, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ isp_try_format(isp_dev, state->pads, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_ISP_PAD_SINK) {
+ struct v4l2_subdev_selection sel = { 0 };
+ int ret;
+
+ format = __isp_get_format(isp_dev, state->pads, STF_ISP_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ isp_try_format(isp_dev, state->pads, STF_ISP_PAD_SRC, format,
+ fmt->which);
+
+ /* Reset sink pad compose selection */
+ sel.which = fmt->which;
+ sel.pad = STF_ISP_PAD_SINK;
+ sel.target = V4L2_SEL_TGT_COMPOSE;
+ sel.r.width = fmt->format.width;
+ sel.r.height = fmt->format.height;
+ ret = isp_set_selection(sd, state, &sel);
+ if (ret < 0)
+ return ret;
+
+ }
+
+ return 0;
+}
+
+static struct v4l2_rect *
+__isp_get_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
+ STF_ISP_PAD_SINK);
+
+ return &isp_dev->compose;
+}
+
+static struct v4l2_rect *
+__isp_get_crop(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_crop(&isp_dev->subdev, state,
+ STF_ISP_PAD_SRC);
+
+ return &isp_dev->crop;
+}
+
+static void isp_try_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_mbus_framefmt *fmt;
+
+ fmt = __isp_get_format(isp_dev, cfg, STF_ISP_PAD_SINK, which);
+
+ if (rect->width > fmt->width)
+ rect->width = fmt->width;
+
+ if (rect->height > fmt->height)
+ rect->height = fmt->height;
+
+ if (fmt->width > rect->width * SCALER_RATIO_MAX)
+ rect->width = (fmt->width + SCALER_RATIO_MAX - 1) /
+ SCALER_RATIO_MAX;
+
+ rect->width &= ~0x7;
+
+ if (fmt->height > rect->height * SCALER_RATIO_MAX)
+ rect->height = (fmt->height + SCALER_RATIO_MAX - 1) /
+ SCALER_RATIO_MAX;
+
+ if (rect->width < 16)
+ rect->width = 16;
+
+ if (rect->height < 4)
+ rect->height = 4;
+}
+
+static void isp_try_crop(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_rect *compose;
+
+ compose = __isp_get_compose(isp_dev, state, which);
+
+ if (rect->width > compose->width)
+ rect->width = compose->width;
+
+ if (rect->width + rect->left > compose->width)
+ rect->left = compose->width - rect->width;
+
+ if (rect->height > compose->height)
+ rect->height = compose->height;
+
+ if (rect->height + rect->top > compose->height)
+ rect->top = compose->height - rect->height;
+
+ // /* isp in line based mode writes multiple of 16 horizontally */
+ rect->left &= ~0x1;
+ rect->top &= ~0x1;
+ rect->width &= ~0x7;
+
+ if (rect->width < 16) {
+ rect->left = 0;
+ rect->width = 16;
+ }
+
+ if (rect->height < 4) {
+ rect->top = 0;
+ rect->height = 4;
+ }
+}
+
+static int isp_get_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_subdev_format fmt = { 0 };
+ struct v4l2_rect *rect;
+ int ret;
+
+ switch (sel->target) {
+ case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+ case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+ fmt.pad = sel->pad;
+ fmt.which = sel->which;
+ ret = isp_get_format(sd, state, &fmt);
+ if (ret < 0)
+ return ret;
+
+ sel->r.left = 0;
+ sel->r.top = 0;
+ sel->r.width = fmt.format.width;
+ sel->r.height = fmt.format.height;
+ break;
+ case V4L2_SEL_TGT_COMPOSE:
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ sel->r = *rect;
+ break;
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ sel->r.left = rect->left;
+ sel->r.top = rect->top;
+ sel->r.width = rect->width;
+ sel->r.height = rect->height;
+ break;
+ case V4L2_SEL_TGT_CROP:
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ sel->r = *rect;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ st_info(ST_ISP, "get left = %d, %d, %d, %d\n",
+ sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+ return 0;
+}
+
+static int isp_set_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_rect *rect;
+ int ret;
+
+ st_info(ST_ISP, "left = %d, %d, %d, %d\n",
+ sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+ if (sel->target == V4L2_SEL_TGT_COMPOSE) {
+ struct v4l2_subdev_selection crop = { 0 };
+
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ isp_try_compose(isp_dev, state->pads, &sel->r, sel->which);
+ *rect = sel->r;
+
+ /* Reset source crop selection */
+ crop.which = sel->which;
+ crop.pad = STF_ISP_PAD_SRC;
+ crop.target = V4L2_SEL_TGT_CROP;
+ crop.r = *rect;
+ ret = isp_set_selection(sd, state, &crop);
+ } else if (sel->target == V4L2_SEL_TGT_CROP) {
+ struct v4l2_subdev_format fmt = { 0 };
+
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ isp_try_crop(isp_dev, state, &sel->r, sel->which);
+
+ *rect = sel->r;
+
+ /* Reset source pad format width and height */
+ fmt.which = sel->which;
+ fmt.pad = STF_ISP_PAD_SRC;
+ ret = isp_get_format(sd, state, &fmt);
+ if (ret < 0)
+ return ret;
+
+ fmt.format.width = rect->width;
+ fmt.format.height = rect->height;
+ ret = isp_set_format(sd, state, &fmt);
+ } else {
+ ret = -EINVAL;
+ }
+
+ st_info(ST_ISP, "out left = %d, %d, %d, %d\n",
+ sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+ return ret;
+}
+
+static int isp_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_ISP_PAD_SINK,
+ .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
+ V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return isp_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int isp_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if (flags & MEDIA_LNK_FL_ENABLED)
+ if (media_entity_remote_pad(local))
+ return -EBUSY;
+ return 0;
+}
+
+static int stf_isp_load_setfile(struct stf_isp_dev *isp_dev, char *file_name)
+{
+ struct device *dev = isp_dev->stfcamss->dev;
+ const struct firmware *fw;
+ u8 *buf = NULL;
+ int *regval_num;
+ int ret;
+
+ st_debug(ST_ISP, "%s, file_name %s\n", __func__, file_name);
+ ret = request_firmware(&fw, file_name, dev);
+ if (ret < 0) {
+ st_err(ST_ISP, "firmware request failed (%d)\n", ret);
+ return ret;
+ }
+ buf = devm_kzalloc(dev, fw->size, GFP_KERNEL);
+ if (!buf)
+ return -ENOMEM;
+ memcpy(buf, fw->data, fw->size);
+
+ mutex_lock(&isp_dev->setfile_lock);
+ if (isp_dev->setfile.state == 1)
+ devm_kfree(dev, isp_dev->setfile.data);
+ isp_dev->setfile.data = buf;
+ isp_dev->setfile.size = fw->size;
+ isp_dev->setfile.state = 1;
+ regval_num = (int *)&buf[fw->size - sizeof(unsigned int)];
+ isp_dev->setfile.settings.regval_num = *regval_num;
+ isp_dev->setfile.settings.regval = (regval_t *)buf;
+ mutex_unlock(&isp_dev->setfile_lock);
+
+ st_debug(ST_ISP, "stf_isp setfile loaded size: %zu B, reg_nul: %d\n",
+ fw->size, isp_dev->setfile.settings.regval_num);
+
+ release_firmware(fw);
+ return ret;
+}
+
+static long stf_isp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ int ret = -ENOIOCTLCMD;
+
+ switch (cmd) {
+ case VIDIOC_STFISP_LOAD_FW: {
+ struct stfisp_fw_info *fw_info = arg;
+
+ if (IS_ERR(fw_info)) {
+ st_err(ST_ISP, "fw_info failed, params invaild\n");
+ return -EINVAL;
+ }
+
+ ret = stf_isp_load_setfile(isp_dev, fw_info->filename);
+ break;
+ }
+ default:
+ break;
+ }
+ return ret;
+}
+
+static const struct v4l2_subdev_core_ops isp_core_ops = {
+ .s_power = isp_set_power,
+ .ioctl = stf_isp_ioctl,
+ .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 isp_video_ops = {
+ .s_stream = isp_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops isp_pad_ops = {
+ .enum_mbus_code = isp_enum_mbus_code,
+ .enum_frame_size = isp_enum_frame_size,
+ .get_fmt = isp_get_format,
+ .set_fmt = isp_set_format,
+ .get_selection = isp_get_selection,
+ .set_selection = isp_set_selection,
+};
+
+static const struct v4l2_subdev_ops isp_v4l2_ops = {
+ .core = &isp_core_ops,
+ .video = &isp_video_ops,
+ .pad = &isp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops isp_v4l2_internal_ops = {
+ .open = isp_init_formats,
+};
+
+static const struct media_entity_operations isp_media_ops = {
+ .link_setup = isp_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_isp_register(struct stf_isp_dev *isp_dev,
+ struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd = &isp_dev->subdev;
+ struct device *dev = isp_dev->stfcamss->dev;
+ struct media_pad *pads = isp_dev->pads;
+ int ret;
+
+ v4l2_subdev_init(sd, &isp_v4l2_ops);
+ sd->internal_ops = &isp_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
+ STF_ISP_NAME, isp_dev->id);
+ v4l2_set_subdevdata(sd, isp_dev);
+
+ ret = isp_init_formats(sd, NULL);
+ if (ret < 0) {
+ st_err(ST_ISP, "Failed to init format: %d\n", ret);
+ return ret;
+ }
+
+ pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &isp_media_ops;
+ ret = media_entity_pads_init(&sd->entity, STF_ISP_PADS_NUM, pads);
+ if (ret < 0) {
+ st_err(ST_ISP, "Failed to init media entity: %d\n", ret);
+ return ret;
+ }
+
+ ret = isp_init_controls(isp_dev);
+ if (ret)
+ goto err_sreg;
+
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ st_err(ST_ISP, "Failed to register subdev: %d\n", ret);
+ goto free_ctrls;
+ }
+
+ if (isp_dev->id == 0)
+ stf_isp_load_setfile(isp_dev, STF_ISP0_SETFILE);
+ else
+ stf_isp_load_setfile(isp_dev, STF_ISP1_SETFILE);
+
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
+err_sreg:
+ media_entity_cleanup(&sd->entity);
+ return ret;
+}
+
+int stf_isp_unregister(struct stf_isp_dev *isp_dev)
+{
+ v4l2_device_unregister_subdev(&isp_dev->subdev);
+ media_entity_cleanup(&isp_dev->subdev.entity);
+ v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
+ mutex_destroy(&isp_dev->stream_lock);
+ mutex_destroy(&isp_dev->setfile_lock);
+ return 0;
+}
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_ISP_H
+#define STF_ISP_H
+
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_ISP_PAD_SINK 0
+#define STF_ISP_PAD_SRC 1
+#define STF_ISP_PADS_NUM 2
+
+#define STF_ISP0_SETFILE "stf_isp0_fw.bin"
+#define STF_ISP1_SETFILE "stf_isp1_fw.bin"
+#define FILENAME_MAX_LEN 30
+
+#define SCALER_RATIO_MAX 1 // no compose function
+#define STF_ISP_REG_OFFSET_MAX 0x0FFF
+#define STF_ISP_REG_DELAY_MAX 100
+
+#define ISP_REG_CSIINTS_ADDR 0x00000008
+#define ISP_REG_DUMP_CFG_0 0x00000024
+#define ISP_REG_DUMP_CFG_1 0x00000028
+#define ISP_REG_IESHD_ADDR 0x00000A50
+
+struct stfisp_fw_info {
+ char __user filename[FILENAME_MAX_LEN];
+};
+
+#define VIDIOC_STFISP_LOAD_FW \
+ _IOW('V', BASE_VIDIOC_PRIVATE + 1, struct stfisp_fw_info)
+
+struct isp_format {
+ u32 code;
+ u8 bpp;
+};
+
+typedef struct {
+ u32 addr;
+ u32 val;
+ u32 mask;
+ u32 delay_ms;
+} regval_t;
+
+struct reg_table {
+ const regval_t *regval;
+ int regval_num;
+};
+
+struct stf_isp_dev;
+enum subdev_type;
+
+struct isp_hw_ops {
+ int (*isp_clk_enable)(struct stf_isp_dev *isp_dev);
+ int (*isp_clk_disable)(struct stf_isp_dev *isp_dev);
+ int (*isp_reset)(struct stf_isp_dev *isp_dev);
+ int (*isp_config_set)(struct stf_isp_dev *isp_dev);
+ int (*isp_set_format)(struct stf_isp_dev *isp_dev,
+ struct v4l2_rect *crop, u32 mcode);
+ // u32 width, u32 height);
+ int (*isp_stream_set)(struct stf_isp_dev *isp_dev, int on);
+};
+
+struct isp_ctrls {
+ struct v4l2_ctrl_handler handler;
+ struct v4l2_ctrl *pixel_rate;
+ struct {
+ struct v4l2_ctrl *auto_exp;
+ struct v4l2_ctrl *exposure;
+ };
+ struct {
+ struct v4l2_ctrl *auto_wb;
+ struct v4l2_ctrl *blue_balance;
+ struct v4l2_ctrl *red_balance;
+ };
+ struct {
+ struct v4l2_ctrl *auto_gain;
+ struct v4l2_ctrl *gain;
+ };
+ struct v4l2_ctrl *brightness;
+ struct v4l2_ctrl *light_freq;
+ struct v4l2_ctrl *saturation;
+ struct v4l2_ctrl *contrast;
+ struct v4l2_ctrl *hue;
+ struct v4l2_ctrl *test_pattern;
+ struct v4l2_ctrl *hflip;
+ struct v4l2_ctrl *vflip;
+};
+
+struct isp_setfile {
+ struct reg_table settings;
+ const u8 *data;
+ unsigned int size;
+ unsigned int state;
+};
+
+struct stf_isp_dev {
+ enum subdev_type sdev_type; // must be frist
+ struct stfcamss *stfcamss;
+ atomic_t ref_count;
+ u8 id;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_ISP_PADS_NUM];
+ struct v4l2_mbus_framefmt fmt[STF_ISP_PADS_NUM];
+ struct v4l2_rect compose;
+ struct v4l2_rect crop;
+ const struct isp_format *formats;
+ unsigned int nformats;
+ struct isp_hw_ops *hw_ops;
+ struct mutex stream_lock;
+ int stream_count;
+
+ struct isp_ctrls ctrls;
+ struct mutex setfile_lock;
+ struct isp_setfile setfile;
+};
+
+extern int stf_isp_subdev_init(struct stfcamss *stfcamss, int id);
+extern int stf_isp_register(struct stf_isp_dev *isp_dev,
+ struct v4l2_device *v4l2_dev);
+extern int stf_isp_unregister(struct stf_isp_dev *isp_dev);
+extern struct isp_hw_ops isp_ops;
+extern void dump_isp_reg(void *__iomem ispbase, int id);
+
+#endif /* STF_ISP_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ *
+ * linux/drivers/media/platform/starfive/stf_isp.c
+ *
+ * PURPOSE: This files contains the driver of VPP.
+ */
+#include "stfcamss.h"
+#include <asm/io.h>
+#include <linux/fb.h>
+#include <linux/module.h>
+#include <video/stf-vin.h>
+#include <linux/delay.h>
+
+static const regval_t isp_sc2235_reg_config_list[] = {
+ {0x00000014, 0x0000000c, 0, 0},
+// {0x00000018, 0x000011BB, 0, 0},
+ {0x00000A1C, 0x00000032, 0, 0},
+// {0x0000001C, 0x00000000, 0, 0},
+// {0x00000020, 0x0437077F, 0, 0},
+// {0x00000A0C, 0x04380780, 0, 0},
+ {0x00000A80, 0x88000000, 0, 0},
+ {0x00000A84, 0x881fa400, 0, 0},
+// {0x00000A88, 0x00000780, 0, 0},
+ {0x00000A8C, 0x00000010, 0, 0},
+ {0x00000A90, 0x00000000, 0, 0},
+ {0x00000A94, 0x803f4810, 0, 0},
+ {0x00000A98, 0x80517990, 0, 0},
+ {0x00000A9c, 0x000005c0, 0, 0},
+ {0x00000AA0, 0x0c000000, 0, 0},
+ {0x00000AA4, 0x0c000000, 0, 0},
+ {0x00000AA8, 0x05a0032a, 0, 0},
+ {0x00000AAC, 0x0418e410, 0, 0},
+ {0x00000AB0, 0x0420cd10, 0, 0},
+ {0x00000AB4, 0x0000021c, 0, 0},
+ {0x00000AB8, 0x08000000, 0, 0},
+ {0x00000ABc, 0x08000000, 0, 0},
+ {0x00000AC0, 0x021c03c0, 0, 0},
+ {0x00000AC4, 0x00000000, 0, 0},
+ {0x00000E40, 0x0000004D, 0, 0},
+ {0x00000E44, 0x00000096, 0, 0},
+ {0x00000E48, 0x0000001D, 0, 0},
+ {0x00000E4C, 0x000001DA, 0, 0},
+ {0x00000E50, 0x000001B6, 0, 0},
+ {0x00000E54, 0x00000070, 0, 0},
+ {0x00000E58, 0x0000009D, 0, 0},
+ {0x00000E5C, 0x0000017C, 0, 0},
+ {0x00000E60, 0x000001E6, 0, 0},
+ {0x00000010, 0x00000000, 0, 0},
+ {0x00000A08, 0x10000022, 0xFFFFFFF, 0},
+ {0x00000044, 0x00000000, 0, 0},
+ {0x00000A00, 0x00120002, 0, 0},
+ {0x00000A00, 0x00120000, 0, 0},
+ {0x00000A50, 0x00000002, 0, 0},
+ {0x00000A00, 0x00120001, 0, 0},
+ {0x00000008, 0x00010000, 0, 0},
+ {0x00000008, 0x00020004, 0, 0},
+ {0x00000000, 0x00000001, 0, 0},
+};
+
+static const regval_t isp_1080p_reg_config_list[] = {
+ {0x00000014, 0x0000000D, 0, 0},
+ // {0x00000018, 0x000011BB, 0, 0},
+ {0x00000A1C, 0x00000032, 0, 0},
+ // {0x0000001C, 0x00000000, 0, 0},
+ // {0x00000020, 0x0437077F, 0, 0},
+ // {0x00000A0C, 0x04380780, 0, 0},
+ // {0x00000A80, 0xF9000000, 0, 0},
+ // {0x00000A84, 0xF91FA400, 0, 0},
+ // {0x00000A88, 0x00000780, 0, 0},
+ {0x00000A8C, 0x00000000, 0, 0},
+ {0x00000A90, 0x00000000, 0, 0},
+ {0x00000E40, 0x0000004C, 0, 0},
+ {0x00000E44, 0x00000097, 0, 0},
+ {0x00000E48, 0x0000001D, 0, 0},
+ {0x00000E4C, 0x000001D5, 0, 0},
+ {0x00000E50, 0x000001AC, 0, 0},
+ {0x00000E54, 0x00000080, 0, 0},
+ {0x00000E58, 0x00000080, 0, 0},
+ {0x00000E5C, 0x00000194, 0, 0},
+ {0x00000E60, 0x000001EC, 0, 0},
+ {0x00000280, 0x00000000, 0, 0},
+ {0x00000284, 0x00000000, 0, 0},
+ {0x00000288, 0x00000000, 0, 0},
+ {0x0000028C, 0x00000000, 0, 0},
+ {0x00000290, 0x00000000, 0, 0},
+ {0x00000294, 0x00000000, 0, 0},
+ {0x00000298, 0x00000000, 0, 0},
+ {0x0000029C, 0x00000000, 0, 0},
+ {0x000002A0, 0x00000000, 0, 0},
+ {0x000002A4, 0x00000000, 0, 0},
+ {0x000002A8, 0x00000000, 0, 0},
+ {0x000002AC, 0x00000000, 0, 0},
+ {0x000002B0, 0x00000000, 0, 0},
+ {0x000002B4, 0x00000000, 0, 0},
+ {0x000002B8, 0x00000000, 0, 0},
+ {0x000002BC, 0x00000000, 0, 0},
+ {0x000002C0, 0x00F000F0, 0, 0},
+ {0x000002C4, 0x00F000F0, 0, 0},
+ {0x000002C8, 0x00800080, 0, 0},
+ {0x000002CC, 0x00800080, 0, 0},
+ {0x000002D0, 0x00800080, 0, 0},
+ {0x000002D4, 0x00800080, 0, 0},
+ {0x000002D8, 0x00B000B0, 0, 0},
+ {0x000002DC, 0x00B000B0, 0, 0},
+ {0x00000E00, 0x24000000, 0, 0},
+ {0x00000E04, 0x159500A5, 0, 0},
+ {0x00000E08, 0x0F9900EE, 0, 0},
+ {0x00000E0C, 0x0CE40127, 0, 0},
+ {0x00000E10, 0x0B410157, 0, 0},
+ {0x00000E14, 0x0A210181, 0, 0},
+ {0x00000E18, 0x094B01A8, 0, 0},
+ {0x00000E1C, 0x08A401CC, 0, 0},
+ {0x00000E20, 0x081D01EE, 0, 0},
+ {0x00000E24, 0x06B20263, 0, 0},
+ {0x00000E28, 0x05D802C7, 0, 0},
+ {0x00000E2C, 0x05420320, 0, 0},
+ {0x00000E30, 0x04D30370, 0, 0},
+ {0x00000E34, 0x047C03BB, 0, 0},
+ {0x00000E38, 0x043703FF, 0, 0},
+ {0x00000010, 0x00000080, 0, 0},
+ {0x00000A08, 0x10000032, 0xFFFFFFF, 0},
+ {0x00000A00, 0x00120002, 0, 0},
+ {0x00000A00, 0x00120000, 0, 0},
+ {0x00000A50, 0x00000002, 0, 0},
+ {0x00000A00, 0x00120001, 0, 0},
+ {0x00000008, 0x00010000, 0, 0},
+ {0x00000008, 0x0002000A, 0, 0},
+ {0x00000000, 0x00000001, 0, 0},
+};
+
+const struct reg_table isp_1920_1080_settings[] = {
+ {isp_1080p_reg_config_list,
+ ARRAY_SIZE(isp_1080p_reg_config_list)},
+};
+
+const struct reg_table isp_sc2235_settings[] = {
+ {isp_sc2235_reg_config_list,
+ ARRAY_SIZE(isp_sc2235_reg_config_list)},
+};
+
+static regval_t isp_format_reg_list[] = {
+ {0x0000001C, 0x00000000, 0, 0},
+ {0x00000020, 0x0437077F, 0, 0},
+ {0x00000A0C, 0x04380780, 0, 0},
+ {0x00000A88, 0x00000780, 0, 0},
+ {0x00000018, 0x000011BB, 0, 0},
+ {0x00000A08, 0x10000022, 0xF0000000, 0},
+};
+
+const struct reg_table isp_format_settings[] = {
+ {isp_format_reg_list,
+ ARRAY_SIZE(isp_format_reg_list)},
+};
+
+static struct reg_table *isp_settings = isp_1920_1080_settings;
+
+static void isp_load_regs(void __iomem *ispbase, const struct reg_table *table)
+{
+ int j;
+ u32 delay_ms, reg_addr, mask, val;
+
+ for (j = 0; j < table->regval_num; j++) {
+ delay_ms = table->regval[j].delay_ms;
+ reg_addr = table->regval[j].addr;
+ val = table->regval[j].val;
+ mask = table->regval[j].mask;
+
+ if (reg_addr % 4
+ || reg_addr > STF_ISP_REG_OFFSET_MAX
+ || delay_ms > STF_ISP_REG_DELAY_MAX)
+ continue;
+
+ if (mask)
+ reg_set_bit(ispbase, reg_addr, mask, val);
+ else
+ reg_write(ispbase, reg_addr, val);
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+}
+
+static int stf_isp_clk_enable(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+
+ if (isp_dev->id == 0) {
+ reg_set_highest_bit(vin->clkgen_base, CLK_U0_ISPV2_TOP_WRAPPER_CLK_C);
+ reg_clear_rst(vin->clkgen_base,SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RST_U0_ISPV2_TOP_WRAPPER_RST_C);
+ reg_clear_rst(vin->clkgen_base,SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RST_U0_ISPV2_TOP_WRAPPER_RST_P);
+ } else {
+ st_err(ST_ISP, "please check isp id :%d\n", isp_dev->id);
+ }
+
+
+ return 0;
+}
+
+static int stf_isp_clk_disable(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+
+ if (isp_dev->id == 0) {
+ reg_assert_rst(vin->clkgen_base,SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RST_U0_ISPV2_TOP_WRAPPER_RST_C);
+ reg_assert_rst(vin->clkgen_base,SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RST_U0_ISPV2_TOP_WRAPPER_RST_P);
+ reg_set_bit(vin->clkgen_base, CLK_U0_ISPV2_TOP_WRAPPER_CLK_C, CLK_U0_ISPV2_CLK_ICG, 0x0);
+ } else {
+ st_err(ST_ISP, "please check isp id :%d\n", isp_dev->id);
+ }
+ return 0;
+}
+
+static int stf_isp_reset(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+
+ return 0;
+}
+
+static int stf_isp_config_set(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+
+ if (isp_dev->id == 0)
+ ispbase = vin->isp_isp0_base;
+ else
+ ispbase = vin->isp_isp1_base;
+
+ st_debug(ST_ISP, "%s, isp_id = %d\n", __func__, isp_dev->id);
+
+ isp_load_regs(ispbase, isp_format_settings);
+ mutex_lock(&isp_dev->setfile_lock);
+ if (isp_dev->setfile.state)
+ isp_load_regs(ispbase, &isp_dev->setfile.settings);
+ else
+ isp_load_regs(ispbase, isp_settings);
+ mutex_unlock(&isp_dev->setfile_lock);
+
+ st_debug(ST_ISP, "config 0x%x = 0x%x\n",
+ isp_format_reg_list[0].addr,
+ isp_format_reg_list[0].val);
+ st_debug(ST_ISP, "config 0x%x = 0x%x\n",
+ isp_format_reg_list[1].addr,
+ isp_format_reg_list[1].val);
+ st_debug(ST_ISP, "config 0x%x = 0x%x\n",
+ isp_format_reg_list[2].addr,
+ isp_format_reg_list[2].val);
+ st_debug(ST_ISP, "config 0x%x = 0x%x\n",
+ isp_format_reg_list[3].addr,
+ isp_format_reg_list[3].val);
+
+ return 0;
+}
+
+static int stf_isp_set_format(struct stf_isp_dev *isp_dev,
+ struct v4l2_rect *crop, u32 mcode)
+ // u32 width, u32 height)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+ u32 val, val1;
+
+ if (isp_dev->id == 0) {
+ ispbase = vin->isp_isp0_base;
+ isp_settings = isp_sc2235_settings;
+ } else {
+ ispbase = vin->isp_isp1_base;
+ isp_settings = isp_sc2235_settings;
+ }
+
+ val = crop->left + (crop->top << 16);
+ isp_format_reg_list[0].addr = ISP_REG_PIC_CAPTURE_START_CFG;
+ isp_format_reg_list[0].val = val;
+
+ val = (crop->width + crop->left - 1)
+ + ((crop->height + crop->top - 1) << 16);
+ isp_format_reg_list[1].addr = ISP_REG_PIC_CAPTURE_END_CFG;
+ isp_format_reg_list[1].val = val;
+
+ val = crop->width + (crop->height << 16);
+ isp_format_reg_list[2].addr = ISP_REG_PIPELINE_XY_SIZE;
+ isp_format_reg_list[2].val = val;
+
+ isp_format_reg_list[3].addr = ISP_REG_STRIDE;
+ isp_format_reg_list[3].val = crop->width;
+
+ switch (mcode) {
+ case MEDIA_BUS_FMT_SRGGB10_1X10:
+ case MEDIA_BUS_FMT_SRGGB8_1X8:
+ // 3 2 3 2 1 0 1 0 B Gb B Gb Gr R Gr R
+ val = 0x0000EE44;
+ val1 = 0x00000000;
+ break;
+ case MEDIA_BUS_FMT_SGRBG10_1X10:
+ case MEDIA_BUS_FMT_SGRBG8_1X8:
+ // 2 3 2 3 0 1 0 1, Gb B Gb B R Gr R Gr
+ val = 0x0000BB11;
+ val1 = 0x20000000;
+ break;
+ case MEDIA_BUS_FMT_SGBRG10_1X10:
+ case MEDIA_BUS_FMT_SGBRG8_1X8:
+ // 1 0 1 0 3 2 3 2, Gr R Gr R B Gb B Gb
+ val = 0x000044EE;
+ val1 = 0x30000000;
+ break;
+ case MEDIA_BUS_FMT_SBGGR10_1X10:
+ case MEDIA_BUS_FMT_SBGGR8_1X8:
+ // 0 1 0 1 2 3 2 3 R Gr R Gr Gb B Gb B
+ val = 0x000011BB;
+ val1 = 0x10000000;
+ break;
+ default:
+ st_err(ST_ISP, "UNKNOW format\n");
+ val = 0x000011BB;
+ val1 = 0x10000000;
+ break;
+ }
+ isp_format_reg_list[4].addr = ISP_REG_RAW_FORMAT_CFG;
+ isp_format_reg_list[4].val = val;
+
+ isp_format_reg_list[5].addr = ISP_REG_ISP_CTRL_1;
+ isp_format_reg_list[5].val = val1;
+ isp_format_reg_list[5].mask = 0xF0000000;
+
+ st_info(ST_ISP, "left: %d, top: %d, width = %d, height = %d, code = 0x%x\n",
+ crop->left, crop->top, crop->width, crop->height, mcode);
+
+ return 0;
+}
+
+static int stf_isp_stream_set(struct stf_isp_dev *isp_dev, int on)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+
+ return 0;
+}
+
+void dump_isp_reg(void *__iomem ispbase, int id)
+{
+ int j;
+ u32 addr, val;
+
+ st_debug(ST_ISP, "DUMP ISP%d register:\n", id);
+ for (j = 0; j < isp_format_settings->regval_num; j++) {
+ addr = isp_format_settings->regval[j].addr;
+ val = ioread32(ispbase + addr);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", addr, val);
+ }
+
+ val = ioread32(ispbase + ISP_REG_Y_PLANE_START_ADDR);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_Y_PLANE_START_ADDR, val);
+ val = ioread32(ispbase + ISP_REG_UV_PLANE_START_ADDR);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_UV_PLANE_START_ADDR, val);
+ val = ioread32(ispbase + ISP_REG_DUMP_CFG_0);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_DUMP_CFG_0, val);
+ val = ioread32(ispbase + ISP_REG_DUMP_CFG_1);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", ISP_REG_DUMP_CFG_1, val);
+
+ for (j = 0; j < isp_settings->regval_num; j++) {
+ addr = isp_settings->regval[j].addr;
+ val = ioread32(ispbase + addr);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", addr, val);
+ }
+}
+
+struct isp_hw_ops isp_ops = {
+ .isp_clk_enable = stf_isp_clk_enable,
+ .isp_clk_disable = stf_isp_clk_disable,
+ .isp_reset = stf_isp_reset,
+ .isp_config_set = stf_isp_config_set,
+ .isp_set_format = stf_isp_set_format,
+ .isp_stream_set = stf_isp_stream_set,
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include "stf_video.h"
+#include <media/media-entity.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define USE_MEDIA_PIPELINE
+
+static const struct stfcamss_format_info formats_pix_st7110_wr[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 16 } },
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_PIX_FMT_RGB565, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 16 } },
+};
+
+static const struct stfcamss_format_info formats_raw_st7110_isp[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+};
+
+static const struct stfcamss_format_info formats_pix_st7110_isp[] = {
+ // { MEDIA_BUS_FMT_YUYV12_2X12, V4L2_PIX_FMT_NV21M, 2,
+ // { { 1, 1 }, { 1, 1 } }, { { 1, 1 }, { 1, 1 } }, { 8 , 4 } },
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_NV21, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_NV21, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_NV21, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_NV21, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+};
+
+static int video_find_format(u32 code, u32 pixelformat,
+ const struct stfcamss_format_info *formats,
+ unsigned int nformats)
+{
+ int i;
+
+ for (i = 0; i < nformats; i++) {
+ if (formats[i].code == code &&
+ formats[i].pixelformat == pixelformat)
+ return i;
+ }
+
+ for (i = 0; i < nformats; i++)
+ if (formats[i].code == code)
+ return i;
+
+ for (i = 0; i < nformats; i++)
+ if (formats[i].pixelformat == pixelformat)
+ return i;
+
+ return -EINVAL;
+}
+
+static int __video_try_fmt(struct stfcamss_video *video,
+ struct v4l2_format *f, int is_mp)
+{
+ struct v4l2_pix_format *pix;
+ struct v4l2_pix_format_mplane *pix_mp;
+ const struct stfcamss_format_info *fi;
+ u32 width, height;
+ u32 bpl;
+ int i, j;
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+ pix = &f->fmt.pix;
+ pix_mp = &f->fmt.pix_mp;
+
+ if (is_mp) {
+ for (i = 0; i < video->nformats; i++)
+ if (pix_mp->pixelformat
+ == video->formats[i].pixelformat)
+ break;
+
+ if (i == video->nformats)
+ i = 0; /* default format */
+
+ fi = &video->formats[i];
+ width = pix_mp->width;
+ height = pix_mp->height;
+
+ memset(pix_mp, 0, sizeof(*pix_mp));
+
+ pix_mp->pixelformat = fi->pixelformat;
+ pix_mp->width = clamp_t(u32, width, 1,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ pix_mp->height = clamp_t(u32, height, 1,
+ STFCAMSS_FRAME_MAX_HEIGHT_RDI);
+ pix_mp->num_planes = fi->planes;
+ for (j = 0; j < pix_mp->num_planes; j++) {
+ bpl = pix_mp->width / fi->hsub[j].numerator *
+ fi->hsub[j].denominator * fi->bpp[j] / 8;
+ bpl = ALIGN(bpl, video->bpl_alignment);
+ pix_mp->plane_fmt[j].bytesperline = bpl;
+ pix_mp->plane_fmt[j].sizeimage = pix_mp->height /
+ fi->vsub[j].numerator
+ * fi->vsub[j].denominator * bpl;
+ }
+
+ pix_mp->field = V4L2_FIELD_NONE;
+ pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
+ pix_mp->flags = 0;
+ pix_mp->ycbcr_enc =
+ V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace);
+ pix_mp->quantization =
+ V4L2_MAP_QUANTIZATION_DEFAULT(true,
+ pix_mp->colorspace, pix_mp->ycbcr_enc);
+ pix_mp->xfer_func =
+ V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace);
+ st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d, "
+ "i = %d, p = %d, s = 0x%x\n",
+ pix_mp->width, pix_mp->height, fi->bpp[0], i,
+ pix_mp->num_planes, pix_mp->plane_fmt[0].sizeimage);
+
+ } else {
+ for (i = 0; i < video->nformats; i++)
+ if (pix->pixelformat == video->formats[i].pixelformat)
+ break;
+
+ if (i == video->nformats)
+ i = 0; /* default format */
+
+ fi = &video->formats[i];
+ width = pix->width;
+ height = pix->height;
+
+ memset(pix, 0, sizeof(*pix));
+
+ pix->pixelformat = fi->pixelformat;
+ pix->width = clamp_t(u32, width, 1,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ pix->height = clamp_t(u32, height, 1,
+ STFCAMSS_FRAME_MAX_HEIGHT_RDI);
+ bpl = pix->width / fi->hsub[0].numerator *
+ fi->hsub[0].denominator * fi->bpp[0] / 8;
+ bpl = ALIGN(bpl, video->bpl_alignment);
+ pix->bytesperline = bpl;
+ pix->sizeimage = pix->height /
+ fi->vsub[0].numerator
+ * fi->vsub[0].denominator * bpl;
+
+ pix->field = V4L2_FIELD_NONE;
+ pix->colorspace = V4L2_COLORSPACE_SRGB;
+ pix->flags = 0;
+ pix->ycbcr_enc =
+ V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
+ pix->quantization =
+ V4L2_MAP_QUANTIZATION_DEFAULT(true,
+ pix->colorspace, pix->ycbcr_enc);
+ pix->xfer_func =
+ V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
+ st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d, "
+ "i = %d, s = 0x%x\n",
+ pix->width, pix->height,
+ fi->bpp[0], i, pix->sizeimage);
+ }
+ return 0;
+}
+
+static int stf_video_init_format(struct stfcamss_video *video, int is_mp)
+{
+ int ret;
+ struct v4l2_format format = {
+ .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+ .fmt.pix = {
+ .width = 1920,
+ .height = 1080,
+ .pixelformat = V4L2_PIX_FMT_RGB565,
+ },
+ };
+
+ struct v4l2_format format_mp = {
+ .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+ .fmt.pix = {
+ .width = 1920,
+ .height = 1080,
+ .pixelformat = V4L2_PIX_FMT_NV21,
+ },
+ };
+
+ if (is_mp)
+ ret = __video_try_fmt(video, &format_mp, true);
+ else
+ ret = __video_try_fmt(video, &format, false);
+
+ if (ret < 0)
+ return ret;
+
+ video->active_fmt = is_mp ? format_mp : format;
+
+ return 0;
+}
+
+static int video_queue_setup(struct vb2_queue *q,
+ unsigned int *num_buffers, unsigned int *num_planes,
+ unsigned int sizes[], struct device *alloc_devs[])
+{
+ struct stfcamss_video *video = vb2_get_drv_priv(q);
+ const struct v4l2_pix_format *format =
+ &video->active_fmt.fmt.pix;
+ const struct v4l2_pix_format_mplane *format_mp =
+ &video->active_fmt.fmt.pix_mp;
+ unsigned int i;
+
+ st_debug(ST_VIDEO, "%s, planes = %d\n", __func__, *num_planes);
+
+ if (video->is_mp) {
+ if (*num_planes) {
+ if (*num_planes != format_mp->num_planes)
+ return -EINVAL;
+
+ for (i = 0; i < *num_planes; i++)
+ if (sizes[i] <
+ format_mp->plane_fmt[i].sizeimage)
+ return -EINVAL;
+
+ return 0;
+ }
+
+ *num_planes = format_mp->num_planes;
+
+ for (i = 0; i < *num_planes; i++)
+ sizes[i] = format_mp->plane_fmt[i].sizeimage;
+ } else {
+ if (*num_planes) {
+ if (*num_planes != 1)
+ return -EINVAL;
+
+ if (sizes[0] < format->sizeimage)
+ return -EINVAL;
+ }
+
+ *num_planes = 1;
+ sizes[0] = format->sizeimage;
+ if (!sizes[0])
+ st_err(ST_VIDEO, "%s: error size is zero!!!\n", __func__);
+ }
+
+ st_info(ST_VIDEO, "%s, planes = %d, size = %d\n",
+ __func__, *num_planes, sizes[0]);
+ return 0;
+}
+
+static int video_buf_init(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+ struct stfcamss_buffer *buffer =
+ container_of(vbuf, struct stfcamss_buffer, vb);
+ const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+ const struct v4l2_pix_format_mplane *fmt_mp =
+ &video->active_fmt.fmt.pix_mp;
+ //struct sg_table *sgt;
+ dma_addr_t *paddr;
+ unsigned int i;
+
+ if (video->is_mp) {
+ for (i = 0; i < fmt_mp->num_planes; i++) {
+ paddr = vb2_plane_cookie(vb, i);
+ buffer->addr[i] = *paddr;
+ }
+
+ if (fmt_mp->num_planes == 1
+ && (fmt_mp->pixelformat == V4L2_PIX_FMT_NV12
+ || fmt_mp->pixelformat == V4L2_PIX_FMT_NV21
+ || fmt_mp->pixelformat == V4L2_PIX_FMT_NV16
+ || fmt_mp->pixelformat == V4L2_PIX_FMT_NV61))
+ buffer->addr[1] = buffer->addr[0] +
+ fmt_mp->width *
+ fmt_mp->height;
+ } else {
+ paddr = vb2_plane_cookie(vb, 0);
+ buffer->addr[0] = *paddr;
+ if (fmt->pixelformat == V4L2_PIX_FMT_NV12
+ || fmt->pixelformat == V4L2_PIX_FMT_NV21
+ || fmt->pixelformat == V4L2_PIX_FMT_NV16
+ || fmt->pixelformat == V4L2_PIX_FMT_NV61)
+ buffer->addr[1] = buffer->addr[0] +
+ fmt->width *
+ fmt->height;
+ }
+
+ return 0;
+}
+
+static int video_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+ const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+ const struct v4l2_pix_format_mplane *fmt_mp =
+ &video->active_fmt.fmt.pix_mp;
+ unsigned int i;
+
+ if (video->is_mp) {
+ for (i = 0; i < fmt_mp->num_planes; i++) {
+ if (fmt_mp->plane_fmt[i].sizeimage
+ > vb2_plane_size(vb, i))
+ return -EINVAL;
+
+ vb2_set_plane_payload(vb, i,
+ fmt_mp->plane_fmt[i].sizeimage);
+ }
+ } else {
+ if (fmt->sizeimage > vb2_plane_size(vb, 0))
+ return -EINVAL;
+ vb2_set_plane_payload(vb, 0, fmt->sizeimage);
+ }
+
+ vbuf->field = V4L2_FIELD_NONE;
+
+ return 0;
+}
+
+static void video_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+ struct stfcamss_buffer *buffer =
+ container_of(vbuf, struct stfcamss_buffer, vb);
+
+ video->ops->queue_buffer(video, buffer);
+}
+
+static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus,
+ struct v4l2_pix_format_mplane *pix,
+ const struct stfcamss_format_info *f,
+ unsigned int alignment)
+{
+ unsigned int i;
+ u32 bytesperline;
+
+ memset(pix, 0, sizeof(*pix));
+ v4l2_fill_pix_format_mplane(pix, mbus);
+ pix->pixelformat = f->pixelformat;
+ pix->num_planes = f->planes;
+ for (i = 0; i < pix->num_planes; i++) {
+ bytesperline = pix->width / f->hsub[i].numerator *
+ f->hsub[i].denominator * f->bpp[i] / 8;
+ bytesperline = ALIGN(bytesperline, alignment);
+ pix->plane_fmt[i].bytesperline = bytesperline;
+ pix->plane_fmt[i].sizeimage = pix->height /
+ f->vsub[i].numerator * f->vsub[i].denominator *
+ bytesperline;
+ }
+
+ return 0;
+}
+
+static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+ struct v4l2_pix_format *pix,
+ const struct stfcamss_format_info *f,
+ unsigned int alignment)
+{
+ u32 bytesperline;
+
+ memset(pix, 0, sizeof(*pix));
+ v4l2_fill_pix_format(pix, mbus);
+ pix->pixelformat = f->pixelformat;
+ bytesperline = pix->width / f->hsub[0].numerator *
+ f->hsub[0].denominator * f->bpp[0] / 8;
+ bytesperline = ALIGN(bytesperline, alignment);
+ pix->bytesperline = bytesperline;
+ pix->sizeimage = pix->height /
+ f->vsub[0].numerator * f->vsub[0].denominator *
+ bytesperline;
+ return 0;
+}
+
+static struct v4l2_subdev *video_remote_subdev(
+ struct stfcamss_video *video, u32 *pad)
+{
+ struct media_pad *remote;
+
+ remote = media_entity_remote_pad(&video->pad);
+
+ if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+ return NULL;
+
+ if (pad)
+ *pad = remote->index;
+
+ return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int video_get_subdev_format(struct stfcamss_video *video,
+ struct v4l2_format *format)
+{
+ struct v4l2_subdev_format fmt;
+ struct v4l2_subdev *subdev;
+ u32 pixelformat;
+ u32 pad;
+ int ret;
+
+ subdev = video_remote_subdev(video, &pad);
+ if (subdev == NULL)
+ return -EPIPE;
+
+ fmt.pad = pad;
+ fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+ ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+ if (ret)
+ return ret;
+
+ pixelformat = video->is_mp ? format->fmt.pix.pixelformat
+ : format->fmt.pix_mp.pixelformat;
+ ret = video_find_format(fmt.format.code, pixelformat,
+ video->formats, video->nformats);
+ if (ret < 0)
+ return ret;
+
+ format->type = video->type;
+
+ if (video->is_mp)
+ return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp,
+ &video->formats[ret], video->bpl_alignment);
+ else
+ return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
+ &video->formats[ret], video->bpl_alignment);
+}
+
+static int video_check_format(struct stfcamss_video *video)
+{
+ struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+ struct v4l2_pix_format_mplane *pix_mp =
+ &video->active_fmt.fmt.pix_mp;
+ struct v4l2_format format;
+ struct v4l2_pix_format *sd_pix = &format.fmt.pix;
+ struct v4l2_pix_format_mplane *sd_pix_mp = &format.fmt.pix_mp;
+ int ret;
+
+ if (video->is_mp) {
+ sd_pix_mp->pixelformat = pix_mp->pixelformat;
+ ret = video_get_subdev_format(video, &format);
+ if (ret < 0)
+ return ret;
+
+ if (pix_mp->pixelformat != sd_pix_mp->pixelformat ||
+ pix_mp->height > sd_pix_mp->height ||
+ pix_mp->width > sd_pix_mp->width ||
+ pix_mp->num_planes != sd_pix_mp->num_planes ||
+ pix_mp->field != format.fmt.pix_mp.field) {
+ st_err(ST_VIDEO,
+ "%s, not match:\n"
+ "0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
+ __func__,
+ pix_mp->pixelformat, sd_pix_mp->pixelformat,
+ pix_mp->height, sd_pix_mp->height,
+ pix_mp->field, format.fmt.pix_mp.field);
+ return -EPIPE;
+ }
+
+ } else {
+ sd_pix->pixelformat = pix->pixelformat;
+ ret = video_get_subdev_format(video, &format);
+ if (ret < 0)
+ return ret;
+
+ if (pix->pixelformat != sd_pix->pixelformat ||
+ pix->height > sd_pix->height ||
+ pix->width > sd_pix->width ||
+ pix->field != format.fmt.pix.field) {
+ st_err(ST_VIDEO,
+ "%s, not match:\n"
+ "0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
+ __func__,
+ pix->pixelformat, sd_pix->pixelformat,
+ pix->height, sd_pix->height,
+ pix->field, format.fmt.pix.field);
+ return -EPIPE;
+ }
+ }
+ return 0;
+}
+
+static int video_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+ struct stfcamss_video *video = vb2_get_drv_priv(q);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ int ret;
+
+#ifdef USE_MEDIA_PIPELINE
+ ret = media_pipeline_start(&vdev->entity, &video->pipe);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to media_pipeline_start: %d\n", ret);
+ return ret;
+ }
+#endif
+ ret = video_check_format(video);
+ if (ret < 0)
+ goto error;
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, video, s_stream, 1);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ goto error;
+ }
+ return 0;
+
+error:
+#ifdef USE_MEDIA_PIPELINE
+ media_pipeline_stop(&vdev->entity);
+#endif
+ video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
+ return ret;
+}
+
+static void video_stop_streaming(struct vb2_queue *q)
+{
+ struct stfcamss_video *video = vb2_get_drv_priv(q);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ v4l2_subdev_call(subdev, video, s_stream, 0);
+ }
+
+#ifdef USE_MEDIA_PIPELINE
+ media_pipeline_stop(&vdev->entity);
+#endif
+ video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops stf_video_vb2_q_ops = {
+ .queue_setup = video_queue_setup,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+ .buf_init = video_buf_init,
+ .buf_prepare = video_buf_prepare,
+ .buf_queue = video_buf_queue,
+ .start_streaming = video_start_streaming,
+ .stop_streaming = video_stop_streaming,
+};
+
+/* -----------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int video_querycap(struct file *file, void *fh,
+ struct v4l2_capability *cap)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ strscpy(cap->driver, "stf camss", sizeof(cap->driver));
+ strscpy(cap->card, "Starfive Camera Subsystem", sizeof(cap->card));
+ snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+ dev_name(video->stfcamss->dev));
+ return 0;
+}
+
+static int video_get_unique_pixelformat_by_index(struct stfcamss_video *video,
+ int ndx)
+{
+ int i, j, k;
+
+ /* find index "i" of "k"th unique pixelformat in formats array */
+ k = -1;
+ for (i = 0; i < video->nformats; i++) {
+ for (j = 0; j < i; j++) {
+ if (video->formats[i].pixelformat ==
+ video->formats[j].pixelformat)
+ break;
+ }
+
+ if (j == i)
+ k++;
+
+ if (k == ndx)
+ return i;
+ }
+
+ return -EINVAL;
+}
+
+static int video_get_pixelformat_by_mbus_code(struct stfcamss_video *video,
+ u32 mcode)
+{
+ int i;
+
+ for (i = 0; i < video->nformats; i++) {
+ if (video->formats[i].code == mcode)
+ return i;
+ }
+
+ return -EINVAL;
+}
+
+static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ int i;
+
+ st_debug(ST_VIDEO, "%s:\n0x%x 0x%x\n 0x%x, 0x%x\n0x%x\n",
+ __func__,
+ f->type, video->type,
+ f->index, video->nformats,
+ f->mbus_code);
+
+ if (f->type != video->type)
+ return -EINVAL;
+ if (f->index >= video->nformats)
+ return -EINVAL;
+
+ if (f->mbus_code) {
+ /* Each entry in formats[] table has unique mbus_code */
+ if (f->index > 0)
+ return -EINVAL;
+
+ i = video_get_pixelformat_by_mbus_code(video, f->mbus_code);
+ } else {
+ i = video_get_unique_pixelformat_by_index(video, f->index);
+ }
+
+ if (i < 0)
+ return -EINVAL;
+
+ f->pixelformat = video->formats[i].pixelformat;
+
+ return 0;
+}
+static struct v4l2_subdev *get_senname(struct file *file, char *name) {
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ char vin_name[40];
+ int ret;
+
+ strcpy(vin_name,entity->name);
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+ entity = pad->entity;
+ }
+ if(strncmp(vin_name,entity->name,13)==0) {
+ st_err(ST_VIDEO, "===== [%s] Please configure pipeline first =====\n", name);
+ return NULL;
+ }
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ return subdev;
+}
+
+static int video_enum_framesizes(struct file *file, void *fh,
+ struct v4l2_frmsizeenum *fsize)
+{
+ struct v4l2_subdev_frame_size_enum fse = {0};
+ struct v4l2_subdev_mbus_code_enum code = {0};
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ int i;
+ int ret;
+
+ for (i = 0; i < video->nformats; i++) {
+ if (video->formats[i].pixelformat == fsize->pixel_format)
+ break;
+ }
+
+ if (i == video->nformats)
+ return -EINVAL;
+
+ sensor = stfcamss_find_sensor(entity);
+ if (sensor) {
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ code.index = 0;
+ code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
+ if (ret < 0)
+ return -EINVAL;
+ fse.index = fsize->index;
+ fse.code = code.code;
+ fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(subdev, pad, enum_frame_size, NULL,&fse);
+ if (ret < 0)
+ return -EINVAL;
+ fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+ fsize->discrete.width = fse.min_width;
+ fsize->discrete.height = fse.min_height;
+ } else {
+ if (fsize->index)
+ return -EINVAL;
+ fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+ fsize->stepwise.min_width = STFCAMSS_FRAME_MIN_WIDTH;
+ fsize->stepwise.max_width = STFCAMSS_FRAME_MAX_WIDTH;
+ fsize->stepwise.min_height = STFCAMSS_FRAME_MIN_HEIGHT;
+ fsize->stepwise.max_height = STFCAMSS_FRAME_MAX_HEIGHT_PIX;
+ fsize->stepwise.step_width = 1;
+ fsize->stepwise.step_height = 1;
+ }
+
+ return 0;
+}
+static int video_enum_frameintervals(struct file *file, void *fh,
+ struct v4l2_frmivalenum *fival)
+{
+ int ret = 0;
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ struct v4l2_subdev_mbus_code_enum code = {0};
+ struct v4l2_subdev_frame_interval_enum fie = {0};
+
+ sensor = stfcamss_find_sensor(entity);
+ if (!sensor)
+ return -EINVAL;
+ fie.index = fival->index;
+ fie.width = fival->width;
+ fie.height = fival->height;
+ fie.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ subdev = media_entity_to_v4l2_subdev(sensor);
+
+ code.index = 0;
+ code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+ ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
+ if (ret < 0)
+ return -EINVAL;
+
+ fie.code = code.code;
+ ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
+ if (ret < 0)
+ return ret;
+
+ fival->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+ fival->discrete = fie.interval;
+
+ return 0;
+}
+
+static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+ st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x\n",
+ __func__, video->active_fmt.type);
+ *f = video->active_fmt;
+ return 0;
+}
+
+static int video_g_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+ st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x\n",
+ __func__, video->active_fmt.type);
+ *f = video->active_fmt;
+ return 0;
+}
+
+static int video_pipeline_s_fmt(struct stfcamss_video *video,
+ struct v4l2_subdev_state *state,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct v4l2_subdev *subdev;
+ struct media_device *mdev = entity->graph_obj.mdev;
+ struct media_graph graph;
+ int ret, index;
+ struct v4l2_subdev_format fmt = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ };
+ struct v4l2_mbus_framefmt *mf = &fmt.format;
+ struct v4l2_pix_format *pix = &f->fmt.pix;
+ struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+ u32 width, height, code;
+
+ /* pix to mbus format */
+ if (video->is_mp) {
+ index = video_find_format(mf->code,
+ pix_mp->pixelformat,
+ video->formats, video->nformats);
+ if (index < 0)
+ return index;
+ v4l2_fill_mbus_format_mplane(mf, pix_mp);
+ mf->code = video->formats[index].code;
+ code = mf->code;
+ width = mf->width;
+ height = mf->height;
+ } else {
+ index = video_find_format(mf->code,
+ pix->pixelformat,
+ video->formats, video->nformats);
+ if (index < 0)
+ return index;
+ v4l2_fill_mbus_format(mf, pix, video->formats[index].code);
+ code = mf->code;
+ width = mf->width;
+ height = mf->height;
+ }
+ /*
+ * Starting from sensor subdevice, walk within
+ * pipeline and set format on each subdevice
+ */
+ mutex_lock(&mdev->graph_mutex);
+ ret = media_graph_walk_init(&graph, mdev);
+ if (ret) {
+ mutex_unlock(&mdev->graph_mutex);
+ return ret;
+ }
+
+ media_graph_walk_start(&graph, entity);
+
+ while (!ret && (entity = media_graph_walk_next(&graph)))
+ if (is_media_entity_v4l2_subdev(entity)) {
+ subdev = media_entity_to_v4l2_subdev(entity);
+ ret = v4l2_subdev_call(subdev, pad, set_fmt, state, &fmt);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ if (mf->code != code ||
+ mf->width != width || mf->height != height) {
+ st_warn(ST_VIDEO,
+ "\"%s\":%d pad fmt has been"
+ " changed to 0x%x %ux%u\n",
+ subdev->name, fmt.pad, mf->code,
+ mf->width, mf->height);
+ }
+ }
+
+ mutex_unlock(&mdev->graph_mutex);
+
+ media_graph_walk_cleanup(&graph);
+
+ if (ret < 0 && ret != -ENOIOCTLCMD) {
+ st_err(ST_VIDEO,
+ "%s: Failed to set fmt 0x%x %ux%u"
+ " on \"%s\":%d pad (%d)\n",
+ __func__, mf->code,
+ mf->width, mf->height,
+ subdev->name, fmt.pad, ret);
+ return ret;
+ }
+
+ if (video->is_mp)
+ video_mbus_to_pix_mp(mf, pix_mp,
+ &video->formats[index], video->bpl_alignment);
+ else
+ video_mbus_to_pix(mf, pix,
+ &video->formats[index], video->bpl_alignment);
+
+ ret = __video_try_fmt(video, f, video->is_mp);
+ if (ret < 0)
+ return ret;
+
+ return 0;
+}
+
+static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+
+ if (vb2_is_busy(&video->vb2_q))
+ return -EBUSY;
+
+ ret = __video_try_fmt(video, f, false);
+ if (ret < 0)
+ return ret;
+
+ ret = video_pipeline_s_fmt(video, NULL, f);
+ if (ret < 0)
+ return ret;
+
+ video->active_fmt = *f;
+
+ return 0;
+}
+
+static int video_s_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+ if (vb2_is_busy(&video->vb2_q))
+ return -EBUSY;
+
+ ret = __video_try_fmt(video, f, true);
+ if (ret < 0)
+ return ret;
+
+ ret = video_pipeline_s_fmt(video, NULL, f);
+ if (ret < 0)
+ return ret;
+
+ video->active_fmt = *f;
+
+ return 0;
+}
+
+static int video_try_fmt(struct file *file,
+ void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ return __video_try_fmt(video, f, false);
+}
+
+static int video_try_fmt_mp(struct file *file,
+ void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ return __video_try_fmt(video, f, true);
+}
+
+static int video_enum_input(struct file *file, void *fh,
+ struct v4l2_input *input)
+{
+ if (input->index > 0)
+ return -EINVAL;
+
+ strscpy(input->name, "camera", sizeof(input->name));
+ input->type = V4L2_INPUT_TYPE_CAMERA;
+
+ return 0;
+}
+
+static int video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+ *input = 0;
+
+ return 0;
+}
+
+static int video_s_input(struct file *file, void *fh, unsigned int input)
+{
+ return input == 0 ? 0 : -EINVAL;
+}
+
+static int video_g_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *p)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ int ret, is_support = 0;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_g_parm_cap(vdev, subdev, p);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ if (!ret)
+ is_support = 1;
+ }
+
+ return is_support ? 0 : ret;
+}
+
+static int video_s_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *p)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_streamparm tmp_p;
+ int ret, is_support = 0;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ tmp_p = *p;
+ ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ if (!ret) {
+ is_support = 1;
+ *p = tmp_p;
+ }
+ }
+
+ return is_support ? 0 : ret;
+}
+
+/* Crop ioctls */
+int video_g_pixelaspect(struct file *file, void *fh,
+ int buf_type, struct v4l2_fract *aspect)
+{
+ return 0;
+}
+
+int video_g_selection(struct file *file, void *fh,
+ struct v4l2_selection *s)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_subdev_selection sel = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .target = s->target,
+ .r = s->r,
+ .flags = s->flags,
+ };
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
+ __func__, sel.target, s->target);
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
+ && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+ return -EINVAL;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sel);
+ if (!ret) {
+ s->r = sel.r;
+ s->flags = sel.flags;
+ break;
+ }
+ if (ret != -ENOIOCTLCMD)
+ break;
+ }
+
+ return ret;
+}
+
+int video_s_selection(struct file *file, void *fh,
+ struct v4l2_selection *s)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_subdev_selection sel = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .target = s->target,
+ .r = s->r,
+ .flags = s->flags,
+ };
+ struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+ struct v4l2_pix_format_mplane *format_mp =
+ &video->active_fmt.fmt.pix_mp;
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
+ __func__, sel.target, s->target);
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
+ && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+ return -EINVAL;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sel);
+ if (!ret) {
+ s->r = sel.r;
+ s->flags = sel.flags;
+ format->width = s->r.width;
+ format->height = s->r.height;
+ format_mp->width = s->r.width;
+ format_mp->height = s->r.height;
+ ret = __video_try_fmt(video, &video->active_fmt,
+ video->is_mp);
+ if (ret < 0)
+ return ret;
+ break;
+ }
+ if (ret != -ENOIOCTLCMD)
+ break;
+ }
+
+ st_debug(ST_VIDEO, "ret = 0x%x, -EINVAL = 0x%x\n", ret, -EINVAL);
+
+ return ret;
+}
+
+int video_g_ctrl(struct file *file, void *fh,
+ struct v4l2_control *ctrls)
+{
+ struct v4l2_subdev *subdev;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL)
+ return -EINVAL;
+
+ ret = v4l2_g_ctrl(subdev->ctrl_handler, ctrls);
+
+ return ret;
+}
+
+static int video_s_ctrl(struct file *file, void *fh,
+ struct v4l2_control *ctrl)
+{
+ struct v4l2_subdev *subdev;
+ struct v4l2_fh *vfh;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL)
+ return -EINVAL;
+
+ vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+ if (!vfh->ctrl_handler)
+ return -ENOTTY;
+
+ ret = v4l2_s_ctrl(vfh,subdev->ctrl_handler, ctrl);
+
+ return ret;
+}
+
+static int video_query_ext_ctrl(struct file *file, void *fh,
+ struct v4l2_query_ext_ctrl *qec)
+{
+ struct v4l2_subdev *subdev;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL)
+ return -EINVAL;
+
+ ret = v4l2_query_ext_ctrl(subdev->ctrl_handler, qec);
+
+ return ret;
+}
+
+static int video_g_ext_ctrls(struct file *file, void *fh,
+ struct v4l2_ext_controls *ctrls)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct v4l2_subdev *subdev;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL)
+ return -EINVAL;
+
+ ret = v4l2_g_ext_ctrls(subdev->ctrl_handler,
+ vdev, subdev->v4l2_dev->mdev, ctrls);
+
+ return ret;
+}
+
+static int video_queryctrl(struct file *file, void *fh,
+ struct v4l2_queryctrl *qc)
+{
+#if 1
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ int ret = 0;
+
+ sensor = stfcamss_find_sensor(entity);
+ if(sensor) {
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ ret= v4l2_queryctrl(subdev->ctrl_handler, qc);
+ } else {
+ // st_err(ST_VIDEO, "== [%s] Please configure pipeline first ==\n", __func__);
+ return -EINVAL;
+ }
+
+ return ret;
+#else
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct v4l2_subdev *subdev;
+ struct v4l2_fh *vfh;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL )
+ return -EINVAL;
+
+ vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+ ret= v4l2_queryctrl(subdev->ctrl_handler, qc);
+
+ return ret;
+#endif
+}
+
+static int video_s_ext_ctrls(struct file *file, void *fh,
+ struct v4l2_ext_controls *ctrls)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct v4l2_subdev *subdev;
+ struct v4l2_fh *vfh;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL )
+ return -EINVAL;
+
+ vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+ if (!vfh->ctrl_handler)
+ return -ENOTTY;
+ ret = v4l2_s_ext_ctrls(vfh, subdev->ctrl_handler,
+ vdev, subdev->v4l2_dev->mdev, ctrls);
+
+ return ret;
+}
+
+static int video_try_ext_ctrls(struct file *file, void *fh,
+ struct v4l2_ext_controls *ctrls)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct v4l2_subdev *subdev;
+ struct v4l2_fh *vfh;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL )
+ return -EINVAL;
+
+ vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh , ctrl_handler);
+ if (!vfh->ctrl_handler)
+ return -ENOTTY;
+ ret = v4l2_try_ext_ctrls(vfh->ctrl_handler,
+ vdev, subdev->v4l2_dev->mdev, ctrls);
+
+ return ret;
+}
+
+static int video_querymenu(struct file *file, void *fh,
+ struct v4l2_querymenu *qm)
+{
+ struct v4l2_subdev *subdev;
+ struct v4l2_fh *vfh;
+ int ret;
+
+ subdev = get_senname(file,__func__);
+ if( subdev == NULL )
+ return -EINVAL;
+ ret = v4l2_querymenu(subdev->ctrl_handler, qm);
+
+ return ret;
+}
+
+static const struct v4l2_ioctl_ops stf_vid_ioctl_ops = {
+ .vidioc_querycap = video_querycap,
+ .vidioc_enum_fmt_vid_cap = video_enum_fmt,
+ .vidioc_enum_framesizes = video_enum_framesizes,
+ .vidioc_enum_frameintervals = video_enum_frameintervals,
+ .vidioc_g_fmt_vid_cap = video_g_fmt,
+ .vidioc_s_fmt_vid_cap = video_s_fmt,
+ .vidioc_try_fmt_vid_cap = video_try_fmt,
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+ .vidioc_enum_input = video_enum_input,
+ .vidioc_g_input = video_g_input,
+ .vidioc_s_input = video_s_input,
+ .vidioc_g_parm = video_g_parm,
+ .vidioc_s_parm = video_s_parm,
+ .vidioc_s_selection = video_s_selection,
+ .vidioc_g_selection = video_g_selection,
+ .vidioc_g_ctrl = video_g_ctrl,
+ .vidioc_s_ctrl = video_s_ctrl,
+ .vidioc_g_ext_ctrls = video_g_ext_ctrls,
+ .vidioc_queryctrl = video_queryctrl,
+ .vidioc_s_ext_ctrls = video_s_ext_ctrls,
+ .vidioc_try_ext_ctrls = video_try_ext_ctrls,
+ //.vidioc_query_ext_ctrl = video_query_ext_ctrl,
+ //.vidioc_querymenu = video_querymenu,
+
+};
+
+static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_mp = {
+ .vidioc_querycap = video_querycap,
+ .vidioc_enum_fmt_vid_cap = video_enum_fmt,
+ .vidioc_enum_framesizes = video_enum_framesizes,
+ .vidioc_enum_frameintervals = video_enum_frameintervals,
+ .vidioc_g_fmt_vid_cap_mplane = video_g_fmt_mp,
+ .vidioc_s_fmt_vid_cap_mplane = video_s_fmt_mp,
+ .vidioc_try_fmt_vid_cap_mplane = video_try_fmt_mp,
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+ .vidioc_enum_input = video_enum_input,
+ .vidioc_g_input = video_g_input,
+ .vidioc_s_input = video_s_input,
+ .vidioc_g_parm = video_g_parm,
+ .vidioc_s_parm = video_s_parm,
+ .vidioc_s_selection = video_s_selection,
+ .vidioc_g_selection = video_g_selection,
+ .vidioc_g_ctrl = video_g_ctrl,
+ .vidioc_s_ctrl = video_s_ctrl,
+ .vidioc_g_ext_ctrls = video_g_ext_ctrls,
+ .vidioc_queryctrl = video_queryctrl,
+ .vidioc_s_ext_ctrls = video_s_ext_ctrls,
+ .vidioc_try_ext_ctrls = video_try_ext_ctrls,
+// .vidioc_querymenu = video_querymenu,
+// .vidioc_query_ext_ctrl = video_query_ext_ctrl,
+};
+
+static int video_open(struct file *file)
+{
+ struct video_device *vdev = video_devdata(file);
+ struct stfcamss_video *video = video_drvdata(file);
+ struct v4l2_fh *vfh;
+ int ret;
+
+ mutex_lock(&video->lock);
+
+ vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
+ if (vfh == NULL) {
+ ret = -ENOMEM;
+ goto error_alloc;
+ }
+
+ v4l2_fh_init(vfh, vdev);
+ v4l2_fh_add(vfh);
+
+ file->private_data = vfh;
+
+#ifdef USE_MEDIA_PIPELINE
+ ret = v4l2_pipeline_pm_get(&vdev->entity);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to power up pipeline: %d\n", ret);
+ goto error_pm_use;
+ }
+#else
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ int i = 0;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, core, s_power, 1);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ goto error_power;
+ i++;
+ }
+#endif
+ mutex_unlock(&video->lock);
+
+ return 0;
+#ifndef USE_MEDIA_PIPELINE
+error_power:
+ entity = &vdev->entity;
+ while (i--) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ v4l2_subdev_call(subdev, core, s_power, 0);
+ }
+#endif
+error_pm_use:
+ v4l2_fh_release(file);
+error_alloc:
+ mutex_unlock(&video->lock);
+ return ret;
+}
+
+static int video_release(struct file *file)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ vb2_fop_release(file);
+#ifdef USE_MEDIA_PIPELINE
+ v4l2_pipeline_pm_put(&vdev->entity);
+#else
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ v4l2_subdev_call(subdev, core, s_power, 0);
+ }
+#endif
+ file->private_data = NULL;
+
+ return 0;
+}
+
+static const struct v4l2_file_operations stf_vid_fops = {
+ .owner = THIS_MODULE,
+ .unlocked_ioctl = video_ioctl2,
+ .open = video_open,
+ .release = video_release,
+ .poll = vb2_fop_poll,
+ .mmap = vb2_fop_mmap,
+ .read = vb2_fop_read,
+};
+
+static void stf_video_release(struct video_device *vdev)
+{
+ struct stfcamss_video *video = video_get_drvdata(vdev);
+
+ media_entity_cleanup(&vdev->entity);
+
+ mutex_destroy(&video->q_lock);
+ mutex_destroy(&video->lock);
+}
+
+int stf_video_register(struct stfcamss_video *video,
+ struct v4l2_device *v4l2_dev,
+ const char *name, int is_mp)
+{
+ struct video_device *vdev;
+ struct vb2_queue *q;
+ struct media_pad *pad = &video->pad;
+ int ret;
+
+ vdev = &video->vdev;
+
+ mutex_init(&video->q_lock);
+
+ q = &video->vb2_q;
+ q->drv_priv = video;
+ q->mem_ops = &vb2_dma_contig_memops;
+ q->ops = &stf_video_vb2_q_ops;
+ q->type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
+ q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+ q->buf_struct_size = sizeof(struct stfcamss_buffer);
+ q->dev = video->stfcamss->dev;
+ q->lock = &video->q_lock;
+ ret = vb2_queue_init(q);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to init vb2 queue: %d\n", ret);
+ goto err_vb2_init;
+ }
+
+ pad->flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vdev->entity, 1, pad);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to init video entity: %d\n",
+ ret);
+ goto err_vb2_init;
+ }
+
+ mutex_init(&video->lock);
+
+ if (video->id == VIN_LINE_WR) {
+ video->formats = formats_pix_st7110_wr;
+ video->nformats = ARRAY_SIZE(formats_pix_st7110_wr);
+ video->bpl_alignment = 8;
+ } else if (video->id == VIN_LINE_ISP0
+ || video->id == VIN_LINE_ISP1) { // ISP0/ISP1
+ video->formats = formats_pix_st7110_isp;
+ video->nformats = ARRAY_SIZE(formats_pix_st7110_isp);
+ video->bpl_alignment = 8;
+ } else {
+ video->formats = formats_raw_st7110_isp;
+ video->nformats = ARRAY_SIZE(formats_raw_st7110_isp);
+ video->bpl_alignment = 16 * 8;
+ }
+ video->is_mp = is_mp;
+
+ ret = stf_video_init_format(video, is_mp);
+ if (ret < 0) {
+ st_err(ST_VIDEO, "Failed to init format: %d\n", ret);
+ goto err_vid_init_format;
+ }
+
+ vdev->fops = &stf_vid_fops;
+ vdev->device_caps = is_mp ? V4L2_CAP_VIDEO_CAPTURE_MPLANE :
+ V4L2_CAP_VIDEO_CAPTURE;
+ vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ vdev->ioctl_ops = is_mp ? &stf_vid_ioctl_ops_mp : &stf_vid_ioctl_ops;
+ vdev->release = stf_video_release;
+ vdev->v4l2_dev = v4l2_dev;
+ vdev->vfl_dir = VFL_DIR_RX;
+ vdev->queue = &video->vb2_q;
+ vdev->lock = &video->lock;
+ strlcpy(vdev->name, name, sizeof(vdev->name));
+
+ ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to register video device: %d\n",
+ ret);
+ goto err_vid_reg;
+ }
+
+ video_set_drvdata(vdev, video);
+ return 0;
+
+err_vid_reg:
+err_vid_init_format:
+ media_entity_cleanup(&vdev->entity);
+ mutex_destroy(&video->lock);
+err_vb2_init:
+ mutex_destroy(&video->q_lock);
+ return ret;
+}
+
+void stf_video_unregister(struct stfcamss_video *video)
+{
+ vb2_video_unregister_device(&video->vdev);
+}
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_VIDEO_H
+#define STF_VIDEO_H
+
+#include <linux/mutex.h>
+#include <media/videobuf2-v4l2.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+
+#define STFCAMSS_FRAME_MIN_WIDTH 1
+#define STFCAMSS_FRAME_MAX_WIDTH 8191
+#define STFCAMSS_FRAME_MIN_HEIGHT 1
+#define STFCAMSS_FRAME_MAX_HEIGHT_RDI 8191
+#define STFCAMSS_FRAME_MAX_HEIGHT_PIX 4096
+#define STFCAMSS_FRAME_WIDTH_ALIGN 8
+
+struct stfcamss_buffer {
+ struct vb2_v4l2_buffer vb;
+ dma_addr_t addr[3];
+ struct list_head queue;
+};
+
+struct stfcamss_video;
+
+struct stfcamss_video_ops {
+ int (*queue_buffer)(struct stfcamss_video *vid,
+ struct stfcamss_buffer *buf);
+ int (*flush_buffers)(struct stfcamss_video *vid,
+ enum vb2_buffer_state state);
+};
+
+struct fract {
+ u8 numerator;
+ u8 denominator;
+};
+
+struct stfcamss_format_info {
+ u32 code;
+ u32 pixelformat;
+ u8 planes;
+ struct fract hsub[3];
+ struct fract vsub[3];
+ u8 bpp[3];
+};
+
+struct stfcamss_video {
+ struct stfcamss *stfcamss;
+ u8 id;
+ struct vb2_queue vb2_q;
+ struct video_device vdev;
+ struct media_pad pad;
+ struct media_pipeline pipe;
+ struct v4l2_format active_fmt;
+ enum v4l2_buf_type type;
+ const struct stfcamss_video_ops *ops;
+ struct mutex lock;
+ struct mutex q_lock;
+ unsigned int bpl_alignment;
+ const struct stfcamss_format_info *formats;
+ unsigned int nformats;
+ unsigned int is_mp;
+};
+
+int stf_video_register(struct stfcamss_video *video,
+ struct v4l2_device *v4l2_dev, const char *name, int is_mp);
+
+void stf_video_unregister(struct stfcamss_video *video);
+
+#endif /* STF_VIDEO_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+
+#include "stfcamss.h"
+
+#define STF_VIN_NAME "stf_vin"
+
+#define vin_line_array(ptr_line) \
+ ((const struct vin_line (*)[]) &(ptr_line[-(ptr_line->id)]))
+
+#define line_to_vin2_dev(ptr_line) \
+ container_of(vin_line_array(ptr_line), struct stf_vin2_dev, line)
+
+#define VIN_FRAME_DROP_MAX_VAL 30
+#define VIN_FRAME_DROP_MIN_VAL 4
+
+// #define VIN_TWO_BUFFER
+
+static const struct vin2_format vin2_formats_st7110[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, 16},
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 12},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 12},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 12},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 12},
+};
+
+static void vin_buffer_done(struct vin_line *line, struct vin_params *params);
+static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output);
+static void vin_output_init_addrs(struct vin_line *line);
+static void vin_init_outputs(struct vin_line *line);
+
+static char *get_line_subdevname(int line_id)
+{
+ char *name = NULL;
+
+ switch (line_id) {
+ case VIN_LINE_WR:
+ name = "wr";
+ break;
+ case VIN_LINE_ISP0:
+ name = "isp0";
+ break;
+ case VIN_LINE_ISP1:
+ name = "isp1";
+ break;
+ case VIN_LINE_ISP0_RAW:
+ name = "isp0_raw";
+ break;
+ case VIN_LINE_ISP1_RAW:
+ name = "isp1_raw";
+ break;
+ default:
+ name = "unknow";
+ break;
+ }
+ return name;
+}
+
+int stf_vin_subdev_init(struct stfcamss *stfcamss)
+{
+ struct stf_vin_dev *vin;
+ struct device *dev = stfcamss->dev;
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ int ret = 0, i;
+
+ vin_dev->stfcamss = stfcamss;
+ vin_dev->hw_ops = &vin_ops;
+ vin_dev->hw_ops->isr_buffer_done = vin_buffer_done;
+
+ vin = stfcamss->vin;
+ atomic_set(&vin_dev->ref_count, 0);
+
+ ret = devm_request_irq(dev,
+ vin->irq, vin_dev->hw_ops->vin_wr_irq_handler,
+ 0, "vin_axiwr_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request irq\n");
+ goto out;
+ }
+
+ ret = devm_request_irq(dev,
+ vin->isp0_irq, vin_dev->hw_ops->vin_isp_irq_handler,
+ 0, "vin_isp0_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp0 irq\n");
+ goto out;
+ }
+
+ ret = devm_request_irq(dev,
+ vin->isp1_irq, vin_dev->hw_ops->vin_isp_irq_handler,
+ 0, "vin_isp1_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp1 irq\n");
+ goto out;
+ }
+
+ vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
+ vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
+#if 0
+ /* Reset device */
+ /*Do not configure the CLK before powering on the device,
+ add vin_power_on() to vin_set_power() 2021 1111 */
+ ret = vin_dev->hw_ops->vin_clk_init(vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "Failed to reset device\n");
+ goto out;
+ }
+
+ // /* set the sysctl config */
+ // ret = vin_dev->hw_ops->vin_config_set(vin_dev);
+// if (ret) {
+// st_err(ST_VIN, "Failed to config device\n");
+// goto out;
+// }
+#endif
+ mutex_init(&vin_dev->power_lock);
+ vin_dev->power_count = 0;
+
+ for (i = VIN_LINE_WR; i < VIN_LINE_MAX; i++) {
+ struct vin_line *l = &vin_dev->line[i];
+ int is_mp;
+
+ is_mp = i == VIN_LINE_WR ? false : true;
+ is_mp = false;
+ l->video_out.type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ l->video_out.stfcamss = stfcamss;
+ l->id = i;
+ l->sdev_type = VIN_DEV_TYPE;
+ l->formats = vin2_formats_st7110;
+ l->nformats = ARRAY_SIZE(vin2_formats_st7110);
+ spin_lock_init(&l->output_lock);
+
+ mutex_init(&l->stream_lock);
+ l->stream_count = 0;
+ mutex_init(&l->power_lock);
+ l->power_count = 0;
+ }
+
+ return 0;
+out:
+ return ret;
+}
+
+static int vin_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+
+ mutex_lock(&line->power_lock);
+ if (on) {
+ if (line->power_count == 0)
+ vin_init_outputs(line);
+ line->power_count++;
+ } else {
+ if (line->power_count == 0) {
+ st_err(ST_VIN,
+ "line power off on power_count == 0\n");
+ goto exit_line;
+ }
+ line->power_count--;
+ }
+exit_line:
+ mutex_unlock(&line->power_lock);
+
+ mutex_lock(&vin_dev->power_lock);
+ if (on) {
+ if (vin_dev->power_count == 0) {
+ vin_dev->hw_ops->vin_power_on(vin_dev,on);
+ vin_dev->hw_ops->vin_clk_enable(vin_dev);
+ vin_dev->hw_ops->vin_config_set(vin_dev);
+ }
+ vin_dev->power_count++;
+ } else {
+ if (vin_dev->power_count == 0) {
+ st_err(ST_VIN,
+ "vin_dev power off on power_count == 0\n");
+ goto exit;
+ }
+ if (vin_dev->power_count == 1) {
+ vin_dev->hw_ops->vin_clk_disable(vin_dev);
+ vin_dev->hw_ops->vin_power_on(vin_dev,on);
+ }
+ vin_dev->power_count--;
+ }
+exit:
+
+ mutex_unlock(&vin_dev->power_lock);
+
+ return 0;
+}
+
+static int vin_enable_output(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ unsigned long flags;
+ unsigned int frame_skip = 0;
+ struct media_entity *sensor;
+
+ sensor = stfcamss_find_sensor(&line->subdev.entity);
+ if (sensor) {
+ struct v4l2_subdev *subdev =
+ media_entity_to_v4l2_subdev(sensor);
+
+ v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
+ frame_skip += VIN_FRAME_DROP_MIN_VAL;
+ if (frame_skip > VIN_FRAME_DROP_MAX_VAL)
+ frame_skip = VIN_FRAME_DROP_MAX_VAL;
+ st_debug(ST_VIN, "%s, frame_skip %d\n", __func__, frame_skip);
+ }
+
+ spin_lock_irqsave(&line->output_lock, flags);
+ output->frame_skip = frame_skip;
+
+ output->state = VIN_OUTPUT_IDLE;
+
+ output->buf[0] = vin_buf_get_pending(output);
+#ifdef VIN_TWO_BUFFER
+ if (line->id == VIN_LINE_WR)
+ output->buf[1] = vin_buf_get_pending(output);
+#endif
+ if (!output->buf[0] && output->buf[1]) {
+ output->buf[0] = output->buf[1];
+ output->buf[1] = NULL;
+ }
+
+ if (output->buf[0])
+ output->state = VIN_OUTPUT_SINGLE;
+
+#ifdef VIN_TWO_BUFFER
+ if (output->buf[1] && line->id == VIN_LINE_WR)
+ output->state = VIN_OUTPUT_CONTINUOUS;
+#endif
+ output->sequence = 0;
+
+ vin_output_init_addrs(line);
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return 0;
+}
+
+static int vin_disable_output(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ unsigned long flags;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ output->state = VIN_OUTPUT_OFF;
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return 0;
+}
+
+static int vin_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+
+ if (line->id == VIN_LINE_WR) {
+ mutex_lock(&line->stream_lock);
+ if (enable) {
+ if (line->stream_count == 0) {
+ vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
+ vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 1);
+ }
+ line->stream_count++;
+ } else {
+ if (line->stream_count == 1) {
+ vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
+ vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 0);
+ }
+ line->stream_count--;
+ }
+ mutex_unlock(&line->stream_lock);
+ }
+
+ if (enable)
+ vin_enable_output(line);
+ else
+ vin_disable_output(line);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__vin_get_format(struct vin_line *line,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(&line->subdev, state, pad);
+ return &line->fmt[pad];
+}
+
+static void vin_try_format(struct vin_line *line,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ unsigned int i;
+
+ switch (pad) {
+ case STF_VIN_PAD_SINK:
+ /* Set format on sink pad */
+
+ for (i = 0; i < line->nformats; i++)
+ if (fmt->code == line->formats[i].code)
+ break;
+
+ /* If not found, use UYVY as default */
+ if (i >= line->nformats)
+ fmt->code = MEDIA_BUS_FMT_RGB565_2X8_LE;
+
+ fmt->width = clamp_t(u32,
+ fmt->width, 1, STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height, 1, STFCAMSS_FRAME_MAX_HEIGHT_PIX);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ break;
+
+ case STF_VIN_PAD_SRC:
+ /* Set and return a format same as sink pad */
+ *fmt = *__vin_get_format(line, state, STF_VIN_PAD_SINK, which);
+ break;
+ }
+
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int vin_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+
+ if (code->index >= line->nformats)
+ return -EINVAL;
+ if (code->pad == STF_VIN_PAD_SINK) {
+ code->code = line->formats[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __vin_get_format(line, state, STF_VIN_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+
+ return 0;
+}
+
+static int vin_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ vin_try_format(line, state, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ vin_try_format(line, state, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int vin_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __vin_get_format(line, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int vin_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __vin_get_format(line, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+
+ if (fmt->pad == STF_VIN_PAD_SINK) {
+ /* Propagate the format from sink to source */
+ format = __vin_get_format(line, state, STF_VIN_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ vin_try_format(line, state, STF_VIN_PAD_SRC, format,
+ fmt->which);
+ }
+
+ return 0;
+}
+
+static int vin_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_VIN_PAD_SINK,
+ .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
+ V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return vin_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static void vin_output_init_addrs(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ dma_addr_t ping_addr;
+ dma_addr_t pong_addr;
+ dma_addr_t y_addr, uv_addr;
+ struct v4l2_mbus_framefmt *format;
+
+ output->active_buf = 0;
+
+ if (output->buf[0]) {
+ ping_addr = output->buf[0]->addr[0];
+ y_addr = output->buf[0]->addr[0];
+ uv_addr = output->buf[0]->addr[1];
+
+ } else
+ ping_addr = 0;
+
+ if (output->buf[1])
+ pong_addr = output->buf[1]->addr[0];
+ else
+ pong_addr = ping_addr;
+
+ switch (line->id) {
+ case VIN_LINE_WR: // wr
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, ping_addr);
+#ifdef VIN_TWO_BUFFER
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, pong_addr);
+#else
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, ping_addr);
+#endif
+ break;
+ case VIN_LINE_ISP0: // isp0
+ case VIN_LINE_ISP1: // isp1
+
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ line->id - VIN_LINE_ISP0,
+ y_addr, uv_addr);
+
+ break;
+ case VIN_LINE_ISP0_RAW: // isp0_raw
+ case VIN_LINE_ISP1_RAW: // isp1_raw
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+ line->id - VIN_LINE_ISP0_RAW, y_addr);
+ break;
+ default:
+ break;
+ }
+}
+
+static void vin_init_outputs(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+
+ output->state = VIN_OUTPUT_OFF;
+ output->buf[0] = NULL;
+ output->buf[1] = NULL;
+ output->active_buf = 0;
+ INIT_LIST_HEAD(&output->pending_bufs);
+}
+
+static void vin_buf_add_pending(struct vin_output *output,
+ struct stfcamss_buffer *buffer)
+{
+ INIT_LIST_HEAD(&buffer->queue);
+ list_add_tail(&buffer->queue, &output->pending_bufs);
+}
+
+static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output)
+{
+ struct stfcamss_buffer *buffer = NULL;
+
+ if (!list_empty(&output->pending_bufs)) {
+ buffer = list_first_entry(&output->pending_bufs,
+ struct stfcamss_buffer,
+ queue);
+ list_del(&buffer->queue);
+ }
+
+ return buffer;
+}
+
+static void vin_output_checkpending(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+
+ if (output->state == VIN_OUTPUT_STOPPING) {
+ /* Release last buffer when hw is idle */
+ if (output->last_buffer) {
+ // vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
+ // VB2_BUF_STATE_DONE);
+ vin_buf_add_pending(output, output->last_buffer);
+ output->last_buffer = NULL;
+ }
+ output->state = VIN_OUTPUT_IDLE;
+
+ /* Buffers received in stopping state are queued in */
+ /* dma pending queue, start next capture here */
+ output->buf[0] = vin_buf_get_pending(output);
+#ifdef VIN_TWO_BUFFER
+ if (line->id == VIN_LINE_WR)
+ output->buf[1] = vin_buf_get_pending(output);
+#endif
+
+ if (!output->buf[0] && output->buf[1]) {
+ output->buf[0] = output->buf[1];
+ output->buf[1] = NULL;
+ }
+
+ if (output->buf[0])
+ output->state = VIN_OUTPUT_SINGLE;
+
+#ifdef VIN_TWO_BUFFER
+ if (output->buf[1] && line->id == VIN_LINE_WR)
+ output->state = VIN_OUTPUT_CONTINUOUS;
+#endif
+ vin_output_init_addrs(line);
+ }
+}
+
+static void vin_buf_update_on_last(struct vin_line *line)
+{
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ struct vin_output *output = &line->output;
+
+ switch (output->state) {
+ case VIN_OUTPUT_CONTINUOUS:
+ output->state = VIN_OUTPUT_SINGLE;
+ output->active_buf = !output->active_buf;
+ break;
+ case VIN_OUTPUT_SINGLE:
+ output->state = VIN_OUTPUT_STOPPING;
+ break;
+ default:
+ st_err_ratelimited(ST_VIN,
+ "Last buff in wrong state! %d\n",
+ output->state);
+ break;
+ }
+}
+
+static void vin_buf_update_on_next(struct vin_line *line)
+{
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ struct vin_output *output = &line->output;
+
+ switch (output->state) {
+ case VIN_OUTPUT_CONTINUOUS:
+ output->active_buf = !output->active_buf;
+ break;
+ case VIN_OUTPUT_SINGLE:
+ default:
+#ifdef VIN_TWO_BUFFER
+ if (line->id == VIN_LINE_WR)
+ st_err_ratelimited(ST_VIN,
+ "Next buf in wrong state! %d\n",
+ output->state);
+#endif
+ break;
+ }
+}
+
+static void vin_buf_update_on_new(struct vin_line *line,
+ struct vin_output *output,
+ struct stfcamss_buffer *new_buf)
+{
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ int inactive_idx;
+
+ switch (output->state) {
+ case VIN_OUTPUT_SINGLE:
+#ifdef VIN_TWO_BUFFER
+ inactive_idx = !output->active_buf;
+
+ if (!output->buf[inactive_idx] && line->id == VIN_LINE_WR) {
+ output->buf[inactive_idx] = new_buf;
+ if (inactive_idx)
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+ output->buf[1]->addr[0]);
+ else
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+ output->buf[0]->addr[0]);
+ output->state = VIN_OUTPUT_CONTINUOUS;
+
+ } else {
+ vin_buf_add_pending(output, new_buf);
+ if (line->id == VIN_LINE_WR)
+ st_warn(ST_VIN, "Inactive buffer is busy\n");
+ }
+#else
+ vin_buf_add_pending(output, new_buf);
+#endif
+ break;
+ case VIN_OUTPUT_IDLE:
+ st_warn(ST_VIN, "Output idle buffer set!\n");
+ if (!output->buf[0]) {
+ output->buf[0] = new_buf;
+ vin_output_init_addrs(line);
+ output->state = VIN_OUTPUT_SINGLE;
+ } else {
+ vin_buf_add_pending(output, new_buf);
+ st_warn(ST_VIN, "Output idle with buffer set!\n");
+ }
+ break;
+ case VIN_OUTPUT_STOPPING:
+ if (output->last_buffer) {
+ output->buf[output->active_buf] = output->last_buffer;
+ output->last_buffer = NULL;
+ } else
+ st_err(ST_VIN, "stop state lost lastbuffer!\n");
+ output->state = VIN_OUTPUT_SINGLE;
+ // vin_output_checkpending(line);
+ vin_buf_add_pending(output, new_buf);
+ break;
+ case VIN_OUTPUT_CONTINUOUS:
+ default:
+ vin_buf_add_pending(output, new_buf);
+ break;
+ }
+}
+
+static void vin_buf_flush_pending(struct vin_output *output,
+ enum vb2_buffer_state state)
+{
+ struct stfcamss_buffer *buf;
+ struct stfcamss_buffer *t;
+
+ list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
+ vb2_buffer_done(&buf->vb.vb2_buf, state);
+ list_del(&buf->queue);
+ }
+}
+
+static void vin_buffer_done(struct vin_line *line, struct vin_params *params)
+{
+ struct stfcamss_buffer *ready_buf;
+ struct vin_output *output = &line->output;
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ struct v4l2_mbus_framefmt *format;
+ dma_addr_t *new_addr, y_addr, uv_addr;
+ unsigned long flags;
+ u32 active_index;
+ u64 ts = ktime_get_ns();
+
+ if (output->state == VIN_OUTPUT_OFF
+ || output->state == VIN_OUTPUT_STOPPING
+ || output->state == VIN_OUTPUT_RESERVED
+ || output->state == VIN_OUTPUT_IDLE) {
+ st_warn(ST_VIN,
+ "output state no ready %d!, %d\n",
+ output->state, line->id);
+ return;
+ }
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ if (output->frame_skip) {
+ output->frame_skip--;
+ goto out_unlock;
+ }
+
+ active_index = output->active_buf;
+
+ ready_buf = output->buf[active_index];
+ if (!ready_buf) {
+ st_err_ratelimited(ST_VIN,
+ "Missing ready buf %d %d!\n",
+ active_index, output->state);
+ active_index = !active_index;
+ ready_buf = output->buf[active_index];
+ if (!ready_buf) {
+ st_err_ratelimited(ST_VIN,
+ "Missing ready buf 2 %d %d!\n",
+ active_index, output->state);
+ goto out_unlock;
+ }
+ }
+
+ /* Get next buffer */
+ output->buf[active_index] = vin_buf_get_pending(output);
+ if (!output->buf[active_index]) {
+ /* No next buffer - set same address */
+ new_addr = ready_buf->addr;
+ vin_buf_update_on_last(line);
+ } else {
+ new_addr = output->buf[active_index]->addr;
+ vin_buf_update_on_next(line);
+ }
+
+ switch (line->id) {
+ case VIN_LINE_WR: // wr
+#ifdef VIN_TWO_BUFFER
+ if (active_index)
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+ new_addr[0]);
+ else
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+ new_addr[0]);
+#else
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+ new_addr[0]);
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+ new_addr[0]);
+#endif
+ break;
+ case VIN_LINE_ISP0: // isp0
+ case VIN_LINE_ISP1: // isp1
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ line->id - VIN_LINE_ISP0,
+ new_addr[0], new_addr[1]);
+ break;
+ case VIN_LINE_ISP0_RAW: // isp0_raw
+ case VIN_LINE_ISP1_RAW: // isp1_raw
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+ line->id - VIN_LINE_ISP0_RAW, new_addr[0]);
+ break;
+
+ default:
+ break;
+ }
+
+ if (output->state == VIN_OUTPUT_STOPPING)
+ output->last_buffer = ready_buf;
+ else {
+ ready_buf->vb.vb2_buf.timestamp = ts;
+ ready_buf->vb.sequence = output->sequence++;
+ vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+ }
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return;
+
+out_unlock:
+ spin_unlock_irqrestore(&line->output_lock, flags);
+}
+
+static int vin_queue_buffer(struct stfcamss_video *vid,
+ struct stfcamss_buffer *buf)
+{
+ struct vin_line *line = container_of(vid, struct vin_line, video_out);
+ struct vin_output *output;
+ unsigned long flags;
+
+
+ output = &line->output;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ vin_buf_update_on_new(line, output, buf);
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+
+ return 0;
+}
+
+static int vin_flush_buffers(struct stfcamss_video *vid,
+ enum vb2_buffer_state state)
+{
+ struct vin_line *line = container_of(vid, struct vin_line, video_out);
+ struct vin_output *output = &line->output;
+ unsigned long flags;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ vin_buf_flush_pending(output, state);
+ if (output->buf[0])
+ vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
+
+ if (output->buf[1])
+ vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
+
+ if (output->last_buffer) {
+ vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
+ output->last_buffer = NULL;
+ }
+ output->buf[0] = output->buf[1] = NULL;
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return 0;
+}
+
+static int vin_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if (flags & MEDIA_LNK_FL_ENABLED)
+ if (media_entity_remote_pad(local))
+ return -EBUSY;
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops vin_core_ops = {
+ .s_power = vin_set_power,
+};
+
+static const struct v4l2_subdev_video_ops vin_video_ops = {
+ .s_stream = vin_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops vin_pad_ops = {
+ .enum_mbus_code = vin_enum_mbus_code,
+ .enum_frame_size = vin_enum_frame_size,
+ .get_fmt = vin_get_format,
+ .set_fmt = vin_set_format,
+};
+
+static const struct v4l2_subdev_ops vin_v4l2_ops = {
+ .core = &vin_core_ops,
+ .video = &vin_video_ops,
+ .pad = &vin_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vin_v4l2_internal_ops = {
+ .open = vin_init_formats,
+};
+
+static const struct stfcamss_video_ops stfcamss_vin_video_ops = {
+ .queue_buffer = vin_queue_buffer,
+ .flush_buffers = vin_flush_buffers,
+};
+
+static const struct media_entity_operations vin_media_ops = {
+ .link_setup = vin_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_vin_register(struct stf_vin2_dev *vin_dev, struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd;
+ struct device *dev = vin_dev->stfcamss->dev;
+ struct stfcamss_video *video_out;
+ struct media_pad *pads;
+ int ret;
+ int i;
+
+ for (i = 0; i < VIN_LINE_MAX; i++) {
+ char name[32];
+ char *sub_name = get_line_subdevname(i);
+ int is_mp;
+
+ is_mp = (i == VIN_LINE_ISP0) || (i == VIN_LINE_ISP1) ? true : false;
+ is_mp = false;
+ sd = &vin_dev->line[i].subdev;
+ pads = vin_dev->line[i].pads;
+ video_out = &vin_dev->line[i].video_out;
+ video_out->id = i;
+
+ v4l2_subdev_init(sd, &vin_v4l2_ops);
+ sd->internal_ops = &vin_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
+ STF_VIN_NAME, vin_dev->id, sub_name);
+ v4l2_set_subdevdata(sd, &vin_dev->line[i]);
+
+ ret = vin_init_formats(sd, NULL);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to init format: %d\n", ret);
+ goto err_init;
+ }
+
+ pads[STF_VIN_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_VIN_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function =
+ MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &vin_media_ops;
+ ret = media_entity_pads_init(&sd->entity,
+ STF_VIN_PADS_NUM, pads);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to init media entity: %d\n", ret);
+ goto err_init;
+ }
+
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to register subdev: %d\n", ret);
+ goto err_reg_subdev;
+ }
+
+ video_out->ops = &stfcamss_vin_video_ops;
+ video_out->bpl_alignment = 16 * 8;
+
+ snprintf(name, ARRAY_SIZE(name), "%s_%s%d",
+ sd->name, "video", i);
+ ret = stf_video_register(video_out, v4l2_dev, name, is_mp);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to register video node: %d\n",
+ ret);
+ goto err_vid_reg;
+ }
+
+ ret = media_create_pad_link(
+ &sd->entity, STF_VIN_PAD_SRC,
+ &video_out->vdev.entity, 0,
+ MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to link %s->%s entities: %d\n",
+ sd->entity.name, video_out->vdev.entity.name,
+ ret);
+ goto err_create_link;
+ }
+ }
+
+ return 0;
+
+err_create_link:
+ stf_video_unregister(video_out);
+err_vid_reg:
+ v4l2_device_unregister_subdev(sd);
+err_reg_subdev:
+ media_entity_cleanup(&sd->entity);
+err_init:
+ for (i--; i >= 0; i--) {
+ sd = &vin_dev->line[i].subdev;
+ video_out = &vin_dev->line[i].video_out;
+
+ stf_video_unregister(video_out);
+ v4l2_device_unregister_subdev(sd);
+ media_entity_cleanup(&sd->entity);
+ }
+ return ret;
+}
+
+int stf_vin_unregister(struct stf_vin2_dev *vin_dev)
+{
+ struct v4l2_subdev *sd;
+ struct stfcamss_video *video_out;
+ int i;
+
+ mutex_destroy(&vin_dev->power_lock);
+ for (i = 0; i < VIN_LINE_MAX; i++) {
+ sd = &vin_dev->line[i].subdev;
+ video_out = &vin_dev->line[i].video_out;
+
+ stf_video_unregister(video_out);
+ v4l2_device_unregister_subdev(sd);
+ media_entity_cleanup(&sd->entity);
+ mutex_destroy(&vin_dev->line[i].stream_lock);
+ mutex_destroy(&vin_dev->line[i].power_lock);
+ }
+ return 0;
+}
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STF_VIN_H
+#define STF_VIN_H
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <linux/spinlock_types.h>
+#include <video/stf-vin.h>
+#include <linux/platform_device.h>
+
+#include "stf_video.h"
+
+#define STF_VIN_PAD_SINK 0
+#define STF_VIN_PAD_SRC 1
+#define STF_VIN_PADS_NUM 2
+
+struct vin2_format {
+ u32 code;
+ u8 bpp;
+};
+
+enum vin_output_state {
+ VIN_OUTPUT_OFF,
+ VIN_OUTPUT_RESERVED,
+ VIN_OUTPUT_SINGLE,
+ VIN_OUTPUT_CONTINUOUS,
+ VIN_OUTPUT_IDLE,
+ VIN_OUTPUT_STOPPING
+};
+
+struct vin_output {
+ int active_buf;
+ struct stfcamss_buffer *buf[2];
+ struct stfcamss_buffer *last_buffer;
+ struct list_head pending_bufs;
+ enum vin_output_state state;
+ unsigned int sequence;
+ unsigned int frame_skip;
+};
+
+enum vin_line_id {
+ VIN_LINE_NONE = -1,
+ VIN_LINE_WR = 0,
+ VIN_LINE_ISP0 = 1,
+ VIN_LINE_ISP1 = 2,
+ VIN_LINE_ISP0_RAW = 3,
+ VIN_LINE_ISP1_RAW = 4,
+ VIN_LINE_MAX = 5
+};
+
+enum subdev_type;
+
+struct vin_line {
+ enum subdev_type sdev_type; // must be frist
+ enum vin_line_id id;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_VIN_PADS_NUM];
+ struct v4l2_mbus_framefmt fmt[STF_VIN_PADS_NUM];
+ struct stfcamss_video video_out;
+ struct mutex stream_lock;
+ int stream_count;
+ struct mutex power_lock;
+ int power_count;
+ struct vin_output output;
+ spinlock_t output_lock;
+ const struct vin2_format *formats;
+ unsigned int nformats;
+};
+
+struct stf_vin2_dev;
+
+struct vin_hw_ops {
+ int (*vin_clk_init)(struct stf_vin2_dev *vin_dev);
+ int (*vin_clk_enable)(struct stf_vin2_dev *vin_dev);
+ int (*vin_clk_disable)(struct stf_vin2_dev *vin_dev);
+ int (*vin_config_set)(struct stf_vin2_dev *vin_dev);
+ int (*vin_wr_stream_set)(struct stf_vin2_dev *vin_dev, int on);
+ void (*vin_wr_irq_enable)(struct stf_vin2_dev *vin_dev, int enable);
+ void (*vin_power_on)(struct stf_vin2_dev *vin_dev, int on);
+ void (*wr_rd_set_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t wr_addr, dma_addr_t rd_addr);
+ void (*vin_wr_set_ping_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t addr);
+ void (*vin_wr_set_pong_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t addr);
+ void (*vin_wr_get_ping_pong_status)(struct stf_vin2_dev *vin_dev);
+ void (*vin_isp_set_yuv_addr)(struct stf_vin2_dev *vin_dev,
+ int isp_id,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_raw_addr)(struct stf_vin2_dev *vin_dev,
+ int isp_id, dma_addr_t raw_addr);
+ irqreturn_t (*vin_wr_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_irq_handler)(int irq, void *priv);
+ void (*isr_buffer_done)(struct vin_line *line,
+ struct vin_params *params);
+};
+
+struct stf_vin2_dev {
+ struct stfcamss *stfcamss;
+ u8 id;
+ struct vin_line line[VIN_LINE_MAX];
+ struct vin_hw_ops *hw_ops;
+ atomic_t ref_count;
+ struct mutex power_lock;
+ int power_count;
+};
+
+extern int stf_vin_subdev_init(struct stfcamss *stfcamss);
+extern int stf_vin_register(struct stf_vin2_dev *vin_dev,
+ struct v4l2_device *v4l2_dev);
+extern int stf_vin_unregister(struct stf_vin2_dev *vin_dev);
+
+extern struct vin_hw_ops vin_ops;
+extern void dump_vin_reg(void *__iomem regbase);
+
+#endif /* STF_VIN_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include "stfcamss.h"
+#include <linux/of_graph.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+static int vin_rstgen_assert_reset(struct stf_vin_dev *vin)
+{
+ u32 val;
+ /*
+ * Software_RESET_assert1 (0x11840004)
+ * ------------------------------------
+ * bit[15] rstn_vin_src
+ * bit[16] rstn_ispslv_axi
+ * bit[17] rstn_vin_axi
+ * bit[18] rstn_vinnoc_axi
+ * bit[19] rstn_isp0_axi
+ * bit[20] rstn_isp0noc_axi
+ * bit[21] rstn_isp1_axi
+ * bit[22] rstn_isp1noc_axi
+ *
+ */
+ u32 val_reg_reset_config = 0x7f8000;
+
+ val = ioread32(vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+ val |= val_reg_reset_config;
+ iowrite32(val, vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+
+ val = ioread32(vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+ val &= ~(val_reg_reset_config);
+
+ iowrite32(val, vin->vin_top_rstgen_base + SOFTWARE_RESET_ASSERT1);
+
+ return 0;
+}
+
+static void vin_intr_clear(void __iomem * sysctrl_base)
+{
+ reg_set_bit(sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x1);
+ reg_set_bit(sysctrl_base, SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x0);
+}
+
+static irqreturn_t stf_vin_wr_irq_handler(int irq, void *priv)
+{
+#if 0
+
+ static struct vin_params params;
+ static struct vin_params vparams;
+ struct stf_vin2_dev *vin_dev = priv;
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ vin_dev->hw_ops->isr_buffer_done(&vin_dev->line[VIN_LINE_WR], ¶ms);
+
+ vin_intr_clear(vin->sysctrl_base);
+#endif
+ return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_irq_handler(int irq, void *priv)
+{
+ static struct vin_params params;
+ struct stf_vin2_dev *vin_dev = priv;
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase;
+ u32 int_status, value;
+ int isp_id = irq == vin->isp0_irq ? 0 : 1;
+
+ if (isp_id == 0)
+ ispbase = vin->isp_isp0_base;
+ else
+ ispbase = vin->isp_isp1_base;
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+ // if (int_status & BIT(24))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP0 + isp_id], ¶ms);
+
+ value = reg_read(ispbase, ISP_REG_CIS_MODULE_CFG);
+ if ((value & BIT(19)) && (int_status & BIT(25)))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP0_RAW + isp_id], ¶ms);
+
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0, int_status);
+
+ return IRQ_HANDLED;
+}
+
+
+static int stf_vin_clk_init(struct stf_vin2_dev *vin_dev)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ return 0;
+
+}
+extern void stf_dvp_io_pad_config(struct stf_vin_dev *vin);
+void stf_do_port_configure(struct stfcamss *stfcamss)
+{
+ struct stf_vin_dev *vin = stfcamss->vin;
+ struct device *dev = stfcamss->dev;
+ struct device_node *node = NULL;
+ struct device_node *remote = NULL;
+
+ for_each_endpoint_of_node(dev->of_node, node) {
+ struct stfcamss_async_subdev *csd;
+ struct v4l2_async_subdev *asd;
+
+ if (!of_device_is_available(node))
+ continue;
+ remote = of_graph_get_remote_port_parent(node);
+ if (!remote)
+ st_err(ST_VIN, "Cannot get remote parent1\n");
+
+ list_for_each_entry(asd, &stfcamss->notifier.asd_list, asd_list) {
+ csd = container_of(asd, struct stfcamss_async_subdev, asd);
+ switch(csd->port){
+ case CSI2RX0_PORT_NUMBER:
+ case CSI2RX1_PORT_NUMBER:
+ break;
+ case DVP_SENSOR_PORT_NUMBER:
+ stf_dvp_io_pad_config(vin);
+ break;
+ case CSI2RX0_SENSOR_PORT_NUMBER:
+ case CSI2RX1_SENSOR_PORT_NUMBER:
+ break;
+ default:
+ break;
+ }
+ }
+
+ of_node_put(remote);
+ if (IS_ERR(asd)) {
+ st_err(ST_CAMSS, "Cannot get remote parent2\n");
+ }
+ }
+}
+static int stf_vin_clk_enable(struct stf_vin2_dev *vin_dev)
+{
+
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ //stf_do_port_configure(vin_dev->stfcamss);
+
+ reg_set_bit(vin->clkgen_base, CLK_DOM4_APB_FUNC, CLK_MUX_SEL, 0x8);
+ reg_set_bit(vin->clkgen_base, CLK_U0_VIN_PCLK, CLK_U0_VIN_PCLK_ICG, 0x1<<31);
+ reg_set_bit(vin->clkgen_base, CLK_U0_VIN_SYS_CLK, CLK_MUX_SEL, 0x2);
+ reg_set_bit(vin->clkgen_base, CLK_U0_ISPV2_TOP_WRAPPER_CLK_C, CLK_U0_ISPV2_CLK_ICG, 0x1<<31);
+ reg_clear_rst(vin->clkgen_base, SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RST_U0_ISPV2_TOP_WRAPPER_RST_P);
+ reg_set_bit(vin->clkgen_base, CLK_U0_ISPV2_TOP_WRAPPER_CLK_C, CLK_U0_ISPV2_MUX_SEL, 0x0);
+ reg_clear_rst(vin->clkgen_base, SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RST_U0_ISPV2_TOP_WRAPPER_RST_C);
+ reg_set_bit(vin->clkgen_base, CLK_DVP_INV, CLK_POLARITY, 0x0);
+ reg_set_bit(vin->clkgen_base, CLK_U0_ISPV2_TOP_WRAPPER_CLK_C, CLK_U0_ISPV2_MUX_SEL, 0x1<<24);
+ reg_clear_rst(vin->clkgen_base, SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE,
+ RSTN_U0_VIN_RST_N_PCLK
+ | RSTN_U0_VIN_RST_P_AXIRD
+ | RSTN_U0_VIN_RST_N_SYS_CLK);
+ reg_set_bit(vin->clkgen_base, CLK_U0_VIN_CLK_P_AXIWR, CLK_U0_VIN_MUX_SEL, 0x0);
+ reg_clear_rst(vin->clkgen_base, SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE, RSTN_U0_VIN_RST_P_AXIWR);
+ reg_set_bit(vin->clkgen_base, CLK_U0_VIN_CLK_P_AXIWR, CLK_U0_VIN_MUX_SEL, 0x1<<24);
+
+ return 0;
+}
+
+
+static int stf_vin_clk_disable(struct stf_vin2_dev *vin_dev)
+{
+
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ reg_assert_rst(vin->clkgen_base,SOFTWARE_RESET_ASSERT0_ASSERT_SET,
+ SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE, RSTN_U0_VIN_RST_N_PCLK
+ | RSTN_U0_VIN_RST_N_SYS_CLK
+ | RSTN_U0_VIN_RST_P_AXIRD
+ | RSTN_U0_VIN_RST_P_AXIWR);
+ reg_set_bit(vin->clkgen_base, CLK_U0_VIN_PCLK, CLK_U0_VIN_PCLK_ICG, 0x0);
+
+ return 0;
+}
+
+static int stf_vin_config_set(struct stf_vin2_dev *vin_dev)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ return 0;
+}
+
+static int stf_vin_wr_stream_set(struct stf_vin2_dev *vin_dev, int on)
+{
+
+#if 0
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ print_reg(ST_VIN, vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL);
+ if (on) {
+ reg_set(vin->sysctrl_base,
+ SYSCTRL_VIN_AXI_CTRL, BIT(1));
+ } else {
+ reg_clear(vin->sysctrl_base,
+ SYSCTRL_VIN_AXI_CTRL, BIT(1));
+ }
+ print_reg(ST_VIN, vin->sysctrl_base, SYSCTRL_VIN_AXI_CTRL);
+#endif
+
+ return 0;
+}
+
+static void stf_vin_wr_irq_enable(struct stf_vin2_dev *vin_dev,
+ int enable)
+{
+
+#if 0
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ unsigned int mask_value = 0, value = 0;
+
+ if (enable) {
+ // value = ~((0x1 << 4) | (0x1 << 20));
+ value = ~(0x1 << 4);
+
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_INTP_CTRL, BIT(4), value);
+ } else {
+ /* mask and clear vin interrupt */
+ // mask_value = (0x1 << 4) | (0x1 << 20);
+ // value = 0x1 | (0x1 << 16) | mask_value;
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x1);
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_INTP_CTRL, BIT(0), 0x0);
+
+ value = 0x1 << 4;
+ reg_set_bit(vin->sysctrl_base,
+ SYSCTRL_VIN_INTP_CTRL, BIT(4), value);
+ }
+#endif
+}
+
+static void stf_vin_power_on(struct stf_vin2_dev *vin_dev, int enable)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ if(enable){
+ reg_write(vin->pmu_test, SW_DEST_POWER_ON, (0x1<<5));
+ reg_write(vin->pmu_test, SW_ENCOURAGE, 0xff);
+ reg_write(vin->pmu_test, SW_ENCOURAGE, 0x05);
+ reg_write(vin->pmu_test, SW_ENCOURAGE, 0x50);
+
+ reg_set_highest_bit(vin->sys_crg, 0xCCU);
+ reg_set_highest_bit(vin->sys_crg, 0xD0U);
+ reg_clear_rst(vin->sys_crg, 0x2FCU,0x30CU, (0x1 << 9));
+ reg_clear_rst(vin->sys_crg, 0x2FCU,0x30CU, (0x1 << 10));
+ } else {
+ reg_assert_rst(vin->sys_crg, 0x2FCU ,0x30cu, BIT(9)); //u0_dom_isp_top_disable
+ reg_assert_rst(vin->sys_crg, 0x2FCU ,0x30cu, BIT(10));
+ reg_set_bit(vin->sys_crg, 0xccu, BIT(31), 0x0);
+ reg_set_bit(vin->sys_crg, 0xd0u, BIT(31), 0x0);
+ reg_write(vin->pmu_test, SW_DEST_POWER_ON, (0x1<<5));
+ reg_write(vin->pmu_test, SW_ENCOURAGE, 0xff);
+ reg_write(vin->pmu_test, SW_ENCOURAGE, 0x0a);
+ reg_write(vin->pmu_test, SW_ENCOURAGE, 0xa0);
+ }
+}
+
+
+
+static void stf_vin_wr_rd_set_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t wr_addr, dma_addr_t rd_addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+#if 0
+
+ /* set the start address*/
+ reg_write(vin->sysctrl_base,
+ SYSCTRL_VIN_WR_START_ADDR, (long)wr_addr);
+ reg_write(vin->sysctrl_base,
+ SYSCTRL_VIN_RD_END_ADDR, (long)rd_addr);
+#endif
+}
+
+void stf_vin_wr_set_ping_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t addr)
+{
+
+#if 0
+
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ /* set the start address */
+ reg_write(vin->sysctrl_base, SYSCTRL_VIN_WR_START_ADDR, (long)addr);
+#endif
+}
+
+void stf_vin_wr_set_pong_addr(struct stf_vin2_dev *vin_dev, dma_addr_t addr)
+{
+
+#if 0
+
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ /* set the start address */
+ reg_write(vin->sysctrl_base, SYSCTRL_VIN_RD_END_ADDR, (long)addr);
+#endif
+}
+
+void stf_vin_isp_set_yuv_addr(struct stf_vin2_dev *vin_dev, int isp_id,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase =
+ isp_id ? vin->isp_isp1_base : vin->isp_isp0_base;
+
+ reg_write(ispbase, ISP_REG_Y_PLANE_START_ADDR, y_addr);
+ reg_write(ispbase, ISP_REG_UV_PLANE_START_ADDR, uv_addr);
+ // shadow update
+ //reg_set_bit(ispbase, ISP_REG_IESHD_ADDR, BIT(1) | BIT(0), 0x3); //fw no configure 2021 1110
+}
+
+void stf_vin_isp_set_raw_addr(struct stf_vin2_dev *vin_dev, int isp_id,
+ dma_addr_t raw_addr)
+{
+
+#if 0
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase =
+ isp_id ? vin->isp_isp1_base : vin->isp_isp0_base;
+
+ reg_write(ispbase, ISP_REG_DUMP_CFG_0, raw_addr);
+ reg_set_bit(ispbase, ISP_REG_CSIINTS_ADDR, 0x3FFFF, 0x3000a);
+#endif
+}
+
+void dump_vin_reg(void *__iomem regbase)
+{
+ st_debug(ST_VIN, "DUMP VIN register:\n");
+ print_reg(ST_VIN, regbase, 0x00);
+ print_reg(ST_VIN, regbase, 0x04);
+ print_reg(ST_VIN, regbase, 0x08);
+ print_reg(ST_VIN, regbase, 0x0c);
+ print_reg(ST_VIN, regbase, 0x10);
+ print_reg(ST_VIN, regbase, 0x14);
+ print_reg(ST_VIN, regbase, 0x18);
+ print_reg(ST_VIN, regbase, 0x1c);
+ print_reg(ST_VIN, regbase, 0x20);
+ print_reg(ST_VIN, regbase, 0x24);
+ print_reg(ST_VIN, regbase, 0x28);
+ print_reg(ST_VIN, regbase, 0x2c);
+ print_reg(ST_VIN, regbase, 0x30);
+ print_reg(ST_VIN, regbase, 0x34);
+ print_reg(ST_VIN, regbase, 0x38);
+ print_reg(ST_VIN, regbase, 0x3c);
+ print_reg(ST_VIN, regbase, 0x40);
+ print_reg(ST_VIN, regbase, 0x44);
+ print_reg(ST_VIN, regbase, 0x48);
+ print_reg(ST_VIN, regbase, 0x4c);
+ print_reg(ST_VIN, regbase, 0x50);
+ print_reg(ST_VIN, regbase, 0x54);
+ print_reg(ST_VIN, regbase, 0x58);
+ print_reg(ST_VIN, regbase, 0x5c);
+}
+
+struct vin_hw_ops vin_ops = {
+
+ .vin_clk_init = stf_vin_clk_init,
+ .vin_clk_enable = stf_vin_clk_enable,
+ .vin_clk_disable = stf_vin_clk_disable,
+ .vin_config_set = stf_vin_config_set,
+ .vin_wr_stream_set = stf_vin_wr_stream_set,
+ .vin_wr_irq_enable = stf_vin_wr_irq_enable,
+ .vin_power_on = stf_vin_power_on,
+ .wr_rd_set_addr = stf_vin_wr_rd_set_addr,
+ .vin_wr_set_ping_addr = stf_vin_wr_set_ping_addr,
+ .vin_wr_set_pong_addr = stf_vin_wr_set_pong_addr,
+ .vin_isp_set_yuv_addr = stf_vin_isp_set_yuv_addr,
+ .vin_isp_set_raw_addr = stf_vin_isp_set_raw_addr,
+ .vin_wr_irq_handler = stf_vin_wr_irq_handler,
+ .vin_isp_irq_handler = stf_vin_isp_irq_handler,
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/of_graph.h>
+#include <linux/of_address.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+
+#include <linux/videodev2.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-fwnode.h>
+#include <linux/debugfs.h>
+
+#include "stfcamss.h"
+
+#ifdef STF_DEBUG
+unsigned int stdbg_level = ST_DEBUG;
+unsigned int stdbg_mask = 0x7F;
+#else
+unsigned int stdbg_level = ST_ERR;
+unsigned int stdbg_mask = 0x7F;
+#endif
+
+static const struct reg_name mem_reg_name[] = {
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ {"mipi0"},
+#endif
+ {"vclk"},
+ {"vrst"},
+ {"mipi1"},
+ {"sctrl"},
+ {"isp0"},
+ {"isp1"},
+ {"tclk"},
+ {"trst"},
+ {"iopad"},
+ {"pmu"},
+ {"syscrg"},
+};
+
+int stfcamss_get_mem_res(struct platform_device *pdev, struct stf_vin_dev *vin)
+{
+ struct device *dev = &pdev->dev;
+ struct resource *res;
+ void __iomem *regs;
+ char *name;
+ int i;
+
+ for (i = 0; i < ARRAY_SIZE(mem_reg_name); i++) {
+ name = (char *)(&mem_reg_name[i]);
+ res = platform_get_resource_byname(pdev, IORESOURCE_MEM, name);
+ regs = devm_ioremap_resource(dev, res);
+ if (IS_ERR(regs))
+ return PTR_ERR(regs);
+
+ if (!strcmp(name, "mipi0"))
+ vin->mipi0_base = regs;
+ else if (!strcmp(name, "vclk"))
+ vin->clkgen_base = regs;
+ else if (!strcmp(name, "vrst"))
+ vin->rstgen_base = regs;
+ else if (!strcmp(name, "mipi1"))
+ vin->mipi1_base = regs;
+ else if (!strcmp(name, "sctrl"))
+ vin->sysctrl_base = regs;
+ else if (!strcmp(name, "isp0"))
+ vin->isp_isp0_base = regs;
+ else if (!strcmp(name, "isp1"))
+ vin->isp_isp1_base = regs;
+ else if (!strcmp(name, "tclk"))
+ vin->vin_top_clkgen_base = regs;
+ else if (!strcmp(name, "trst"))
+ vin->vin_top_rstgen_base = regs;
+ else if (!strcmp(name, "iopad"))
+ vin->vin_top_iopad_base = regs;
+ else if (!strcmp(name,"pmu"))
+ vin->pmu_test = regs;
+ else if (!strcmp(name,"syscrg"))
+ vin->sys_crg = regs;
+ else
+ st_err(ST_CAMSS, "Could not match resource name\n");
+ }
+
+ return 0;
+}
+
+int vin_parse_dt(struct device *dev, struct stf_vin_dev *vin)
+{
+ int ret = 0;
+ struct device_node *np = dev->of_node;
+
+ if (!np)
+ return -EINVAL;
+
+ return ret;
+}
+
+struct media_entity *stfcamss_find_sensor(struct media_entity *entity)
+{
+ struct media_pad *pad;
+
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ return NULL;
+
+ pad = media_entity_remote_pad(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ return NULL;
+
+ entity = pad->entity;
+
+ if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
+ return entity;
+ }
+}
+
+static int stfcamss_of_parse_endpoint_node(struct device *dev,
+ struct device_node *node,
+ struct stfcamss_async_subdev *csd)
+{
+ struct v4l2_fwnode_endpoint vep = { { 0 } };
+ struct v4l2_fwnode_bus_parallel *parallel_bus = &vep.bus.parallel;
+ struct v4l2_fwnode_bus_mipi_csi2 *csi2_bus = &vep.bus.mipi_csi2;
+ struct dvp_cfg *dvp = &csd->interface.dvp;
+ struct csi2phy_cfg *csiphy = &csd->interface.csiphy;
+ int ret;
+
+ v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
+ st_debug(ST_CAMSS, "%s: vep.base.port = 0x%x, id = 0x%x\n",
+ __func__, vep.base.port, vep.base.id);
+
+ csd->port = vep.base.port;
+ switch (csd->port) {
+ case CSI2RX0_PORT_NUMBER:
+ case CSI2RX1_PORT_NUMBER:
+ break;
+ case DVP_SENSOR_PORT_NUMBER:
+ st_debug(ST_CAMSS, "%s, flags = 0x%x\n", __func__,
+ parallel_bus->flags);
+ dvp->flags = parallel_bus->flags;
+ dvp->bus_width = parallel_bus->bus_width;
+ dvp->data_shift = parallel_bus->data_shift;
+ break;
+ case CSI2RX0_SENSOR_PORT_NUMBER:
+ case CSI2RX1_SENSOR_PORT_NUMBER:
+ st_debug(ST_CAMSS, "%s, CSI2 flags = 0x%x\n",
+ __func__, parallel_bus->flags);
+ csiphy->flags = csi2_bus->flags;
+ memcpy(csiphy->data_lanes,
+ csi2_bus->data_lanes, csi2_bus->num_data_lanes);
+ csiphy->clock_lane = csi2_bus->clock_lane;
+ csiphy->num_data_lanes = csi2_bus->num_data_lanes;
+ memcpy(csiphy->lane_polarities,
+ csi2_bus->lane_polarities,
+ csi2_bus->num_data_lanes + 1);
+ break;
+ default:
+ break;
+ };
+
+ return 0;
+}
+
+static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
+{
+ struct device *dev = stfcamss->dev;
+ struct device_node *node = NULL;
+ struct device_node *remote = NULL;
+ int ret, num_subdevs = 0;
+
+ for_each_endpoint_of_node(dev->of_node, node) {
+ struct stfcamss_async_subdev *csd;
+
+ if (!of_device_is_available(node))
+ continue;
+
+ remote = of_graph_get_remote_port_parent(node);
+ if (!remote) {
+ st_err(ST_CAMSS, "Cannot get remote parent\n");
+ ret = -EINVAL;
+ goto err_cleanup;
+ }
+
+ csd = v4l2_async_notifier_add_fwnode_subdev(
+ &stfcamss->notifier, of_fwnode_handle(remote),
+ struct stfcamss_async_subdev);
+ of_node_put(remote);
+ if (IS_ERR(csd)) {
+ ret = PTR_ERR(csd);
+ goto err_cleanup;
+ }
+
+ ret = stfcamss_of_parse_endpoint_node(dev, node, csd);
+ if (ret < 0)
+ goto err_cleanup;
+
+ num_subdevs++;
+ }
+
+ return num_subdevs;
+
+err_cleanup:
+ of_node_put(node);
+ return ret;
+}
+
+static int stfcamss_init_subdevices(struct stfcamss *stfcamss)
+{
+ int ret, i;
+
+ ret = stf_dvp_subdev_init(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_dvp sub-device: %d\n",
+ ret);
+ return ret;
+ }
+
+ for (i = 0; i < stfcamss->csiphy_num; i++) {
+ ret = stf_csiphy_subdev_init(stfcamss, i);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_csiphy sub-device: %d\n",
+ ret);
+ return ret;
+ }
+ }
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ for (i = 0; i < stfcamss->csi_num; i++) {
+ ret = stf_csi_subdev_init(stfcamss, i);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_csi sub-device: %d\n",
+ ret);
+ return ret;
+ }
+ }
+#endif
+
+ for (i = 0; i < stfcamss->isp_num; i++) {
+ ret = stf_isp_subdev_init(stfcamss, i);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_isp sub-device: %d\n",
+ ret);
+ return ret;
+ }
+ }
+
+ ret = stf_vin_subdev_init(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_vin sub-device: %d\n",
+ ret);
+ return ret;
+ }
+ return ret;
+}
+
+static int stfcamss_register_subdevices(struct stfcamss *stfcamss)
+{
+ int ret, i, j;
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+ struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
+ struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
+ struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+
+ ret = stf_dvp_register(dvp_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf dvp entity: %d\n",
+ ret);
+ goto err_reg_dvp;
+ }
+
+ for (i = 0; i < stfcamss->csiphy_num; i++) {
+ ret = stf_csiphy_register(&csiphy_dev[i],
+ &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf csiphy%d entity: %d\n",
+ i, ret);
+ goto err_reg_csiphy;
+ }
+ }
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ for (i = 0; i < stfcamss->csi_num; i++) {
+ ret = stf_csi_register(&csi_dev[i],
+ &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf csi%d entity: %d\n",
+ i, ret);
+ goto err_reg_csi;
+ }
+ }
+#endif
+
+ for (i = 0; i < stfcamss->isp_num; i++) {
+ ret = stf_isp_register(&isp_dev[i],
+ &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf isp%d entity: %d\n",
+ i, ret);
+ goto err_reg_isp;
+ }
+ }
+
+ ret = stf_vin_register(vin_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register vin entity: %d\n",
+ ret);
+ goto err_reg_vin;
+ }
+
+ ret = media_create_pad_link(
+ &dvp_dev->subdev.entity,
+ STF_DVP_PAD_SRC,
+ &vin_dev->line[VIN_LINE_WR].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->vin entities: %d\n",
+ dvp_dev->subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ for (i = 0; i < stfcamss->csi_num; i++) {
+ ret = media_create_pad_link(
+ &csi_dev[i].subdev.entity,
+ STF_CSI_PAD_SRC,
+ &vin_dev->line[VIN_LINE_WR].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->vin entities: %d\n",
+ csi_dev[i].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+ }
+
+ for (j = 0; j < stfcamss->csi_num; j++) {
+ ret = media_create_pad_link(
+ &csiphy_dev[j].subdev.entity,
+ STF_CSIPHY_PAD_SRC,
+ &csi_dev[j].subdev.entity,
+ STF_CSI_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ csiphy_dev[j].subdev.entity.name,
+ csi_dev[j].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+ }
+#endif
+ for (i = 0; i < stfcamss->isp_num; i++) {
+ ret = media_create_pad_link(
+ &isp_dev[i].subdev.entity,
+ STF_ISP_PAD_SRC,
+ &vin_dev->line[i + VIN_LINE_ISP0].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev[i].subdev.entity.name,
+ vin_dev->line[i + VIN_LINE_ISP0]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev[i].subdev.entity,
+ STF_ISP_PAD_SRC,
+ &vin_dev->line[i + VIN_LINE_ISP0_RAW].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev[i].subdev.entity.name,
+ vin_dev->line[i + VIN_LINE_ISP0_RAW]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &dvp_dev->subdev.entity,
+ STF_DVP_PAD_SRC,
+ &isp_dev[i].subdev.entity,
+ STF_ISP_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ dvp_dev->subdev.entity.name,
+ isp_dev[i].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+ #ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ for (j = 0; j < stfcamss->csi_num; j++) {
+ ret = media_create_pad_link(
+ &csi_dev[j].subdev.entity,
+ STF_CSI_PAD_SRC,
+ &isp_dev[i].subdev.entity,
+ STF_ISP_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ csi_dev[j].subdev.entity.name,
+ isp_dev[i].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+ }
+ #endif
+ }
+
+ return ret;
+
+err_link:
+ stf_vin_unregister(stfcamss->vin_dev);
+err_reg_vin:
+ i = stfcamss->isp_num;
+err_reg_isp:
+ for (i--; i >= 0; i--)
+ stf_isp_unregister(&stfcamss->isp_dev[i]);
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ i = stfcamss->csi_num;
+err_reg_csi:
+ for (i--; i >= 0; i--)
+ stf_csi_unregister(&stfcamss->csi_dev[i]);
+#endif
+
+ i = stfcamss->csiphy_num;
+err_reg_csiphy:
+ for (i--; i >= 0; i--)
+ stf_csiphy_unregister(&stfcamss->csiphy_dev[i]);
+
+ stf_dvp_unregister(stfcamss->dvp_dev);
+err_reg_dvp:
+ return ret;
+}
+
+static void stfcamss_unregister_subdevices(struct stfcamss *stfcamss)
+{
+ int i;
+
+ stf_dvp_unregister(stfcamss->dvp_dev);
+
+ i = stfcamss->csiphy_num;
+ for (i--; i >= 0; i--)
+ stf_csiphy_unregister(&stfcamss->csiphy_dev[i]);
+
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ i = stfcamss->csi_num;
+ for (i--; i >= 0; i--)
+ stf_csi_unregister(&stfcamss->csi_dev[i]);
+#endif
+
+ i = stfcamss->isp_num;
+ for (i--; i >= 0; i--)
+ stf_isp_unregister(&stfcamss->isp_dev[i]);
+
+ stf_vin_unregister(stfcamss->vin_dev);
+}
+
+static int stfcamss_register_mediadevice_subdevnodes(
+ struct v4l2_async_notifier *async,
+ struct v4l2_subdev *sd)
+{
+ struct stfcamss *stfcamss =
+ container_of(async, struct stfcamss, notifier);
+ struct v4l2_device *v4l2_dev = &stfcamss->v4l2_dev;
+ int ret;
+
+ if (sd->host_priv) {
+ struct media_entity *sensor = &sd->entity;
+ struct media_entity *input = sd->host_priv;
+ unsigned int i;
+
+ for (i = 0; i < sensor->num_pads; i++) {
+ if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+ break;
+ }
+ if (i == sensor->num_pads) {
+ st_err(ST_CAMSS,
+ "No source pad in external entity\n");
+ return -EINVAL;
+ }
+
+ ret = media_create_pad_link(sensor, i,
+ input, STF_PAD_SINK,
+ MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ sensor->name, input->name, ret);
+ return ret;
+ }
+ }
+
+ ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+ if (ret < 0)
+ return ret;
+
+ if (stfcamss->media_dev.devnode)
+ return ret;
+
+ st_debug(ST_CAMSS, "stfcamss register media device\n");
+ return media_device_register(&stfcamss->media_dev);
+}
+
+static int stfcamss_subdev_notifier_bound(struct v4l2_async_notifier *async,
+ struct v4l2_subdev *subdev,
+ struct v4l2_async_subdev *asd)
+{
+ struct stfcamss *stfcamss =
+ container_of(async, struct stfcamss, notifier);
+ struct stfcamss_async_subdev *csd =
+ container_of(asd, struct stfcamss_async_subdev, asd);
+ enum port_num port = csd->port;
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ struct stf_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+ struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
+ struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
+ struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+ int ret;
+ u32 id, isp_id;
+
+ switch (port) {
+ case CSI2RX0_PORT_NUMBER:
+ case CSI2RX1_PORT_NUMBER:
+ id = port - CSI2RX0_PORT_NUMBER;
+ isp_dev = &isp_dev[id];
+ subdev->host_priv = &isp_dev->subdev.entity;
+ break;
+ case DVP_SENSOR_PORT_NUMBER:
+ dvp_dev->dvp = &csd->interface.dvp;
+ subdev->host_priv = &dvp_dev->subdev.entity;
+ break;
+ case CSI2RX0_SENSOR_PORT_NUMBER:
+ case CSI2RX1_SENSOR_PORT_NUMBER:
+ id = port - CSI2RX0_SENSOR_PORT_NUMBER;
+ csiphy_dev = &csiphy_dev[id];
+ csiphy_dev->csiphy = &csd->interface.csiphy;
+ subdev->host_priv = &csiphy_dev->subdev.entity;
+ break;
+ default:
+ break;
+ };
+
+ stfcamss_register_mediadevice_subdevnodes(async, subdev);
+
+ return 0;
+}
+
+static int stfcamss_subdev_notifier_complete(
+ struct v4l2_async_notifier *async)
+{
+ struct stfcamss *stfcamss =
+ container_of(async, struct stfcamss, notifier);
+ struct v4l2_device *v4l2_dev = &stfcamss->v4l2_dev;
+ struct v4l2_subdev *sd;
+ int ret;
+
+ list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
+ if (sd->host_priv) {
+ struct media_entity *sensor = &sd->entity;
+ struct media_entity *input = sd->host_priv;
+ unsigned int i;
+
+ for (i = 0; i < sensor->num_pads; i++) {
+ if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+ break;
+ }
+ if (i == sensor->num_pads) {
+ st_err(ST_CAMSS,
+ "No source pad in external entity\n");
+ return -EINVAL;
+ }
+
+ ret = media_create_pad_link(sensor, i,
+ input, STF_PAD_SINK,
+ MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ sensor->name, input->name, ret);
+ return ret;
+ }
+ }
+ }
+
+ ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+ if (ret < 0)
+ return ret;
+
+ return media_device_register(&stfcamss->media_dev);
+}
+
+static const struct v4l2_async_notifier_operations
+stfcamss_subdev_notifier_ops = {
+ .bound = stfcamss_subdev_notifier_bound,
+ // .complete = stfcamss_subdev_notifier_complete,
+};
+
+static const struct media_device_ops stfcamss_media_ops = {
+ .link_notify = v4l2_pipeline_link_notify,
+};
+
+#ifdef CONFIG_DEBUG_FS
+enum module_id {
+ VIN_MODULE = 0,
+ ISP0_MODULE,
+ ISP1_MODULE,
+ CSI0_MODULE,
+ CSI1_MODULE,
+ CSIPHY_MODULE,
+ DVP_MODULE,
+ CLK_MODULE,
+};
+
+static enum module_id id_num = ISP0_MODULE;
+
+void dump_clk_reg(void __iomem *reg_base)
+{
+ int i;
+
+ st_info(ST_CAMSS, "DUMP Clk register:\n");
+ for (i = 0; i <= CLK_C_ISP1_CTRL; i += 4)
+ print_reg(ST_CAMSS, reg_base, i);
+}
+
+static ssize_t vin_debug_read(struct file *file, char __user *user_buf,
+ size_t count, loff_t *ppos)
+{
+ char buf[256];
+ int ret, len = 0, i;
+ struct device *dev = file->private_data;
+ u32 val = 0;
+ void __iomem *reg_base;
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+ struct stf_vin_dev *vin = stfcamss->vin;
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ struct stf_isp_dev *isp0_dev = &stfcamss->isp_dev[0];
+ struct stf_isp_dev *isp1_dev = &stfcamss->isp_dev[1];
+ struct stf_csi_dev *csi0_dev = &stfcamss->csi_dev[0];
+ struct stf_csi_dev *csi1_dev = &stfcamss->csi_dev[1];
+
+ switch (id_num) {
+ case VIN_MODULE:
+ case CSIPHY_MODULE:
+ case DVP_MODULE:
+ mutex_lock(&vin_dev->power_lock);
+ if (vin_dev->power_count > 0) {
+ reg_base = vin->sysctrl_base;
+ dump_vin_reg(reg_base);
+ }
+ mutex_unlock(&vin_dev->power_lock);
+ break;
+ case ISP0_MODULE:
+ mutex_lock(&isp0_dev->stream_lock);
+ if (isp0_dev->stream_count > 0) {
+ reg_base = vin->isp_isp0_base;
+ dump_isp_reg(reg_base, 0);
+ }
+ mutex_unlock(&isp0_dev->stream_lock);
+ break;
+ case ISP1_MODULE:
+ mutex_lock(&isp1_dev->stream_lock);
+ if (isp1_dev->stream_count > 0) {
+ reg_base = vin->isp_isp1_base;
+ dump_isp_reg(reg_base, 1);
+ }
+ mutex_unlock(&isp1_dev->stream_lock);
+ break;
+ case CSI0_MODULE:
+ mutex_lock(&csi0_dev->stream_lock);
+ if (csi0_dev->stream_count > 0) {
+ reg_base = vin->mipi0_base;
+ dump_csi_reg(reg_base, 0);
+ }
+ mutex_unlock(&csi0_dev->stream_lock);
+ break;
+ case CSI1_MODULE:
+ mutex_lock(&csi1_dev->stream_lock);
+ if (csi1_dev->stream_count > 0) {
+ reg_base = vin->mipi1_base;
+ dump_csi_reg(reg_base, 1);
+ }
+ mutex_unlock(&csi1_dev->stream_lock);
+ break;
+ case CLK_MODULE:
+ mutex_lock(&vin_dev->power_lock);
+ if (vin_dev->power_count > 0) {
+ reg_base = vin->clkgen_base;
+ dump_clk_reg(reg_base);
+ }
+ mutex_unlock(&vin_dev->power_lock);
+ break;
+ default:
+ break;
+ }
+
+ return 0;
+}
+
+static void set_reg_val(struct stfcamss *stfcamss, int id, u32 offset, u32 val)
+{
+ struct stf_vin_dev *vin = stfcamss->vin;
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ struct stf_isp_dev *isp0_dev = &stfcamss->isp_dev[0];
+ struct stf_isp_dev *isp1_dev = &stfcamss->isp_dev[1];
+ struct stf_csi_dev *csi0_dev = &stfcamss->csi_dev[0];
+ struct stf_csi_dev *csi1_dev = &stfcamss->csi_dev[1];
+ void __iomem *reg_base;
+
+ switch (id) {
+ case VIN_MODULE:
+ case CSIPHY_MODULE:
+ case DVP_MODULE:
+ mutex_lock(&vin_dev->power_lock);
+ if (vin_dev->power_count > 0) {
+ reg_base = vin->sysctrl_base;
+ print_reg(ST_VIN, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_VIN, reg_base, offset);
+ }
+ mutex_unlock(&vin_dev->power_lock);
+ break;
+ case ISP0_MODULE:
+ mutex_lock(&isp0_dev->stream_lock);
+ if (isp0_dev->stream_count > 0) {
+ reg_base = vin->isp_isp0_base;
+ print_reg(ST_ISP, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_ISP, reg_base, offset);
+ }
+ mutex_unlock(&isp0_dev->stream_lock);
+ break;
+ case ISP1_MODULE:
+ mutex_lock(&isp1_dev->stream_lock);
+ if (isp1_dev->stream_count > 0) {
+ reg_base = vin->isp_isp1_base;
+ print_reg(ST_ISP, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_ISP, reg_base, offset);
+ }
+ mutex_unlock(&isp1_dev->stream_lock);
+ break;
+ case CSI0_MODULE:
+ mutex_lock(&csi0_dev->stream_lock);
+ if (csi0_dev->stream_count > 0) {
+ reg_base = vin->mipi0_base;
+ print_reg(ST_CSI, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_CSI, reg_base, offset);
+ }
+ mutex_unlock(&csi0_dev->stream_lock);
+ break;
+ case CSI1_MODULE:
+ mutex_lock(&csi1_dev->stream_lock);
+ if (csi1_dev->stream_count > 0) {
+ reg_base = vin->mipi1_base;
+ print_reg(ST_CSI, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_CSI, reg_base, offset);
+ }
+ mutex_unlock(&csi1_dev->stream_lock);
+ break;
+ case CLK_MODULE:
+ mutex_lock(&vin_dev->power_lock);
+ if (vin_dev->power_count > 0) {
+ reg_base = vin->clkgen_base;
+ print_reg(ST_CAMSS, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_CAMSS, reg_base, offset);
+ }
+ mutex_unlock(&vin_dev->power_lock);
+ break;
+ default:
+ break;
+
+ }
+}
+
+static u32 atoi(const char *s)
+{
+ u32 ret = 0, d = 0;
+ char ch;
+ int hex = 0;
+
+ if ((*s == '0') && (*(s+1) == 'x')) {
+ hex = 1;
+ s += 2;
+ }
+
+ while (1) {
+ if (!hex) {
+ d = (*s++) - '0';
+ if (d > 9)
+ break;
+ ret *= 10;
+ ret += d;
+ } else {
+ ch = tolower(*s++);
+ if (isdigit(ch))
+ d = ch - '0';
+ else if (islower(ch))
+ d = ch - 'a' + 10;
+ else
+ break;
+ if (d > 15)
+ break;
+ ret *= 16;
+ ret += d;
+ }
+ }
+
+ return ret;
+}
+
+static ssize_t vin_debug_write(struct file *file, const char __user *user_buf,
+ size_t count, loff_t *ppos)
+{
+ struct device *dev = file->private_data;
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+ struct stf_vin_dev *vin = stfcamss->vin;
+ char *buf;
+ char *line;
+ char *p;
+ static const char *delims = " \t\r";
+ char *token;
+ u32 offset, val;
+
+ buf = memdup_user_nul(user_buf, min_t(size_t, PAGE_SIZE, count));
+ if (IS_ERR(buf))
+ return PTR_ERR(buf);
+ p = buf;
+ st_debug(ST_CAMSS, "dup buf: %s, len: %d, count: %d\n", p, strlen(p), count);
+ while (p && *p) {
+ p = skip_spaces(p);
+ line = strsep(&p, "\n");
+ if (!*line || *line == '#')
+ break;
+ token = strsep(&line, delims);
+ if (!token)
+ goto out;
+ id_num = atoi(token);
+ token = strsep(&line, delims);
+ if (!token)
+ goto out;
+ offset = atoi(token);
+ token = strsep(&line, delims);
+ if (!token)
+ goto out;
+ val = atoi(token);
+ }
+ set_reg_val(stfcamss, id_num, offset, val);
+out:
+ kfree(buf);
+ st_info(ST_CAMSS, "id_num = %d, offset = 0x%x, 0x%x\n", id_num, offset, val);
+ return count;
+}
+
+static const struct file_operations vin_debug_fops = {
+ .open = simple_open,
+ .read = vin_debug_read,
+ .write = vin_debug_write,
+};
+#endif /* CONFIG_DEBUG_FS */
+
+
+static int stfcamss_probe(struct platform_device *pdev)
+{
+ struct stfcamss *stfcamss;
+ struct stf_vin_dev *vin;
+ struct device_node *node;
+ struct resource res_mem;
+ struct device *dev = &pdev->dev;
+ int ret = 0, num_subdevs;
+
+ printk("stfcamss probe enter!\n");
+
+ stfcamss = devm_kzalloc(dev, sizeof(struct stfcamss), GFP_KERNEL);
+ if (!stfcamss)
+ return -ENOMEM;
+
+ stfcamss->isp_num = 2;
+ stfcamss->csi_num = 2;
+#ifndef CONFIG_VIDEO_CADENCE_CSI2RX
+ stfcamss->csiphy_num = 2;
+#else
+ stfcamss->csiphy_num = 1;
+#endif
+
+ stfcamss->dvp_dev = devm_kzalloc(dev,
+ sizeof(*stfcamss->dvp_dev), GFP_KERNEL);
+ if (!stfcamss->dvp_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->csiphy_dev = devm_kzalloc(dev,
+ stfcamss->csiphy_num * sizeof(*stfcamss->csiphy_dev),
+ GFP_KERNEL);
+ if (!stfcamss->csiphy_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->csi_dev = devm_kzalloc(dev,
+ stfcamss->csi_num * sizeof(*stfcamss->csi_dev),
+ GFP_KERNEL);
+ if (!stfcamss->csi_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->isp_dev = devm_kzalloc(dev,
+ stfcamss->isp_num * sizeof(*stfcamss->isp_dev),
+ GFP_KERNEL);
+ if (!stfcamss->isp_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->vin_dev = devm_kzalloc(dev,
+ sizeof(*stfcamss->vin_dev),
+ GFP_KERNEL);
+ if (!stfcamss->vin_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->vin = devm_kzalloc(dev,
+ sizeof(struct stf_vin_dev),
+ GFP_KERNEL);
+ if (!stfcamss->vin) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ vin = stfcamss->vin;
+
+ vin->irq = platform_get_irq(pdev, 0);
+ if (vin->irq <= 0) {
+ st_err(ST_CAMSS, "Could not get irq\n");
+ goto err_cam;
+ }
+
+ vin->isp0_irq = platform_get_irq(pdev, 1);
+ if (vin->isp0_irq <= 0) {
+ st_err(ST_CAMSS, "Could not get isp0 irq\n");
+ goto err_cam;
+ }
+
+ vin->isp1_irq = platform_get_irq(pdev, 2);
+ if (vin->isp1_irq <= 0) {
+ st_err(ST_CAMSS, "Could not get isp1 irq\n");
+ goto err_cam;
+ }
+
+ ret = stfcamss_get_mem_res(pdev, vin);
+ if (ret) {
+ st_err(ST_CAMSS, "Could not map registers\n");
+ goto err_cam;
+ }
+
+ ret = vin_parse_dt(dev, vin);
+ if (ret)
+ goto err_cam;
+
+ vin->dev = dev;
+ stfcamss->dev = dev;
+ platform_set_drvdata(pdev, stfcamss);
+
+ v4l2_async_notifier_init(&stfcamss->notifier);
+
+ num_subdevs = stfcamss_of_parse_ports(stfcamss);
+ if (num_subdevs < 0) {
+ ret = num_subdevs;
+ goto err_cam_noti;
+ }
+
+ ret = stfcamss_init_subdevices(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to init subdevice: %d\n", ret);
+ goto err_cam_noti;
+ }
+
+ stfcamss->media_dev.dev = stfcamss->dev;
+ strscpy(stfcamss->media_dev.model, "Starfive Camera Subsystem",
+ sizeof(stfcamss->media_dev.model));
+ stfcamss->media_dev.ops = &stfcamss_media_ops;
+ media_device_init(&stfcamss->media_dev);
+
+ stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
+
+ ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to register V4L2 device: %d\n", ret);
+ goto err_cam_noti_med;
+ }
+
+ ret = stfcamss_register_subdevices(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to register subdevice: %d\n", ret);
+ goto err_cam_noti_med_vreg;
+ }
+
+ if (num_subdevs) {
+ stfcamss->notifier.ops = &stfcamss_subdev_notifier_ops;
+ ret = v4l2_async_notifier_register(&stfcamss->v4l2_dev,
+ &stfcamss->notifier);
+ if (ret) {
+ st_err(ST_CAMSS,
+ "Failed to register async subdev nodes: %d\n",
+ ret);
+ goto err_cam_noti_med_vreg_sub;
+ }
+ } else {
+ ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register subdev nodes: %d\n",
+ ret);
+ goto err_cam_noti_med_vreg_sub;
+ }
+
+ ret = media_device_register(&stfcamss->media_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to register media device: %d\n",
+ ret);
+ goto err_cam_noti_med_vreg_sub_medreg;
+ }
+ }
+
+#ifdef CONFIG_DEBUG_FS
+ stfcamss->debugfs_entry = debugfs_create_dir("stfcamss", NULL);
+ stfcamss->vin_debugfs = debugfs_create_file("stf_vin",
+ S_IRUGO | S_IWUSR, stfcamss->debugfs_entry,
+ (void *)dev, &vin_debug_fops);
+ debugfs_create_u32("dbg_level",
+ S_IRUGO | S_IWUSR, stfcamss->debugfs_entry,
+ &stdbg_level);
+ debugfs_create_u32("dbg_mask",
+ S_IRUGO | S_IWUSR, stfcamss->debugfs_entry,
+ &stdbg_mask);
+#endif
+ printk("stfcamss probe out!\n");
+
+ return 0;
+
+#ifdef CONFIG_DEBUG_FS
+ debugfs_remove(stfcamss->vin_debugfs);
+ debugfs_remove_recursive(stfcamss->debugfs_entry);
+ stfcamss->debugfs_entry = NULL;
+#endif
+
+err_cam_noti_med_vreg_sub_medreg:
+err_cam_noti_med_vreg_sub:
+ stfcamss_unregister_subdevices(stfcamss);
+err_cam_noti_med_vreg:
+ v4l2_device_unregister(&stfcamss->v4l2_dev);
+err_cam_noti_med:
+ media_device_cleanup(&stfcamss->media_dev);
+err_cam_noti:
+ v4l2_async_notifier_cleanup(&stfcamss->notifier);
+err_cam:
+ // kfree(stfcamss);
+ return ret;
+}
+
+static int stfcamss_remove(struct platform_device *pdev)
+{
+ struct stfcamss *stfcamss = platform_get_drvdata(pdev);
+
+ dev_info(&pdev->dev, "remove done\n");
+
+#ifdef CONFIG_DEBUG_FS
+ debugfs_remove(stfcamss->vin_debugfs);
+ debugfs_remove_recursive(stfcamss->debugfs_entry);
+ stfcamss->debugfs_entry = NULL;
+#endif
+
+ stfcamss_unregister_subdevices(stfcamss);
+ v4l2_device_unregister(&stfcamss->v4l2_dev);
+ media_device_cleanup(&stfcamss->media_dev);
+
+ kfree(stfcamss);
+
+ return 0;
+}
+
+static const struct of_device_id stfcamss_of_match[] = {
+ {.compatible = "starfive,stf-vin"},
+ { /* end node */ },
+};
+
+MODULE_DEVICE_TABLE(of, stfcamss_of_match);
+
+static struct platform_driver stfcamss_driver = {
+ .probe = stfcamss_probe,
+ .remove = stfcamss_remove,
+ .driver = {
+ .name = DRV_NAME,
+ .of_match_table = of_match_ptr(stfcamss_of_match),
+ },
+};
+
+static int __init stfcamss_init(void)
+{
+ return platform_driver_register(&stfcamss_driver);
+}
+
+static void __exit stfcamss_cleanup(void)
+{
+ platform_driver_unregister(&stfcamss_driver);
+}
+
+module_init(stfcamss_init);
+//fs_initcall(stfcamss_init);
+module_exit(stfcamss_cleanup);
+
+MODULE_LICENSE("GPL");
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 StarFive Technology Co., Ltd.
+ */
+#ifndef STFCAMSS_H
+#define STFCAMSS_H
+
+#include <linux/io.h>
+#include <linux/delay.h>
+
+enum sensor_type {
+ SENSOR_VIN,
+ SENSOR_ISP0, // need replace sensor
+ SENSOR_ISP1, // need replace sensor
+};
+
+enum subdev_type {
+ VIN_DEV_TYPE,
+ ISP0_DEV_TYPE,
+ ISP1_DEV_TYPE,
+};
+
+#include "stf_common.h"
+#include "stf_dvp.h"
+#include "stf_csi.h"
+#include "stf_csiphy.h"
+#include "stf_isp.h"
+#include "stf_vin.h"
+
+#define STF_PAD_SINK 0
+#define STF_PAD_SRC 1
+#define STF_PADS_NUM 2
+
+enum port_num {
+ CSI2RX0_PORT_NUMBER = 0,
+ CSI2RX1_PORT_NUMBER,
+ DVP_SENSOR_PORT_NUMBER,
+ CSI2RX0_SENSOR_PORT_NUMBER,
+ CSI2RX1_SENSOR_PORT_NUMBER
+};
+
+struct stfcamss {
+ struct stf_vin_dev *vin; // stfcamss phy res
+ struct v4l2_device v4l2_dev;
+ struct media_device media_dev;
+ struct device *dev;
+ struct stf_vin2_dev *vin_dev; // subdev
+ struct stf_dvp_dev *dvp_dev; // subdev
+ int csi_num;
+ struct stf_csi_dev *csi_dev; // subdev
+ int csiphy_num;
+ struct stf_csiphy_dev *csiphy_dev; // subdev
+ int isp_num;
+ struct stf_isp_dev *isp_dev; // subdev
+ struct v4l2_async_notifier notifier;
+#ifdef CONFIG_DEBUG_FS
+ struct dentry *debugfs_entry;
+ struct dentry *vin_debugfs;
+#endif
+};
+
+struct stfcamss_async_subdev {
+ struct v4l2_async_subdev asd; // must be first
+ enum port_num port;
+ struct {
+ struct dvp_cfg dvp;
+ struct csi2phy_cfg csiphy;
+ } interface;
+};
+
+extern struct media_entity *stfcamss_find_sensor(struct media_entity *entity);
+
+#endif /* STFCAMSS_H */
--- /dev/null
+/* include/video/stf-vin.h
+ *
+ * Copyright 2020 starfive tech.
+ * Eric Tang <eric.tang@starfivetech.com>
+ *
+ * Generic vin notifier interface
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+*/
+#ifndef _VIDEO_VIN_H
+#define _VIDEO_VIN_H
+
+#include <linux/cdev.h>
+
+#define DRV_NAME "stf-vin"
+#define FB_FIRST_ADDR 0xf9000000
+#define FB_SECOND_ADDR 0xf97e9000
+
+#define RESERVED_MEM_SIZE 0x1000000
+
+#define VIN_MIPI_CONTROLLER0_OFFSET 0x00000
+#define VIN_CLKGEN_OFFSET 0x10000
+#define VIN_RSTGEN_OFFSET 0x20000
+#define VIN_MIPI_CONTROLLER1_OFFSET 0x30000
+#define VIN_SYSCONTROLLER_OFFSET 0x40000
+
+#define VD_1080P 1080
+#define VD_720P 720
+#define VD_PAL 480
+
+#define VD_HEIGHT_1080P VD_1080P
+#define VD_WIDTH_1080P 1920
+
+#define VD_HEIGHT_720P VD_720P
+#define VD_WIDTH_720P 1080
+
+#define VD_HEIGHT_480 480
+#define VD_WIDTH_640 640
+
+#define SEEED_WIDTH_800 800
+#define SEEED_HIGH_480 480
+
+#define VIN_TOP_CLKGEN_BASE_ADDR 0x11800000
+#define VIN_TOP_RSTGEN_BASE_ADDR 0x11840000
+#define VIN_TOP_IOPAD_BASE_ADDR 0x11858000
+
+#define ISP_BASE_MIPI0_ADDR 0x19800000
+#define ISP_BASE_CLKGEN_ADDR 0x19810000
+#define ISP_BASE_RSTGEN_ADDR 0x19820000
+#define ISP_BASE_MIPI1_ADDR 0x19830000
+#define ISP_BASE_SYSCTRL_ADDR 0x19840000
+#define ISP_BASE_ISP0_ADDR 0x19870000
+#define ISP_BASE_ISP1_ADDR 0x198a0000
+
+
+//vin clk registers
+#define CLK_VIN_SRC_CTRL 0x188
+#define CLK_ISP0_AXI_CTRL 0x190
+#define CLK_ISP0NOC_AXI_CTRL 0x194
+#define CLK_ISPSLV_AXI_CTRL 0x198
+#define CLK_ISP1_AXI_CTRL 0x1A0
+#define CLK_ISP1NOC_AXI_CTRL 0x1A4
+#define CLK_VIN_AXI 0x1AC
+#define CLK_VINNOC_AXI 0x1B0
+
+
+#define CLK_DOM4_APB_FUNC 0x0
+#define CLK_MUX_SEL 0xffffff
+
+#define CLK_DVP_INV 0x8
+#define CLK_U0_VIN_PCLK 0x18
+#define CLK_U0_VIN_PCLK_ICG (0x1<<31)
+
+#define CLK_U0_VIN_SYS_CLK 0x1c
+#define CLK_U0_VIN_CLK_P_AXIWR 0x30
+#define CLK_U0_VIN_MUX_SEL (BIT(24) | BIT(25) | BIT(26) | BIT(27) | BIT(28) | BIT(29))
+
+#define CLK_U0_ISPV2_TOP_WRAPPER_CLK_C 0x34u
+#define CLK_U0_ISPV2_MUX_SEL (0x1<<24 | 0x1<<25 | 0x1<<26 | 0x1<<27 | 0x1<<28 | 0x1<< 29)
+
+#define CLK_U0_ISPV2_CLK_ICG (0x1<<31)
+
+#define SOFTWARE_RESET_ASSERT0_ASSERT_SET 0x38U
+#define SOFTWARE_RESET_ASSERT0_ASSERT_SET_STATE 0x3CU
+#define RST_U0_ISPV2_TOP_WRAPPER_RST_P BIT(0)
+#define RST_U0_ISPV2_TOP_WRAPPER_RST_C BIT(1)
+#define RSTN_U0_VIN_RST_N_PCLK BIT(4)
+#define RSTN_U0_VIN_RST_N_SYS_CLK BIT(9)
+#define RSTN_U0_VIN_RST_P_AXIRD BIT(10)
+#define RSTN_U0_VIN_RST_P_AXIWR BIT(11)
+
+
+#define CLK_POLARITY (0x1<<30)
+
+
+
+
+
+#define SW_DEST_POWER_ON 0x0C
+#define SW_ENCOURAGE 0x44
+
+
+//isp clk registers
+#define CLK_DPHY_CFGCLK_ISPCORE_2X_CTRL 0x00
+#define CLK_DPHY_REFCLK_ISPCORE_2X_CTRL 0x04
+#define CLK_DPHY_TXCLKESC_IN_CTRL 0x08
+#define CLK_MIPI_RX0_PXL_CTRL 0x0c
+#define CLK_MIPI_RX1_PXL_CTRL 0x10
+#define CLK_MIPI_RX0_PXL_0_CTRL 0X14
+#define CLK_MIPI_RX0_PXL_1_CTRL 0X18
+#define CLK_MIPI_RX0_PXL_2_CTRL 0X1C
+#define CLK_MIPI_RX0_PXL_3_CTRL 0X20
+#define CLK_MIPI_RX0_SYS0_CTRL 0x24
+#define CLK_MIPI_RX1_PXL_0_CTRL 0X28
+#define CLK_MIPI_RX1_PXL_1_CTRL 0X2C
+#define CLK_MIPI_RX1_PXL_2_CTRL 0X30
+#define CLK_MIPI_RX1_PXL_3_CTRL 0X34
+#define CLK_MIPI_RX1_SYS1_CTRL 0x38
+#define CLK_ISP0_CTRL 0x3c
+#define CLK_ISP0_2X_CTRL 0x40
+#define CLK_ISP0_MIPI_CTRL 0x44
+#define CLK_C_ISP0_CTRL 0x64
+#define CLK_ISP1_CTRL 0x48
+#define CLK_ISP1_2X_CTRL 0x4C
+#define CLK_ISP1_MIPI_CTRL 0x50
+#define CLK_C_ISP1_CTRL 0x68
+#define CLK_CSI2RX0_APB_CTRL 0x58
+
+
+#define CLK_VIN_AXI_WR_CTRL 0x5C
+
+#define SOFTWARE_RESET_ASSERT0 0x0
+#define SOFTWARE_RESET_ASSERT1 0x4
+#define SOFTWARE_RESET_STATUS 0x4
+
+#define IOPAD_REG81 0x144
+#define IOPAD_REG82 0x148
+#define IOPAD_REG83 0x14C
+#define IOPAD_REG84 0x150
+#define IOPAD_REG85 0x154
+#define IOPAD_REG86 0x158
+#define IOPAD_REG87 0x15C
+#define IOPAD_REG88 0x160
+#define IOPAD_REG89 0x164
+
+//sys control REG DEFINE
+#define SYSCONSAIF_SYSCFG_0 0X0
+#define SYSCONSAIF_SYSCFG_4 0x4
+#define U0_VIN_CNFG_AXIRD_END_ADDR 0xffffffff
+#define SYSCONSAIF_SYSCFG_8 0x8
+#define U0_VIN_CNFG_AXIRD_LINE_CNT_END (BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) | BIT(12) | BIT(13))
+#define U0_VIN_CNFG_AXIRD_LINE_CNT_START (BIT(14) | BIT(15) | BIT(16) | BIT(17) | BIT(18) | BIT(19) | BIT(20) | BIT(21) | BIT(22) | BIT(23) | BIT(24) | BIT(25))
+#define SYSCONSAIF_SYSCFG_12 0xc
+#define U0_VIN_CNFG_AXIRD_PIX_CNT_END (BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) | BIT(12))
+#define U0_VIN_CNFG_AXIRD_PIX_CNT_START (BIT(13) | BIT(14) | BIT(15) | BIT(16) | BIT(17) | BIT(18) | BIT(19) | BIT(20) | BIT(21) | BIT(22) | BIT(23) | BIT(24) | BIT(25))
+#define U0_VIN_CNFG_AXIRD_PIX_CT (BIT(26) | BIT(27))
+#define SYSCONSAIF_SYSCFG_16 0x10
+#define U0_VIN_CNFG_AXIRD_START_ADDR 0xFFFFFFFF
+#define SYSCONSAIF_SYSCFG_20 0x14
+#define U0_VIN_CNFG_AXIWR0_EN BIT(4)
+#define U0_VIN_CNFG_AXIWR0_CHANNEL_SEL (BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define SYSCONSAIF_SYSCFG_24 0x18
+#define SYSCONSAIF_SYSCFG_28 0x1c
+#define UO_VIN_CNFG_AXIWR0_PIXEL_HEIGH_BIT_SEL (BIT(15 | BIT(16)))
+#define SYSCONSAIF_SYSCFG_32 0x20
+
+#define SYSCONSAIF_SYSCFG_36 0x24
+#define U0_VIN_CNFG_DVP_HS_POS (0x1<<1)
+#define U0_VIN_CNFG_DVP_SWAP_EN 2
+#define U0_VIN_CNFG_DVP_VS_POS (0x1<<3)
+#define U0_VIN_CNFG_GEN_EN_AXIRD 4
+#define U0_VIN_CNFG_ISP_DVP_EN0 5
+
+#define U0_VIN_CNFG_PIX_NUM (0x1<<13 | 0x1<<14 | 0x1<<15 | 0x1<<16)
+#define U0_VIN_CNFG_AXIRD_AXI_CNT_END (BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) | BIT(12) | BIT(13))
+
+#define U0_VIN_CNFG_AXI_DVP_EN BIT(2)
+#define U0_VIN_CNFG_AXIRD_INTR_MASK BIT(1)
+#define U0_VIN_CNFG_AXIWRD_INTR_MASK BIT(1)
+#define U0_VIN_CNFG_AXIWR0_START_ADDR 0xffffffff
+#define U0_VIN_CNFG_COLOR_BAR_EN 0X0
+#define U0_VIN_CNFG_AXIWR0_PIX_CNT_CT (BIT(13) | BIT(14))
+#define U0_VIN_CNFG_AXIWR0_PIX_CNT_CNT_END (BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) | BIT(12))
+#define U0_VIN_CNFG_AXIWR0_PIXEL_HITH_BIT_SEL (BIT(15) | BIT(16))
+#define U0_VIN_CNFG_AXIWR0_END_ADDR 0xFFFFFFFF
+#define SYSCTRL_REG4 0x10
+#define SYSCTRL_DPHY_CTRL 0x14
+#define SYSCTRL_VIN_AXI_CTRL 0x18
+#define SYSCTRL_VIN_WR_START_ADDR 0x28
+#define SYSCTRL_VIN_RD_END_ADDR 0x2C
+#define SYSCTRL_VIN_WR_PIX_TOTAL 0x30
+#define SYSCTRL_VIN_RD_PIX_TOTAL 0x34
+#define SYSCTRL_VIN_RW_CTRL 0x38
+#define SYSCTRL_VIN_SRC_CHAN_SEL 0x24
+#define SYSCTRL_VIN_SRC_DW_SEL 0x40
+#define SYSCTRL_VIN_RD_VBLANK 0x44
+#define SYSCTRL_VIN_RD_VEND 0x48
+#define SYSCTRL_VIN_RD_HBLANK 0x4C
+#define SYSCTRL_VIN_RD_HEND 0x50
+#define SYSCTRL_VIN_INTP_CTRL 0x54
+
+#define ISP_NO_SCALE_ENABLE (0x1<<20)
+#define ISP_MULTI_FRAME_ENABLE (0x1<<17)
+#define ISP_SS0_ENABLE (0x1<<11)
+#define ISP_SS1_ENABLE (0x1<<12)
+#define ISP_RESET (0x1<<1)
+#define ISP_ENBALE (0x1)
+
+
+
+ //ISP REG DEFINE
+#define ISP_REG_DVP_POLARITY_CFG 0x00000014
+#define ISP_REG_RAW_FORMAT_CFG 0x00000018
+#define ISP_REG_CFA_MODE 0x00000A1C
+#define ISP_REG_PIC_CAPTURE_START_CFG 0x0000001C
+#define ISP_REG_PIC_CAPTURE_END_CFG 0x00000020
+#define ISP_REG_PIPELINE_XY_SIZE 0x00000A0C
+#define ISP_REG_Y_PLANE_START_ADDR 0x00000A80
+#define ISP_REG_UV_PLANE_START_ADDR 0x00000A84
+#define ISP_REG_STRIDE 0x00000A88
+#define ISP_REG_PIXEL_COORDINATE_GEN 0x00000A8C
+#define ISP_REG_PIXEL_AXI_CONTROL 0x00000A90
+#define ISP_REG_SS_AXI_CONTROL 0x00000AC4
+#define ISP_REG_RGB_TO_YUV_COVERSION0 0x00000E40
+#define ISP_REG_RGB_TO_YUV_COVERSION1 0x00000E44
+#define ISP_REG_RGB_TO_YUV_COVERSION2 0x00000E48
+#define ISP_REG_RGB_TO_YUV_COVERSION3 0x00000E4C
+#define ISP_REG_RGB_TO_YUV_COVERSION4 0x00000E50
+#define ISP_REG_RGB_TO_YUV_COVERSION5 0x00000E54
+#define ISP_REG_RGB_TO_YUV_COVERSION6 0x00000E58
+#define ISP_REG_RGB_TO_YUV_COVERSION7 0x00000E5C
+#define ISP_REG_RGB_TO_YUV_COVERSION8 0x00000E60
+#define ISP_REG_CIS_MODULE_CFG 0x00000010
+#define ISP_REG_ISP_CTRL_1 0x00000A08
+#define ISP_REG_ISP_CTRL_0 0x00000A00
+#define ISP_REG_DC_AXI_ID 0x00000044
+#define ISP_REG_CSI_INPUT_EN_AND_STATUS 0x00000000
+
+//CSI registers
+#define DEVICE_CONFIG 0x00
+#define SOFT_RESET 0x04
+#define STATIC_CFG 0x08
+#define ERROR_BYPASS_CFG 0x10
+#define MONITOR_IRQS 0x18
+#define MONITOR_IRQS_MASK_CFG 0x1c
+#define INFO_IRQS 0x20
+#define INFO_IRQS_MASK_CFG 0x24
+#define ERROR_IRQS 0x28
+#define ERROR_IRQS_MASK_CFG 0x2c
+#define DPHY_LANE_CONTROL 0x40
+#define DPHY_STATUS 0x48
+#define DPHY_ERR_STATUS_IRQ 0x4C
+#define DPHY_ERR_IRQ_MASK_CFG 0x50
+#define INTEGRATION_DEBUG 0x60
+#define ERROR_DEBUG 0x74
+
+#define STREAM0_CTRL 0x100
+#define STREAM0_STATUS 0x104
+#define STREAM0_DATA_CFG 0x108
+#define STREAM0_CFG 0x10c
+#define STREAM0_MONITOR_CTRL 0x110
+#define STREAM0_MONITOR_FRAME 0x114
+#define STREAM0_MONITOR_LB 0x118
+#define STREAM0_TIMER 0x11c
+#define STREAM0_FCC_CFG 0x120
+#define STREAM0_FCC_CTRL 0x124
+#define STREAM0_FIFO_FILL_LVL 0x128
+
+typedef enum
+{
+ DT_RAW6 = 0x28,
+ DT_RAW7 = 0x29,
+ DT_RAW8 = 0x2a,
+ DT_RAW10 = 0x2b,
+ DT_RAW12 = 0x2c,
+ DT_RAW14 = 0x2d,
+} mipicam_data_type_t;
+
+
+enum VIN_SOURCE_FORMAT {
+ SRC_COLORBAR_VIN_ISP = 0,
+ SRC_DVP_SENSOR_VIN,
+ SRC_DVP_SENSOR_VIN_ISP,//need replace sensor
+ SRC_CSI2RX_VIN_ISP,
+ SRC_DVP_SENSOR_VIN_OV5640,
+};
+
+struct reg_name {
+ char name[10];
+};
+
+typedef struct
+{
+ int dlane_nb;
+ int dlane_map[4];
+ int dlane_en[4];
+ int dlane_pn_swap[4];
+ int clane_nb;
+ int clane_map[2];
+ int clane_pn_swap[2];
+} csi2rx_dphy_cfg_t;
+
+typedef struct
+{
+ int lane_nb;
+ int dlane_map[4];
+ int dt;
+ int hsize;
+ int vsize;
+} csi2rx_cfg_t;
+
+
+typedef struct
+{
+ int mipi_id, w, h, dt, bpp, fps,lane;
+ u8 clane_swap;
+ u8 clane_pn_swap;
+ u8 dlane_swap[4];
+ u8 dlane_pn_swap[4];
+} csi_format;
+
+struct vin_params {
+ void *paddr;
+ unsigned long size;
+};
+
+struct vin_buf {
+ void *vaddr;
+ dma_addr_t paddr;
+ u32 size;
+};
+
+struct vin_framesize {
+ u32 width;
+ u32 height;
+};
+
+struct vin_format {
+ enum VIN_SOURCE_FORMAT format;
+ u8 fps;
+};
+
+struct stf_vin_dev {
+ /* Protects the access of variables shared within the interrupt */
+ spinlock_t irqlock;
+ int irq;
+ struct device *dev;
+ struct cdev vin_cdev;
+ void __iomem *base;
+ void __iomem *mipi0_base;
+ void __iomem *clkgen_base;
+ void __iomem *rstgen_base;
+ void __iomem *mipi1_base;
+ void __iomem *sysctrl_base;
+ void __iomem *isp_isp0_base;
+ void __iomem *isp_isp1_base;
+ void __iomem *vin_top_clkgen_base;
+ void __iomem *vin_top_rstgen_base;
+ void __iomem *vin_top_iopad_base;
+ void __iomem *pmu_test;
+ void __iomem * sys_crg;
+ struct vin_framesize frame;
+ struct vin_format format;
+ bool isp0;
+ bool isp1;
+ int isp0_irq;
+ int isp1_irq;
+ u32 major;
+ struct vin_buf buf;
+
+ wait_queue_head_t wq;
+ bool condition;
+ int odd;
+
+ csi_format csi_fmt;
+};
+
+extern int vin_notifier_register(struct notifier_block *nb);
+extern void vin_notifier_unregister(struct notifier_block *nb);
+extern int vin_notifier_call(unsigned long e, void *v);
+#endif
projects / platform / kernel / linux-starfive.git / commitdiff