ExternalVendorCode/openwrt
1
0
Fork 0
mirror of https://github.com/openwrt/openwrt.git synced 2024-12-23 15:32:33 +00:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity
1fa28dd899
openwrt/target/linux/starfive/patches-6.6/0085-media-starfive-Add-vin-driver-support.patch
Zoltan HERPAI e2e2fc3cd0 starfive: add patches for 6.6 
Add updated patches for 6.6. DMA/cache-handling patches
have been reworked / backported from upstream.

Signed-off-by: Zoltan HERPAI <wigyori@uid0.hu>
2024-09-03 00:03:19 +02:00

24015 lines
636 KiB
Diff
Raw Blame History

From 908b10ebc95eb29caae8c4737b23a29af5c6298f Mon Sep 17 00:00:00 2001
From: Changhuang Liang <changhuang.liang@starfivetech.com>
Date: Mon, 5 Jun 2023 13:54:16 +0800
Subject: [PATCH 085/116] media: starfive: Add vin driver support
Add vin driver support.
Signed-off-by: Changhuang Liang <changhuang.liang@starfivetech.com>
---
drivers/media/platform/Kconfig | 1 +
drivers/media/platform/Makefile | 1 +
drivers/media/platform/starfive/Kconfig | 56 +
drivers/media/platform/starfive/Makefile | 24 +
.../platform/starfive/v4l2_driver/Readme.txt | 11 +
.../starfive/v4l2_driver/imx219_mipi.c | 1583 ++++++++
.../starfive/v4l2_driver/ov13850_mipi.c | 1921 ++++++++++
.../starfive/v4l2_driver/ov4689_mipi.c | 2975 +++++++++++++++
.../platform/starfive/v4l2_driver/ov5640.c | 3227 +++++++++++++++++
.../platform/starfive/v4l2_driver/sc2235.c | 1914 ++++++++++
.../starfive/v4l2_driver/stf_common.h | 185 +
.../platform/starfive/v4l2_driver/stf_csi.c | 465 +++
.../platform/starfive/v4l2_driver/stf_csi.h | 61 +
.../starfive/v4l2_driver/stf_csi_hw_ops.c | 310 ++
.../starfive/v4l2_driver/stf_csiphy.c | 357 ++
.../starfive/v4l2_driver/stf_csiphy.h | 188 +
.../starfive/v4l2_driver/stf_csiphy_hw_ops.c | 335 ++
.../starfive/v4l2_driver/stf_dmabuf.c | 123 +
.../starfive/v4l2_driver/stf_dmabuf.h | 12 +
.../platform/starfive/v4l2_driver/stf_dvp.c | 385 ++
.../platform/starfive/v4l2_driver/stf_dvp.h | 67 +
.../starfive/v4l2_driver/stf_dvp_hw_ops.c | 187 +
.../platform/starfive/v4l2_driver/stf_event.c | 36 +
.../platform/starfive/v4l2_driver/stf_isp.c | 1521 ++++++++
.../platform/starfive/v4l2_driver/stf_isp.h | 222 ++
.../starfive/v4l2_driver/stf_isp_hw_ops.c | 1550 ++++++++
.../starfive/v4l2_driver/stf_isp_ioctl.h | 133 +
.../platform/starfive/v4l2_driver/stf_video.c | 1552 ++++++++
.../platform/starfive/v4l2_driver/stf_video.h | 83 +
.../platform/starfive/v4l2_driver/stf_vin.c | 1515 ++++++++
.../platform/starfive/v4l2_driver/stf_vin.h | 182 +
.../starfive/v4l2_driver/stf_vin_hw_ops.c | 433 +++
.../platform/starfive/v4l2_driver/stfcamss.c | 1369 +++++++
.../platform/starfive/v4l2_driver/stfcamss.h | 117 +
include/uapi/linux/jh7110-isp.h | 253 ++
include/uapi/linux/v4l2-controls.h | 6 +
include/video/stf-vin.h | 443 +++
37 files changed, 23803 insertions(+)
create mode 100644 drivers/media/platform/starfive/Kconfig
create mode 100644 drivers/media/platform/starfive/Makefile
create mode 100644 drivers/media/platform/starfive/v4l2_driver/Readme.txt
create mode 100644 drivers/media/platform/starfive/v4l2_driver/imx219_mipi.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/ov13850_mipi.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/ov4689_mipi.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/ov5640.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/sc2235.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_common.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_csi.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_csi.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_csi_hw_ops.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_csiphy.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_csiphy.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_csiphy_hw_ops.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_dmabuf.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_dmabuf.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_dvp.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_dvp.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_dvp_hw_ops.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_event.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_isp.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_isp.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_isp_hw_ops.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_isp_ioctl.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_video.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_video.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_vin.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_vin.h
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stf_vin_hw_ops.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stfcamss.c
create mode 100644 drivers/media/platform/starfive/v4l2_driver/stfcamss.h
create mode 100644 include/uapi/linux/jh7110-isp.h
create mode 100644 include/video/stf-vin.h
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -80,6 +80,7 @@ source "drivers/media/platform/renesas/K
source "drivers/media/platform/rockchip/Kconfig"
source "drivers/media/platform/samsung/Kconfig"
source "drivers/media/platform/st/Kconfig"
+source "drivers/media/platform/starfive/Kconfig"
source "drivers/media/platform/sunxi/Kconfig"
source "drivers/media/platform/ti/Kconfig"
source "drivers/media/platform/verisilicon/Kconfig"
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -23,6 +23,7 @@ obj-y += renesas/
obj-y += rockchip/
obj-y += samsung/
obj-y += st/
+obj-y += starfive/
obj-y += sunxi/
obj-y += ti/
obj-y += verisilicon/
--- /dev/null
+++ b/drivers/media/platform/starfive/Kconfig
@@ -0,0 +1,56 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+comment "Starfive media platform drivers"
+
+config VIN_SENSOR_OV5640
+ tristate "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
+ tristate "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
+ tristate "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
+
+config VIN_SENSOR_OV13850
+ bool "VIN SENSOR support OV13850"
+ depends on VIDEO_STF_VIN
+ select V4L2_FWNODE
+ default n
+ help
+ Say Y here if you want to have support for VIN sensor OV13850
+
+config VIN_SENSOR_IMX219
+ tristate "VIN SENSOR support IMX219"
+ depends on VIDEO_STF_VIN
+ select V4L2_FWNODE
+ default n
+ help
+ Say Y here if you want to have support for VIN sensor IMX219
+
+config VIDEO_STF_VIN
+ tristate "starfive VIC video in support"
+ depends on V4L_PLATFORM_DRIVERS
+ depends on VIDEO_DEV
+ 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.
--- /dev/null
+++ b/drivers/media/platform/starfive/Makefile
@@ -0,0 +1,24 @@
+# SPDX-License-Identifier: GPL-2.0
+
+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_VIN_SENSOR_OV13850) += v4l2_driver/ov13850_mipi.o
+obj-$(CONFIG_VIN_SENSOR_IMX219) += v4l2_driver/imx219_mipi.o
+
+starfivecamss-objs += 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 \
+ v4l2_driver/stf_dmabuf.o
+
+obj-$(CONFIG_VIDEO_STF_VIN) += starfivecamss.o \
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/Readme.txt
@@ -0,0 +1,11 @@
+
+/dev/video0: Output the camera data directly.
+/dev/video1: Output the data of the camera converted by isp.
+
+ensure linux/arch/riscv/configs/starfive_jh7110_defconfig:
+CONFIG_VIDEO_STF_VIN=y
+CONFIG_VIN_SENSOR_SC2235=y
+CONFIG_VIN_SENSOR_OV4689=y
+
+Only support the lane0/lane5 of dphy as clock lane, lane1/lane2/lane3/lane4
+as data lane.
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/imx219_mipi.c
@@ -0,0 +1,1583 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for Sony IMX219 cameras.
+ * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
+ *
+ * Based on Sony imx258 camera driver
+ * Copyright (C) 2018 Intel Corporation
+ *
+ * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
+ * Copyright 2018 Qtechnology A/S
+ *
+ * Flip handling taken from the Sony IMX319 driver.
+ * Copyright (C) 2018 Intel Corporation
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+#include <asm/unaligned.h>
+
+#define IMX219_REG_VALUE_08BIT 1
+#define IMX219_REG_VALUE_16BIT 2
+
+#define IMX219_REG_MODE_SELECT 0x0100
+#define IMX219_MODE_STANDBY 0x00
+#define IMX219_MODE_STREAMING 0x01
+
+/* Chip ID */
+#define IMX219_REG_CHIP_ID 0x0000
+#define IMX219_CHIP_ID 0x0219
+
+/* External clock frequency is 24.0M */
+#define IMX219_XCLK_FREQ 24000000
+
+/* Pixel rate is fixed at 182.4M for all the modes */
+#define IMX219_PIXEL_RATE 182400000
+
+#define IMX219_DEFAULT_LINK_FREQ 456000000
+
+/* V_TIMING internal */
+#define IMX219_REG_VTS 0x0160
+#define IMX219_VTS_15FPS 0x0dc6
+#define IMX219_VTS_30FPS_1080P 0x06e3
+#define IMX219_VTS_30FPS_BINNED 0x06e3
+#define IMX219_VTS_30FPS_1280x720 0x06e3
+#define IMX219_VTS_30FPS_640x480 0x06e3
+#define IMX219_VTS_MAX 0xffff
+
+#define IMX219_VBLANK_MIN 4
+
+/*Frame Length Line*/
+#define IMX219_FLL_MIN 0x08a6
+#define IMX219_FLL_MAX 0xffff
+#define IMX219_FLL_STEP 1
+#define IMX219_FLL_DEFAULT 0x0c98
+
+/* HBLANK control - read only */
+#define IMX219_PPL_DEFAULT 3448
+
+/* Exposure control */
+#define IMX219_REG_EXPOSURE 0x015a
+#define IMX219_EXPOSURE_MIN 4
+#define IMX219_EXPOSURE_STEP 1
+#define IMX219_EXPOSURE_DEFAULT 0x640
+#define IMX219_EXPOSURE_MAX 65535
+
+/* Analog gain control */
+#define IMX219_REG_ANALOG_GAIN 0x0157
+#define IMX219_ANA_GAIN_MIN 0
+#define IMX219_ANA_GAIN_MAX 232
+#define IMX219_ANA_GAIN_STEP 1
+#define IMX219_ANA_GAIN_DEFAULT 0xd0
+
+/* Digital gain control */
+#define IMX219_REG_DIGITAL_GAIN 0x0158
+#define IMX219_DGTL_GAIN_MIN 0x0100
+#define IMX219_DGTL_GAIN_MAX 0x0fff
+#define IMX219_DGTL_GAIN_DEFAULT 0x0100
+#define IMX219_DGTL_GAIN_STEP 1
+
+#define IMX219_REG_ORIENTATION 0x0172
+
+/* Test Pattern Control */
+#define IMX219_REG_TEST_PATTERN 0x0600
+#define IMX219_TEST_PATTERN_DISABLE 0
+#define IMX219_TEST_PATTERN_SOLID_COLOR 1
+#define IMX219_TEST_PATTERN_COLOR_BARS 2
+#define IMX219_TEST_PATTERN_GREY_COLOR 3
+#define IMX219_TEST_PATTERN_PN9 4
+
+/* Test pattern colour components */
+#define IMX219_REG_TESTP_RED 0x0602
+#define IMX219_REG_TESTP_GREENR 0x0604
+#define IMX219_REG_TESTP_BLUE 0x0606
+#define IMX219_REG_TESTP_GREENB 0x0608
+#define IMX219_TESTP_COLOUR_MIN 0
+#define IMX219_TESTP_COLOUR_MAX 0x03ff
+#define IMX219_TESTP_COLOUR_STEP 1
+#define IMX219_TESTP_RED_DEFAULT IMX219_TESTP_COLOUR_MAX
+#define IMX219_TESTP_GREENR_DEFAULT 0
+#define IMX219_TESTP_BLUE_DEFAULT 0
+#define IMX219_TESTP_GREENB_DEFAULT 0
+
+/* IMX219 native and active pixel array size. */
+#define IMX219_NATIVE_WIDTH 3296U
+#define IMX219_NATIVE_HEIGHT 2480U
+#define IMX219_PIXEL_ARRAY_LEFT 8U
+#define IMX219_PIXEL_ARRAY_TOP 8U
+#define IMX219_PIXEL_ARRAY_WIDTH 3280U
+#define IMX219_PIXEL_ARRAY_HEIGHT 2464U
+
+struct imx219_reg {
+ u16 address;
+ u8 val;
+};
+
+struct imx219_reg_list {
+ unsigned int num_of_regs;
+ const struct imx219_reg *regs;
+};
+
+/* Mode : resolution and related config&values */
+struct imx219_mode {
+ /* Frame width */
+ unsigned int width;
+ /* Frame height */
+ unsigned int height;
+
+ unsigned int fps;
+
+ /* Analog crop rectangle. */
+ struct v4l2_rect crop;
+
+ /* V-timing */
+ unsigned int vts_def;
+
+ /* Default register values */
+ struct imx219_reg_list reg_list;
+};
+
+/*
+ * Register sets lifted off the i2C interface from the Raspberry Pi firmware
+ * driver.
+ * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
+ */
+
+static const struct imx219_reg mode_1920_1080_regs[] = {
+ {0x0100, 0x00},
+ {0x30eb, 0x05},
+ {0x30eb, 0x0c},
+ {0x300a, 0xff},
+ {0x300b, 0xff},
+ {0x30eb, 0x05},
+ {0x30eb, 0x09},
+ {0x0114, 0x01},
+ {0x0128, 0x00},
+ {0x012a, 0x18},
+ {0x012b, 0x00},
+ {0x0162, 0x0d},
+ {0x0163, 0x78},
+ {0x0164, 0x02},
+ {0x0165, 0xa8},
+ {0x0166, 0x0a},
+ {0x0167, 0x27},
+ {0x0168, 0x02},
+ {0x0169, 0xb4},
+ {0x016a, 0x06},
+ {0x016b, 0xeb},
+ {0x016c, 0x07},
+ {0x016d, 0x80},
+ {0x016e, 0x04},
+ {0x016f, 0x38},
+ {0x0170, 0x01},
+ {0x0171, 0x01},
+ {0x0174, 0x00},
+ {0x0175, 0x00},
+ {0x0301, 0x05},
+ {0x0303, 0x01},
+ {0x0304, 0x03},
+ {0x0305, 0x03},
+ {0x0306, 0x00},
+ {0x0307, 0x39},
+ {0x030b, 0x01},
+ {0x030c, 0x00},
+ {0x030d, 0x72},
+ {0x0624, 0x07},
+ {0x0625, 0x80},
+ {0x0626, 0x04},
+ {0x0627, 0x38},
+ {0x455e, 0x00},
+ {0x471e, 0x4b},
+ {0x4767, 0x0f},
+ {0x4750, 0x14},
+ {0x4540, 0x00},
+ {0x47b4, 0x14},
+ {0x4713, 0x30},
+ {0x478b, 0x10},
+ {0x478f, 0x10},
+ {0x4793, 0x10},
+ {0x4797, 0x0e},
+ {0x479b, 0x0e},
+};
+
+static const struct imx219_reg mode_1280_720_regs[] = {
+ {0x0100, 0x00},
+ {0x30eb, 0x05},
+ {0x30eb, 0x0c},
+ {0x300a, 0xff},
+ {0x300b, 0xff},
+ {0x30eb, 0x05},
+ {0x30eb, 0x09},
+ {0x0114, 0x01},
+ {0x0128, 0x00},
+ {0x012a, 0x18},
+ {0x012b, 0x00},
+ {0x0162, 0x0d},
+ {0x0163, 0x78},
+ {0x0164, 0x01},
+ {0x0165, 0x68},
+ {0x0166, 0x0b},
+ {0x0167, 0x67},
+ {0x0168, 0x02},
+ {0x0169, 0x00},
+ {0x016a, 0x07},
+ {0x016b, 0x9f},
+ {0x016c, 0x05},
+ {0x016d, 0x00},
+ {0x016e, 0x02},
+ {0x016f, 0xd0},
+ {0x0170, 0x01},
+ {0x0171, 0x01},
+ {0x0174, 0x01},
+ {0x0175, 0x01},
+ {0x0301, 0x05},
+ {0x0303, 0x01},
+ {0x0304, 0x03},
+ {0x0305, 0x03},
+ {0x0306, 0x00},
+ {0x0307, 0x39},
+ {0x030b, 0x01},
+ {0x030c, 0x00},
+ {0x030d, 0x72},
+ {0x0624, 0x06},
+ {0x0625, 0x68},
+ {0x0626, 0x04},
+ {0x0627, 0xd0},
+ {0x455e, 0x00},
+ {0x471e, 0x4b},
+ {0x4767, 0x0f},
+ {0x4750, 0x14},
+ {0x4540, 0x00},
+ {0x47b4, 0x14},
+ {0x4713, 0x30},
+ {0x478b, 0x10},
+ {0x478f, 0x10},
+ {0x4793, 0x10},
+ {0x4797, 0x0e},
+ {0x479b, 0x0e},
+};
+
+static const struct imx219_reg mode_640_480_regs[] = {
+ {0x0100, 0x00},
+ {0x30eb, 0x05},
+ {0x30eb, 0x0c},
+ {0x300a, 0xff},
+ {0x300b, 0xff},
+ {0x30eb, 0x05},
+ {0x30eb, 0x09},
+ {0x0114, 0x01},
+ {0x0128, 0x00},
+ {0x012a, 0x18},
+ {0x012b, 0x00},
+ {0x0162, 0x0d},
+ {0x0163, 0x78},
+ {0x0164, 0x03},
+ {0x0165, 0xe8},
+ {0x0166, 0x08},
+ {0x0167, 0xe7},
+ {0x0168, 0x02},
+ {0x0169, 0xf0},
+ {0x016a, 0x06},
+ {0x016b, 0xaf},
+ {0x016c, 0x02},
+ {0x016d, 0x80},
+ {0x016e, 0x01},
+ {0x016f, 0xe0},
+ {0x0170, 0x01},
+ {0x0171, 0x01},
+ {0x0174, 0x03},
+ {0x0175, 0x03},
+ {0x0301, 0x05},
+ {0x0303, 0x01},
+ {0x0304, 0x03},
+ {0x0305, 0x03},
+ {0x0306, 0x00},
+ {0x0307, 0x39},
+ {0x030b, 0x01},
+ {0x030c, 0x00},
+ {0x030d, 0x72},
+ {0x0624, 0x06},
+ {0x0625, 0x68},
+ {0x0626, 0x04},
+ {0x0627, 0xd0},
+ {0x455e, 0x00},
+ {0x471e, 0x4b},
+ {0x4767, 0x0f},
+ {0x4750, 0x14},
+ {0x4540, 0x00},
+ {0x47b4, 0x14},
+ {0x4713, 0x30},
+ {0x478b, 0x10},
+ {0x478f, 0x10},
+ {0x4793, 0x10},
+ {0x4797, 0x0e},
+ {0x479b, 0x0e},
+};
+
+static const struct imx219_reg raw8_framefmt_regs[] = {
+ {0x018c, 0x08},
+ {0x018d, 0x08},
+ {0x0309, 0x08},
+};
+
+static const struct imx219_reg raw10_framefmt_regs[] = {
+ {0x018c, 0x0a},
+ {0x018d, 0x0a},
+ {0x0309, 0x0a},
+};
+
+static const s64 imx219_link_freq_menu[] = {
+ IMX219_DEFAULT_LINK_FREQ,
+};
+
+static const char * const imx219_test_pattern_menu[] = {
+ "Disabled",
+ "Color Bars",
+ "Solid Color",
+ "Grey Color Bars",
+ "PN9"
+};
+
+static const int imx219_test_pattern_val[] = {
+ IMX219_TEST_PATTERN_DISABLE,
+ IMX219_TEST_PATTERN_COLOR_BARS,
+ IMX219_TEST_PATTERN_SOLID_COLOR,
+ IMX219_TEST_PATTERN_GREY_COLOR,
+ IMX219_TEST_PATTERN_PN9,
+};
+
+/* regulator supplies */
+static const char * const imx219_supply_name[] = {
+ /* Supplies can be enabled in any order */
+ "VANA", /* Analog (2.8V) supply */
+ "VDIG", /* Digital Core (1.8V) supply */
+ "VDDL", /* IF (1.2V) supply */
+};
+
+#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
+
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 codes[] = {
+ MEDIA_BUS_FMT_SRGGB10_1X10,
+ MEDIA_BUS_FMT_SGRBG10_1X10,
+ MEDIA_BUS_FMT_SGBRG10_1X10,
+ MEDIA_BUS_FMT_SBGGR10_1X10,
+
+ MEDIA_BUS_FMT_SRGGB8_1X8,
+ MEDIA_BUS_FMT_SGRBG8_1X8,
+ MEDIA_BUS_FMT_SGBRG8_1X8,
+ MEDIA_BUS_FMT_SBGGR8_1X8,
+};
+
+/*
+ * Initialisation delay between XCLR low->high and the moment when the sensor
+ * can start capture (i.e. can leave software stanby) must be not less than:
+ * t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
+ * where
+ * t4 is fixed, and is max 200uS,
+ * t5 is fixed, and is 6000uS,
+ * t6 depends on the sensor external clock, and is max 32000 clock periods.
+ * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
+ * So for any acceptable external clock t6 is always within the range of
+ * 1185 to 5333 uS, and is always less than t5.
+ * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
+ * initialize the sensor over I2C, and then exit the software standby.
+ *
+ * This start-up time can be optimized a bit more, if we start the writes
+ * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
+ * initialization over I2C may complete before (t4+t5) expires, and we must
+ * ensure that capture is not started before (t4+t5).
+ *
+ * This delay doesn't account for the power supply startup time. If needed,
+ * this should be taken care of via the regulator framework. E.g. in the
+ * case of DT for regulator-fixed one should define the startup-delay-us
+ * property.
+ */
+#define IMX219_XCLR_MIN_DELAY_US 6200
+#define IMX219_XCLR_DELAY_RANGE_US 1000
+
+/* Mode configs */
+static const struct imx219_mode supported_modes[] = {
+ {
+ /* 1080P 30fps cropped */
+ .width = 1920,
+ .height = 1080,
+ .fps = 30,
+ .crop = {
+ .left = 688,
+ .top = 700,
+ .width = 1920,
+ .height = 1080
+ },
+ .vts_def = IMX219_VTS_30FPS_1080P,
+ .reg_list = {
+ .num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
+ .regs = mode_1920_1080_regs,
+ },
+ },
+ {
+ /* 1280x720 30fps mode */
+ .width = 1280,
+ .height = 720,
+ .fps = 30,
+ .crop = {
+ .left = 360,
+ .top = 512,
+ .width = 2560,
+ .height = 1440
+ },
+ .vts_def = IMX219_VTS_30FPS_1280x720,
+ .reg_list = {
+ .num_of_regs = ARRAY_SIZE(mode_1280_720_regs),
+ .regs = mode_1280_720_regs,
+ },
+ },
+ {
+ /* 640x480 30fps mode */
+ .width = 640,
+ .height = 480,
+ .fps = 30,
+ .crop = {
+ .left = 1008,
+ .top = 760,
+ .width = 1280,
+ .height = 960
+ },
+ .vts_def = IMX219_VTS_30FPS_640x480,
+ .reg_list = {
+ .num_of_regs = ARRAY_SIZE(mode_640_480_regs),
+ .regs = mode_640_480_regs,
+ },
+ },
+};
+
+struct imx219 {
+ struct v4l2_subdev sd;
+ struct media_pad pad;
+ //struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
+
+ struct v4l2_mbus_framefmt fmt;
+
+ struct clk *xclk; /* system clock to IMX219 */
+ u32 xclk_freq;
+
+ struct gpio_desc *reset_gpio;
+ struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
+
+ struct v4l2_ctrl_handler ctrl_handler;
+ /* V4L2 Controls */
+ struct v4l2_ctrl *pixel_rate;
+ struct v4l2_ctrl *link_freq;
+ struct v4l2_ctrl *exposure;
+ struct v4l2_ctrl *vflip;
+ struct v4l2_ctrl *hflip;
+ struct v4l2_ctrl *vblank;
+ struct v4l2_ctrl *hblank;
+
+ /* Current mode */
+ const struct imx219_mode *mode;
+ struct v4l2_fract frame_interval;
+
+ /*
+ * Mutex for serialized access:
+ * Protect sensor module set pad format and start/stop streaming safely.
+ */
+ struct mutex mutex;
+
+ /* Streaming on/off */
+ int streaming;
+};
+
+static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
+{
+ return container_of(_sd, struct imx219, sd);
+}
+
+/* Read registers up to 2 at a time */
+static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ struct i2c_msg msgs[2];
+ u8 addr_buf[2] = { reg >> 8, reg & 0xff };
+ u8 data_buf[4] = { 0, };
+ int ret;
+
+ if (len > 4)
+ return -EINVAL;
+
+ /* Write register address */
+ msgs[0].addr = client->addr;
+ msgs[0].flags = 0;
+ msgs[0].len = ARRAY_SIZE(addr_buf);
+ msgs[0].buf = addr_buf;
+
+ /* Read data from register */
+ msgs[1].addr = client->addr;
+ msgs[1].flags = I2C_M_RD;
+ msgs[1].len = len;
+ msgs[1].buf = &data_buf[4 - len];
+
+ ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+ if (ret != ARRAY_SIZE(msgs))
+ return -EIO;
+
+ *val = get_unaligned_be32(data_buf);
+
+ return 0;
+}
+
+/* Write registers up to 2 at a time */
+static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ u8 buf[6];
+
+ if (len > 4)
+ return -EINVAL;
+
+ put_unaligned_be16(reg, buf);
+ put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+ if (i2c_master_send(client, buf, len + 2) != len + 2)
+ return -EIO;
+
+ return 0;
+}
+
+/* Write a list of registers */
+static int imx219_write_regs(struct imx219 *imx219,
+ const struct imx219_reg *regs, u32 len)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ unsigned int i;
+ int ret;
+
+ for (i = 0; i < len; i++) {
+ ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
+ if (ret) {
+ dev_err_ratelimited(&client->dev,
+ "Failed to write reg 0x%4.4x. error = %d\n",
+ regs[i].address, ret);
+
+ return ret;
+ }
+ }
+
+ return 0;
+}
+
+/* Get bayer order based on flip setting. */
+static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
+{
+ unsigned int i;
+
+ lockdep_assert_held(&imx219->mutex);
+
+ for (i = 0; i < ARRAY_SIZE(codes); i++)
+ if (codes[i] == code)
+ break;
+
+ if (i >= ARRAY_SIZE(codes))
+ i = 0;
+
+ i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
+ (imx219->hflip->val ? 1 : 0);
+
+ return codes[i];
+}
+
+static void imx219_set_default_format(struct imx219 *imx219)
+{
+ struct v4l2_mbus_framefmt *fmt;
+
+ fmt = &imx219->fmt;
+ fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+ fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+ fmt->colorspace, fmt->ycbcr_enc);
+ fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+ fmt->width = supported_modes[0].width;
+ fmt->height = supported_modes[0].height;
+ fmt->field = V4L2_FIELD_NONE;
+}
+
+static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ struct v4l2_mbus_framefmt *try_fmt =
+ v4l2_subdev_get_try_format(sd, fh->state, 0);
+ struct v4l2_rect *try_crop;
+
+ mutex_lock(&imx219->mutex);
+
+ /* Initialize try_fmt */
+ try_fmt->width = supported_modes[0].width;
+ try_fmt->height = supported_modes[0].height;
+ try_fmt->code = imx219_get_format_code(imx219, MEDIA_BUS_FMT_SRGGB10_1X10);
+ try_fmt->field = V4L2_FIELD_NONE;
+
+ /* Initialize try_crop rectangle. */
+ try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
+ try_crop->top = IMX219_PIXEL_ARRAY_TOP;
+ try_crop->left = IMX219_PIXEL_ARRAY_LEFT;
+ try_crop->width = IMX219_PIXEL_ARRAY_WIDTH;
+ try_crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
+
+ mutex_unlock(&imx219->mutex);
+
+ return 0;
+}
+
+static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct imx219 *imx219 =
+ container_of(ctrl->handler, struct imx219, ctrl_handler);
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ int ret;
+
+ if (ctrl->id == V4L2_CID_VBLANK) {
+ int exposure_max, exposure_def;
+
+ /* Update max exposure while meeting expected vblanking */
+ exposure_max = imx219->mode->height + ctrl->val - 4;
+ exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+ exposure_max : IMX219_EXPOSURE_DEFAULT;
+ __v4l2_ctrl_modify_range(imx219->exposure, imx219->exposure->minimum,
+ exposure_max, imx219->exposure->step, exposure_def);
+ }
+
+ /*
+ * Applying V4L2 control value only happens
+ * when power is up for streaming
+ */
+ if (pm_runtime_get_if_in_use(&client->dev) == 0)
+ return 0;
+
+ switch (ctrl->id) {
+ case V4L2_CID_ANALOGUE_GAIN:
+ ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
+ IMX219_REG_VALUE_08BIT, ctrl->val);
+ break;
+ case V4L2_CID_EXPOSURE:
+ ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
+ IMX219_REG_VALUE_16BIT, ctrl->val);
+ break;
+ case V4L2_CID_DIGITAL_GAIN:
+ ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
+ IMX219_REG_VALUE_16BIT, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN:
+ ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
+ IMX219_REG_VALUE_16BIT, imx219_test_pattern_val[ctrl->val]);
+ break;
+ case V4L2_CID_HFLIP:
+ case V4L2_CID_VFLIP:
+ ret = imx219_write_reg(imx219, IMX219_REG_ORIENTATION, 1,
+ imx219->hflip->val | imx219->vflip->val << 1);
+ break;
+ case V4L2_CID_VBLANK:
+ ret = imx219_write_reg(imx219, IMX219_REG_VTS, IMX219_REG_VALUE_16BIT,
+ imx219->mode->height + ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN_RED:
+ ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
+ IMX219_REG_VALUE_16BIT, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN_GREENR:
+ ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
+ IMX219_REG_VALUE_16BIT, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN_BLUE:
+ ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
+ IMX219_REG_VALUE_16BIT, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN_GREENB:
+ ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
+ IMX219_REG_VALUE_16BIT, ctrl->val);
+ break;
+ default:
+ dev_info(&client->dev,
+ "ctrl(id:0x%x,val:0x%x) is not handled\n", ctrl->id, ctrl->val);
+ ret = -EINVAL;
+ break;
+ }
+
+ pm_runtime_put(&client->dev);
+
+ return ret;
+}
+
+static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
+ .s_ctrl = imx219_set_ctrl,
+};
+
+static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+
+ if (code->index >= (ARRAY_SIZE(codes) / 4))
+ return -EINVAL;
+
+ mutex_lock(&imx219->mutex);
+ code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
+ mutex_unlock(&imx219->mutex);
+
+ return 0;
+}
+
+static int imx219_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ u32 code;
+
+ if (fse->index >= ARRAY_SIZE(supported_modes))
+ return -EINVAL;
+
+ mutex_lock(&imx219->mutex);
+ code = imx219_get_format_code(imx219, fse->code);
+ mutex_unlock(&imx219->mutex);
+ if (fse->code != code)
+ return -EINVAL;
+
+ fse->min_width = supported_modes[fse->index].width;
+ fse->max_width = fse->min_width;
+ fse->min_height = supported_modes[fse->index].height;
+ fse->max_height = fse->min_height;
+
+ return 0;
+}
+
+static int imx219_try_frame_interval(struct imx219 *imx219,
+ struct v4l2_fract *fi,
+ u32 w, u32 h)
+{
+ const struct imx219_mode *mode;
+
+ mode = v4l2_find_nearest_size(supported_modes, ARRAY_SIZE(supported_modes),
+ width, height, w, h);
+ if (!mode || (mode->width != w || mode->height != h))
+ return -EINVAL;
+
+ fi->numerator = 1;
+ fi->denominator = mode->fps;
+
+ return mode->fps;
+}
+
+static int imx219_enum_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_interval_enum *fie)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ struct v4l2_fract tpf;
+ u32 code;
+ int ret;
+
+ if (fie->index >= ARRAY_SIZE(supported_modes))
+ return -EINVAL;
+
+ mutex_lock(&imx219->mutex);
+ code = imx219_get_format_code(imx219, fie->code);
+ if (fie->code != code) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ ret = imx219_try_frame_interval(imx219, &tpf,
+ fie->width, fie->height);
+ if (ret < 0) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ mutex_unlock(&imx219->mutex);
+ fie->interval = tpf;
+
+ return 0;
+
+out:
+ mutex_unlock(&imx219->mutex);
+ return ret;
+}
+
+static int imx219_g_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+
+ mutex_lock(&imx219->mutex);
+ fi->interval = imx219->frame_interval;
+ mutex_unlock(&imx219->mutex);
+
+ return 0;
+}
+
+static int imx219_s_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ const struct imx219_mode *mode = imx219->mode;
+ int frame_rate, ret = 0;
+
+ if (fi->pad != 0)
+ return -EINVAL;
+
+ mutex_lock(&imx219->mutex);
+
+ if (imx219->streaming) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ frame_rate = imx219_try_frame_interval(imx219, &fi->interval,
+ mode->width, mode->height);
+ if (frame_rate < 0) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ imx219->frame_interval = fi->interval;
+
+out:
+ mutex_unlock(&imx219->mutex);
+ return ret;
+}
+
+static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
+{
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+ fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+ fmt->colorspace, fmt->ycbcr_enc);
+ fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+}
+
+static void imx219_update_pad_format(struct imx219 *imx219,
+ const struct imx219_mode *mode,
+ struct v4l2_subdev_format *fmt)
+{
+ fmt->format.width = mode->width;
+ fmt->format.height = mode->height;
+ fmt->format.field = V4L2_FIELD_NONE;
+ imx219_reset_colorspace(&fmt->format);
+}
+
+static int __imx219_get_pad_format(struct imx219 *imx219,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+ struct v4l2_mbus_framefmt *try_fmt =
+ v4l2_subdev_get_try_format(&imx219->sd, state, fmt->pad);
+ /* update the code which could change due to vflip or hflip: */
+ try_fmt->code = imx219_get_format_code(imx219, try_fmt->code);
+ fmt->format = *try_fmt;
+ } else {
+ imx219_update_pad_format(imx219, imx219->mode, fmt);
+ fmt->format.code = imx219_get_format_code(imx219, imx219->fmt.code);
+ }
+
+ return 0;
+}
+
+static int imx219_get_pad_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ int ret;
+
+ mutex_lock(&imx219->mutex);
+ ret = __imx219_get_pad_format(imx219, state, fmt);
+ mutex_unlock(&imx219->mutex);
+
+ return ret;
+}
+
+static int imx219_set_pad_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ const struct imx219_mode *mode;
+ struct v4l2_mbus_framefmt *framefmt;
+ int exposure_max, exposure_def, hblank;
+ unsigned int i;
+
+ mutex_lock(&imx219->mutex);
+
+ for (i = 0; i < ARRAY_SIZE(codes); i++)
+ if (codes[i] == fmt->format.code)
+ break;
+ if (i >= ARRAY_SIZE(codes))
+ i = 0;
+
+ /* Bayer order varies with flips */
+ fmt->format.code = imx219_get_format_code(imx219, codes[i]);
+
+ mode = v4l2_find_nearest_size(supported_modes, ARRAY_SIZE(supported_modes),
+ width, height, fmt->format.width, fmt->format.height);
+ imx219_update_pad_format(imx219, mode, fmt);
+ if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+ framefmt = v4l2_subdev_get_try_format(sd, state, fmt->pad);
+ *framefmt = fmt->format;
+ } else if (imx219->mode != mode ||
+ imx219->fmt.code != fmt->format.code) {
+ imx219->fmt = fmt->format;
+ imx219->mode = mode;
+ /* Update limits and set FPS to default */
+ __v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
+ IMX219_VTS_MAX - mode->height, 1,
+ mode->vts_def - mode->height);
+ __v4l2_ctrl_s_ctrl(imx219->vblank, mode->vts_def - mode->height);
+ /* Update max exposure while meeting expected vblanking */
+ exposure_max = mode->vts_def - 4;
+ exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+ exposure_max : IMX219_EXPOSURE_DEFAULT;
+ __v4l2_ctrl_modify_range(imx219->exposure, imx219->exposure->minimum,
+ exposure_max, imx219->exposure->step, exposure_def);
+ /*
+ * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
+ * depends on mode->width only, and is not changeble in any
+ * way other than changing the mode.
+ */
+ hblank = IMX219_PPL_DEFAULT - mode->width;
+ __v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1, hblank);
+ }
+
+ mutex_unlock(&imx219->mutex);
+
+ return 0;
+}
+
+static int imx219_set_framefmt(struct imx219 *imx219)
+{
+ switch (imx219->fmt.code) {
+ case MEDIA_BUS_FMT_SRGGB8_1X8:
+ case MEDIA_BUS_FMT_SGRBG8_1X8:
+ case MEDIA_BUS_FMT_SGBRG8_1X8:
+ case MEDIA_BUS_FMT_SBGGR8_1X8:
+ return imx219_write_regs(imx219, raw8_framefmt_regs,
+ ARRAY_SIZE(raw8_framefmt_regs));
+
+ case MEDIA_BUS_FMT_SRGGB10_1X10:
+ case MEDIA_BUS_FMT_SGRBG10_1X10:
+ case MEDIA_BUS_FMT_SGBRG10_1X10:
+ case MEDIA_BUS_FMT_SBGGR10_1X10:
+ return imx219_write_regs(imx219, raw10_framefmt_regs,
+ ARRAY_SIZE(raw10_framefmt_regs));
+ }
+
+ return -EINVAL;
+}
+
+static const struct v4l2_rect *
+__imx219_get_pad_crop(struct imx219 *imx219, struct v4l2_subdev_state *state,
+ unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+ switch (which) {
+ case V4L2_SUBDEV_FORMAT_TRY:
+ return v4l2_subdev_get_try_crop(&imx219->sd, state, pad);
+ case V4L2_SUBDEV_FORMAT_ACTIVE:
+ return &imx219->mode->crop;
+ }
+
+ return NULL;
+}
+
+static int imx219_get_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ switch (sel->target) {
+ case V4L2_SEL_TGT_CROP: {
+ struct imx219 *imx219 = to_imx219(sd);
+
+ mutex_lock(&imx219->mutex);
+ sel->r = *__imx219_get_pad_crop(imx219, state, sel->pad, sel->which);
+ mutex_unlock(&imx219->mutex);
+ return 0;
+ }
+
+ case V4L2_SEL_TGT_NATIVE_SIZE:
+ sel->r.top = 0;
+ sel->r.left = 0;
+ sel->r.width = IMX219_NATIVE_WIDTH;
+ sel->r.height = IMX219_NATIVE_HEIGHT;
+ return 0;
+
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ sel->r.top = IMX219_PIXEL_ARRAY_TOP;
+ sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
+ sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
+ sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
+ return 0;
+ }
+
+ return -EINVAL;
+}
+
+static int imx219_start_streaming(struct imx219 *imx219)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ const struct imx219_reg_list *reg_list;
+ int ret;
+
+ /* Apply default values of current mode */
+ reg_list = &imx219->mode->reg_list;
+ ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
+ if (ret) {
+ dev_err(&client->dev, "%s failed to set mode\n", __func__);
+ goto err;
+ }
+
+ ret = imx219_set_framefmt(imx219);
+ if (ret) {
+ dev_err(&client->dev, "%s failed to set frame format: %d\n",
+ __func__, ret);
+ goto err;
+ }
+
+ /* Apply customized values from user */
+ ret = __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
+ if (ret)
+ goto err;
+
+ /* set stream on register */
+ ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+ IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
+ if (ret)
+ goto err;
+
+ /* vflip and hflip cannot change during streaming */
+ __v4l2_ctrl_grab(imx219->vflip, true);
+ __v4l2_ctrl_grab(imx219->hflip, true);
+
+ return 0;
+
+err:
+ return ret;
+}
+
+static void imx219_stop_streaming(struct imx219 *imx219)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ int ret;
+
+ /* set stream off register */
+ ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+ IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
+ if (ret)
+ dev_err(&client->dev, "%s failed to set stream\n", __func__);
+
+ __v4l2_ctrl_grab(imx219->vflip, false);
+ __v4l2_ctrl_grab(imx219->hflip, false);
+}
+
+static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct imx219 *imx219 = to_imx219(sd);
+ struct i2c_client *client = v4l2_get_subdevdata(sd);
+ int ret = 0;
+
+ mutex_lock(&imx219->mutex);
+
+ if (enable) {
+ ret = pm_runtime_get_sync(&client->dev);
+ if (ret < 0) {
+ pm_runtime_put_noidle(&client->dev);
+ mutex_unlock(&imx219->mutex);
+ return ret;
+ }
+
+ if (imx219->streaming)
+ goto unlock;
+
+ /*
+ * Apply default & customized values
+ * and then start streaming.
+ */
+ ret = imx219_start_streaming(imx219);
+ if (ret)
+ goto err_unlock;
+ } else {
+ imx219_stop_streaming(imx219);
+ pm_runtime_put(&client->dev);
+ }
+
+unlock:
+ imx219->streaming += enable ? 1 : -1;
+ WARN_ON(imx219->streaming < 0);
+
+ mutex_unlock(&imx219->mutex);
+
+ return ret;
+
+err_unlock:
+ pm_runtime_put(&client->dev);
+ mutex_unlock(&imx219->mutex);
+
+ return ret;
+}
+
+/* Power/clock management functions */
+static int imx219_power_on(struct device *dev)
+{
+ struct v4l2_subdev *sd = dev_get_drvdata(dev);
+ struct imx219 *imx219 = to_imx219(sd);
+ int ret;
+
+ ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES, imx219->supplies);
+ if (ret) {
+ dev_err(dev, "%s: failed to enable regulators\n",
+ __func__);
+ return ret;
+ }
+
+ ret = clk_prepare_enable(imx219->xclk);
+ if (ret) {
+ dev_err(dev, "%s: failed to enable clock\n", __func__);
+ goto reg_off;
+ }
+
+ gpiod_set_value_cansleep(imx219->reset_gpio, 1);
+ usleep_range(IMX219_XCLR_MIN_DELAY_US,
+ IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
+
+ return 0;
+
+reg_off:
+ regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+
+ return ret;
+}
+
+static int imx219_power_off(struct device *dev)
+{
+ struct v4l2_subdev *sd = dev_get_drvdata(dev);
+ struct imx219 *imx219 = to_imx219(sd);
+
+ gpiod_set_value_cansleep(imx219->reset_gpio, 0);
+ regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+ clk_disable_unprepare(imx219->xclk);
+
+ return 0;
+}
+
+static int imx219_get_regulators(struct imx219 *imx219)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ unsigned int i;
+
+ for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
+ imx219->supplies[i].supply = imx219_supply_name[i];
+
+ return devm_regulator_bulk_get(&client->dev,
+ IMX219_NUM_SUPPLIES, imx219->supplies);
+}
+
+/* Verify chip ID */
+static int imx219_identify_module(struct imx219 *imx219)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ int ret;
+ u32 val;
+
+ ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
+ IMX219_REG_VALUE_16BIT, &val);
+ if (ret) {
+ dev_err(&client->dev, "failed to read chip id %x\n",
+ IMX219_CHIP_ID);
+ return ret;
+ }
+
+ if (val != IMX219_CHIP_ID) {
+ dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+ IMX219_CHIP_ID, val);
+ return -EIO;
+ }
+
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, IMX219_CHIP_ID);
+
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops imx219_core_ops = {
+ .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+ .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx219_video_ops = {
+ .g_frame_interval = imx219_g_frame_interval,
+ .s_frame_interval = imx219_s_frame_interval,
+ .s_stream = imx219_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
+ .enum_mbus_code = imx219_enum_mbus_code,
+ .get_fmt = imx219_get_pad_format,
+ .set_fmt = imx219_set_pad_format,
+ .get_selection = imx219_get_selection,
+ .enum_frame_size = imx219_enum_frame_size,
+ .enum_frame_interval = imx219_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops imx219_subdev_ops = {
+ .core = &imx219_core_ops,
+ .video = &imx219_video_ops,
+ .pad = &imx219_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
+ .open = imx219_open,
+};
+
+/* Initialize control handlers */
+static int imx219_init_controls(struct imx219 *imx219)
+{
+ struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+ struct v4l2_ctrl_handler *ctrl_hdlr;
+ unsigned int height = imx219->mode->height;
+ struct v4l2_fwnode_device_properties props;
+ int exposure_max, exposure_def, hblank;
+ int i, ret;
+
+ ctrl_hdlr = &imx219->ctrl_handler;
+ ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
+ if (ret)
+ return ret;
+
+ mutex_init(&imx219->mutex);
+ ctrl_hdlr->lock = &imx219->mutex;
+
+ /* By default, PIXEL_RATE is read only */
+ imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_PIXEL_RATE, IMX219_PIXEL_RATE,
+ IMX219_PIXEL_RATE, 1, IMX219_PIXEL_RATE);
+
+ imx219->link_freq =
+ v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_LINK_FREQ,
+ ARRAY_SIZE(imx219_link_freq_menu) - 1, 0, imx219_link_freq_menu);
+ if (imx219->link_freq)
+ imx219->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+ /* Initial vblank/hblank/exposure parameters based on current mode */
+ imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
+ IMX219_VTS_MAX - height, 1,
+ imx219->mode->vts_def - height);
+ hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
+ imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_HBLANK, hblank, hblank, 1, hblank);
+ if (imx219->hblank)
+ imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+ exposure_max = imx219->mode->vts_def - 4;
+ exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+ exposure_max : IMX219_EXPOSURE_DEFAULT;
+ imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_EXPOSURE, IMX219_EXPOSURE_MIN, exposure_max,
+ IMX219_EXPOSURE_STEP, exposure_def);
+
+ v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+ IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
+ IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
+
+ v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+ IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
+ IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
+
+ imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_HFLIP, 0, 1, 1, 0);
+ if (imx219->hflip)
+ imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+ imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_VFLIP, 0, 1, 1, 0);
+ if (imx219->vflip)
+ imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+ v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_TEST_PATTERN,
+ ARRAY_SIZE(imx219_test_pattern_menu) - 1,
+ 0, 0, imx219_test_pattern_menu);
+ for (i = 0; i < 4; i++) {
+ /*
+ * The assumption is that
+ * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
+ * V4L2_CID_TEST_PATTERN_BLUE == V4L2_CID_TEST_PATTERN_RED + 2
+ * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
+ */
+ v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+ V4L2_CID_TEST_PATTERN_RED + i,
+ IMX219_TESTP_COLOUR_MIN,
+ IMX219_TESTP_COLOUR_MAX,
+ IMX219_TESTP_COLOUR_STEP,
+ IMX219_TESTP_COLOUR_MAX);
+ /* The "Solid color" pattern is white by default */
+ }
+
+ if (ctrl_hdlr->error) {
+ ret = ctrl_hdlr->error;
+ dev_err(&client->dev, "%s control init failed (%d)\n",
+ __func__, ret);
+ goto error;
+ }
+
+ ret = v4l2_fwnode_device_parse(&client->dev, &props);
+ if (ret)
+ goto error;
+
+ ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops, &props);
+ if (ret)
+ goto error;
+
+ imx219->sd.ctrl_handler = ctrl_hdlr;
+
+ return 0;
+
+error:
+ v4l2_ctrl_handler_free(ctrl_hdlr);
+ mutex_destroy(&imx219->mutex);
+
+ return ret;
+}
+
+static void imx219_free_controls(struct imx219 *imx219)
+{
+ v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
+ mutex_destroy(&imx219->mutex);
+}
+
+static int imx219_check_hwcfg(struct device *dev)
+{
+ struct fwnode_handle *endpoint;
+ struct v4l2_fwnode_endpoint ep_cfg = {
+ .bus_type = V4L2_MBUS_CSI2_DPHY
+ };
+ int ret = -EINVAL;
+
+ endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+ if (!endpoint) {
+ dev_err(dev, "endpoint node not found\n");
+ return -EINVAL;
+ }
+
+ if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
+ dev_err(dev, "could not parse endpoint\n");
+ goto error_out;
+ }
+
+ /* Check the number of MIPI CSI2 data lanes */
+ if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2) {
+ dev_err(dev, "only 2 data lanes are currently supported\n");
+ goto error_out;
+ }
+
+ /* Check the link frequency set in device tree */
+ if (!ep_cfg.nr_of_link_frequencies) {
+ dev_err(dev, "link-frequency property not found in DT\n");
+ goto error_out;
+ }
+
+ if (ep_cfg.nr_of_link_frequencies != 1 ||
+ ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
+ dev_err(dev, "Link frequency not supported: %lld\n",
+ ep_cfg.link_frequencies[0]);
+ goto error_out;
+ }
+
+ ret = 0;
+
+error_out:
+ v4l2_fwnode_endpoint_free(&ep_cfg);
+ fwnode_handle_put(endpoint);
+
+ return ret;
+}
+
+static int imx219_probe(struct i2c_client *client)
+{
+ struct device *dev = &client->dev;
+ struct imx219 *imx219;
+ int ret;
+
+ imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
+ if (!imx219)
+ return -ENOMEM;
+
+ v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
+
+ /* Check the hardware configuration in device tree */
+ if (imx219_check_hwcfg(dev))
+ return -EINVAL;
+
+ /* Get system clock (xclk) */
+ imx219->xclk = devm_clk_get(dev, NULL);
+ if (IS_ERR(imx219->xclk)) {
+ dev_err(dev, "failed to get xclk\n");
+ return PTR_ERR(imx219->xclk);
+ }
+
+ imx219->xclk_freq = clk_get_rate(imx219->xclk);
+ if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
+ dev_err(dev, "xclk frequency not supported: %d Hz\n",
+ imx219->xclk_freq);
+ return -EINVAL;
+ }
+
+ ret = imx219_get_regulators(imx219);
+ if (ret) {
+ dev_err(dev, "failed to get regulators\n");
+ return ret;
+ }
+
+ /* Request optional enable pin */
+ imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_LOW);
+ usleep_range(IMX219_XCLR_MIN_DELAY_US,
+ IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
+
+ /*
+ * The sensor must be powered for imx219_identify_module()
+ * to be able to read the CHIP_ID register
+ */
+ ret = imx219_power_on(dev);
+ if (ret)
+ return ret;
+
+ ret = imx219_identify_module(imx219);
+ if (ret)
+ goto error_power_off;
+
+ /* Set default mode to max resolution */
+ imx219->mode = &supported_modes[0];
+ imx219->frame_interval.numerator = 1;
+ imx219->frame_interval.denominator = supported_modes[0].fps;
+
+ /* sensor doesn't enter LP-11 state upon power up until and unless
+ * streaming is started, so upon power up switch the modes to:
+ * streaming -> standby
+ */
+ ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+ IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
+ if (ret < 0)
+ goto error_power_off;
+ usleep_range(100, 110);
+
+ /* put sensor back to standby mode */
+ ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+ IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
+ if (ret < 0)
+ goto error_power_off;
+ usleep_range(100, 110);
+
+ ret = imx219_init_controls(imx219);
+ if (ret)
+ goto error_power_off;
+
+ /* Initialize subdev */
+ imx219->sd.internal_ops = &imx219_internal_ops;
+ imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+ imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+ /* Initialize source pad */
+ imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+ /* Initialize default format */
+ imx219_set_default_format(imx219);
+
+ ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
+ if (ret) {
+ dev_err(dev, "failed to init entity pads: %d\n", ret);
+ goto error_handler_free;
+ }
+
+ ret = v4l2_async_register_subdev_sensor(&imx219->sd);
+ if (ret < 0) {
+ dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
+ goto error_media_entity;
+ }
+
+ /* Enable runtime PM and turn off the device */
+ pm_runtime_set_active(dev);
+ pm_runtime_enable(dev);
+ pm_runtime_idle(dev);
+
+ return 0;
+
+error_media_entity:
+ media_entity_cleanup(&imx219->sd.entity);
+
+error_handler_free:
+ imx219_free_controls(imx219);
+
+error_power_off:
+ imx219_power_off(dev);
+
+ return ret;
+}
+
+static void imx219_remove(struct i2c_client *client)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct imx219 *imx219 = to_imx219(sd);
+
+ v4l2_async_unregister_subdev(sd);
+ media_entity_cleanup(&sd->entity);
+ imx219_free_controls(imx219);
+
+ pm_runtime_disable(&client->dev);
+ if (!pm_runtime_status_suspended(&client->dev))
+ imx219_power_off(&client->dev);
+ pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id imx219_dt_ids[] = {
+ { .compatible = "sony,imx219" },
+ { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx219_dt_ids);
+
+static const struct dev_pm_ops imx219_pm_ops = {
+ SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend, pm_runtime_force_resume)
+ SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
+};
+
+static struct i2c_driver imx219_i2c_driver = {
+ .driver = {
+ .name = "imx219",
+ .of_match_table = imx219_dt_ids,
+ .pm = &imx219_pm_ops,
+ },
+ .probe = imx219_probe,
+ .remove = imx219_remove,
+};
+
+module_i2c_driver(imx219_i2c_driver);
+
+MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com");
+MODULE_DESCRIPTION("Sony IMX219 sensor driver");
+MODULE_LICENSE("GPL v2");
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/ov13850_mipi.c
@@ -0,0 +1,1921 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2021-2023 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 OV13850_XCLK_MIN 6000000
+#define OV13850_XCLK_MAX 54000000
+
+#define OV13850_LINK_FREQ_500MHZ 500000000LL
+
+/**
+ *OV13850 PLL
+ *
+ *PLL1:
+ *
+ *REF_CLK -> /PREDIVP[0] -> /PREDIV[2:0] -> *DIVP[9:8,7:0] -> /DIVM[3:0] -> /DIV_MIPI[1:0] -> PCLK
+ *(6-64M) 0x030A 0x0300 0x0301,0x302 0x0303 0x0304
+ * `-> MIPI_PHY_CLK
+ *
+ *
+ *PLL2:
+ * 000: 1
+ * 001: 1.5
+ * 010: 2
+ * 011: 2.5
+ * 100: 3
+ * 101: 4
+ * 0: /1 110: 6
+ * 1: /2 111: 8
+ *REF_CLK -> /PREDIVP[3] -> /PREDIV[2:0] -> /DIVP[9:0] -> /DIVDAC[3:0] -> DAC_CLK =
+ *(6~64M) 0x3611
+ * -> /DIVSP[3:0] -> /DIVS[2:0] -> SCLK
+ *
+ * -> /(1+DIVSRAM[3:0]) -> SRAM_CLK
+ */
+
+// PREDIVP
+#define OV13850_REG_PLL1_PREDIVP 0x030a
+#define OV13850_PREDIVP_1 0
+#define OV13850_PREDIVP_2 1
+
+// PREDIV
+#define OV13850_REG_PLL1_PREDIV 0x0300
+#define OV13850_PREDIV_1 0
+#define OV13850_PREDIV_1_5 1
+#define OV13850_PREDIV_2 2
+#define OV13850_PREDIV_2_5 3
+#define OV13850_PREDIV_3 4
+#define OV13850_PREDIV_4 5
+#define OV13850_PREDIV_6 6
+#define OV13850_PREDIV_8 7
+
+// DIVP
+#define OV13850_REG_PLL1_DIVP_H 0x0301
+#define OV13850_REG_PLL1_DIVP_L 0x0302
+#define OV13850_REG_PLL1_DIVP OV13850_REG_PLL1_DIVP_H
+
+// DIVM
+#define OV13850_REG_PLL1_DIVM 0x0303
+#define OV13850_DIVM(n) ((n)-1) // n=1~16
+
+// DIV_MIPI
+#define OV13850_REG_PLL1_DIV_MIPI 0x0304
+#define OV13850_DIV_MIPI_4 0
+#define OV13850_DIV_MIPI_5 1
+#define OV13850_DIV_MIPI_6 2
+#define OV13850_DIV_MIPI_8 3
+
+// system control
+#define OV13850_STREAM_CTRL 0x0100
+#define OV13850_REG_MIPI_SC 0x300f
+#define OV13850_MIPI_SC_8_BIT 0x0
+#define OV13850_MIPI_SC_10_BIT 0x1
+#define OV13850_MIPI_SC_12_BIT 0x2
+#define OV13850_GET_MIPI_SC_MIPI_BIT(v) ((v) & 0x3)
+#define OV13850_REG_MIPI_SC_CTRL0 0x3012
+#define OV13850_GET_MIPI_SC_CTRL0_LANE_NUM(v) ((v)>>4 & 0xf)
+
+// timing
+#define OV13850_REG_H_CROP_START_H 0x3800
+#define OV13850_REG_H_CROP_START_L 0x3801
+#define OV13850_REG_H_CROP_START OV13850_REG_H_CROP_START_H
+#define OV13850_REG_V_CROP_START_H 0x3802
+#define OV13850_REG_V_CROP_START_L 0x3803
+#define OV13850_REG_V_CROP_START OV13850_REG_V_CROP_START_H
+
+#define OV13850_REG_H_CROP_END_H 0x3804
+#define OV13850_REG_H_CROP_END_L 0x3805
+#define OV13850_REG_H_CROP_END OV13850_REG_H_CROP_END_H
+#define OV13850_REG_V_CROP_END_H 0x3806
+#define OV13850_REG_V_CROP_END_L 0x3807
+#define OV13850_REG_V_CROP_END OV13850_REG_V_CROP_END_H
+
+#define OV13850_REG_H_OUTPUT_SIZE_H 0x3808
+#define OV13850_REG_H_OUTPUT_SIZE_L 0x3809
+#define OV13850_REG_H_OUTPUT_SIZE OV13850_REG_H_OUTPUT_SIZE_H
+#define OV13850_REG_V_OUTPUT_SIZE_H 0x380a
+#define OV13850_REG_V_OUTPUT_SIZE_L 0x380b
+#define OV13850_REG_V_OUTPUT_SIZE OV13850_REG_V_OUTPUT_SIZE_H
+
+#define OV13850_REG_TIMING_HTS_H 0x380c
+#define OV13850_REG_TIMING_HTS_L 0x380d
+#define OV13850_REG_TIMING_HTS OV13850_REG_TIMING_HTS_H
+#define OV13850_REG_TIMING_VTS_H 0x380e
+#define OV13850_REG_TIMING_VTS_L 0x380f
+#define OV13850_REG_TIMING_VTS OV13850_REG_TIMING_VTS_H
+
+
+#define OV13850_REG_H_WIN_OFF_H 0x3810
+#define OV13850_REG_H_WIN_OFF_L 0x3811
+#define OV13850_REG_V_WIN_OFF_H 0x3812
+#define OV13850_REG_V_WIN_OFF_L 0x3813
+
+#define OV13850_REG_H_INC 0x3814
+#define OV13850_REG_V_INC 0x3815
+
+enum ov13850_mode_id {
+ OV13850_MODE_1080P_1920_1080 = 0,
+ OV13850_NUM_MODES,
+};
+
+enum ov13850_frame_rate {
+ OV13850_15_FPS = 0,
+ OV13850_30_FPS,
+ OV13850_60_FPS,
+ OV13850_NUM_FRAMERATES,
+};
+
+static const int ov13850_framerates[] = {
+ [OV13850_15_FPS] = 15,
+ [OV13850_30_FPS] = 30,
+ [OV13850_60_FPS] = 60,
+};
+
+struct ov13850_pixfmt {
+ u32 code;
+ u32 colorspace;
+};
+
+static const struct ov13850_pixfmt ov13850_formats[] = {
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_COLORSPACE_SRGB, },
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_COLORSPACE_SRGB, },
+};
+
+/* regulator supplies */
+static const char * const ov13850_supply_name[] = {
+ "DOVDD", /* Digital I/O (1.8V) supply */
+ "AVDD", /* Analog (2.8V) supply */
+ "DVDD", /* Digital Core (1.5V) supply */
+};
+
+#define OV13850_NUM_SUPPLIES ARRAY_SIZE(ov13850_supply_name)
+
+/*
+ * Image size under 1280 * 960 are SUBSAMPLING
+ * Image size upper 1280 * 960 are SCALING
+ */
+enum ov13850_downsize_mode {
+ SUBSAMPLING,
+ SCALING,
+};
+
+struct reg_value {
+ u16 reg_addr;
+ u8 val;
+ u8 mask;
+ u32 delay_ms;
+};
+
+struct ov13850_mode_info {
+ enum ov13850_mode_id id;
+ enum ov13850_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 ov13850_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 ov13850_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 OV13850 */
+ u32 xclk_freq;
+
+ struct regulator_bulk_data supplies[OV13850_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 ov13850_mode_info *current_mode;
+ const struct ov13850_mode_info *last_mode;
+ enum ov13850_frame_rate current_fr;
+ struct v4l2_fract frame_interval;
+
+ struct ov13850_ctrls ctrls;
+
+ u32 prev_sysclk, prev_hts;
+ u32 ae_low, ae_high, ae_target;
+
+ bool pending_mode_change;
+ bool streaming;
+};
+
+static inline struct ov13850_dev *to_ov13850_dev(struct v4l2_subdev *sd)
+{
+ return container_of(sd, struct ov13850_dev, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+ return &container_of(ctrl->handler, struct ov13850_dev,
+ ctrls.handler)->sd;
+}
+
+/* ov13850 initial register */
+static const struct reg_value ov13850_init_setting_30fps_1080P[] = {
+
+};
+
+static const struct reg_value ov13850_setting_1080P_1920_1080[] = {
+//;XVCLK=24Mhz, SCLK=4x120Mhz, MIPI 640Mbps, DACCLK=240Mhz
+/*
+ * using quarter size to scale down
+ */
+ {0x0103, 0x01, 0, 0}, // ; software reset
+
+ {0x0300, 0x01, 0, 0}, //; PLL
+ {0x0301, 0x00, 0, 0}, //; PLL1_DIVP_hi
+ {0x0302, 0x28, 0, 0}, //; PLL1_DIVP_lo
+ {0x0303, 0x00, 0, 0}, // ; PLL
+ {0x030a, 0x00, 0, 0}, // ; PLL
+ //{0xffff, 20, 0, 0},
+ {0x300f, 0x11, 0, 0}, // SFC modified, MIPI_SRC, [1:0] 00-8bit, 01-10bit, 10-12bit
+ {0x3010, 0x01, 0, 0}, // ; MIPI PHY
+ {0x3011, 0x76, 0, 0}, // ; MIPI PHY
+ {0x3012, 0x41, 0, 0}, // ; MIPI 4 lane
+ {0x3013, 0x12, 0, 0}, // ; MIPI control
+ {0x3014, 0x11, 0, 0}, // ; MIPI control
+ {0x301f, 0x03, 0, 0}, //
+ {0x3106, 0x00, 0, 0}, //
+ {0x3210, 0x47, 0, 0}, //
+ {0x3500, 0x00, 0, 0}, // ; exposure HH
+ {0x3501, 0x67, 0, 0}, // ; exposure H
+ {0x3502, 0x80, 0, 0}, // ; exposure L
+ {0x3506, 0x00, 0, 0}, // ; short exposure HH
+ {0x3507, 0x02, 0, 0}, // ; short exposure H
+ {0x3508, 0x00, 0, 0}, // ; shour exposure L
+ {0x3509, 0x10, 0, 0},//00},//8},
+ {0x350a, 0x00, 0, 0}, // ; gain H
+ {0x350b, 0x10, 0, 0}, // ; gain L
+ {0x350e, 0x00, 0, 0}, // ; short gain H
+ {0x350f, 0x10, 0, 0}, // ; short gain L
+ {0x3600, 0x40, 0, 0}, // ; analog control
+ {0x3601, 0xfc, 0, 0}, // ; analog control
+ {0x3602, 0x02, 0, 0}, // ; analog control
+ {0x3603, 0x48, 0, 0}, // ; analog control
+ {0x3604, 0xa5, 0, 0}, // ; analog control
+ {0x3605, 0x9f, 0, 0}, // ; analog control
+ {0x3607, 0x00, 0, 0}, // ; analog control
+ {0x360a, 0x40, 0, 0}, // ; analog control
+ {0x360b, 0x91, 0, 0}, // ; analog control
+ {0x360c, 0x49, 0, 0}, // ; analog control
+ {0x360f, 0x8a, 0, 0}, //
+ {0x3611, 0x10, 0, 0}, // ; PLL2
+ //{0x3612, 0x23, 0, 0}, // ; PLL2
+ {0x3612, 0x13, 0, 0}, // ; PLL2
+ //{0x3613, 0x33, 0, 0}, // ; PLL2
+ {0x3613, 0x22, 0, 0}, // ; PLL2
+ //{0xffff, 50, 0, 0},
+ {0x3614, 0x28, 0, 0}, //[7:0] PLL2_DIVP lo
+ {0x3615, 0x08, 0, 0}, //[7:6] Debug mode, [5:4] N_pump clock div, [3:2] P_pump clock div, [1:0] PLL2_DIVP hi
+ {0x3641, 0x02, 0, 0},
+ {0x3660, 0x82, 0, 0},
+ {0x3668, 0x54, 0, 0},
+ {0x3669, 0x40, 0, 0},
+ {0x3667, 0xa0, 0, 0},
+ {0x3702, 0x40, 0, 0},
+ {0x3703, 0x44, 0, 0},
+ {0x3704, 0x2c, 0, 0},
+ {0x3705, 0x24, 0, 0},
+ {0x3706, 0x50, 0, 0},
+ {0x3707, 0x44, 0, 0},
+ {0x3708, 0x3c, 0, 0},
+ {0x3709, 0x1f, 0, 0},
+ {0x370a, 0x26, 0, 0},
+ {0x370b, 0x3c, 0, 0},
+ {0x3720, 0x66, 0, 0},
+ {0x3722, 0x84, 0, 0},
+ {0x3728, 0x40, 0, 0},
+ {0x372a, 0x00, 0, 0},
+ {0x372f, 0x90, 0, 0},
+ {0x3710, 0x28, 0, 0},
+ {0x3716, 0x03, 0, 0},
+ {0x3718, 0x10, 0, 0},
+ {0x3719, 0x08, 0, 0},
+ {0x371c, 0xfc, 0, 0},
+ {0x3760, 0x13, 0, 0},
+ {0x3761, 0x34, 0, 0},
+ {0x3767, 0x24, 0, 0},
+ {0x3768, 0x06, 0, 0},
+ {0x3769, 0x45, 0, 0},
+ {0x376c, 0x23, 0, 0},
+ {0x3d84, 0x00, 0, 0}, // ; OTP program disable
+ {0x3d85, 0x17, 0, 0}, // ; OTP power up load data enable, power load setting enable, software load setting
+ {0x3d8c, 0x73, 0, 0}, // ; OTP start address H
+ {0x3d8d, 0xbf, 0, 0}, // ; OTP start address L
+ {0x3800, 0x00, 0, 0}, // ; H crop start H
+ {0x3801, 0x08, 0, 0}, // ; H crop start L
+ {0x3802, 0x00, 0, 0}, // ; V crop start H
+ {0x3803, 0x04, 0, 0}, // ; V crop start L
+ {0x3804, 0x10, 0, 0}, // ; H crop end H
+ {0x3805, 0x97, 0, 0}, // ; H crop end L
+ {0x3806, 0x0c, 0, 0}, // ; V crop end H
+ {0x3807, 0x4b, 0, 0}, // ; V crop end L
+ {0x3808, 0x08, 0, 0}, // ; H output size H
+ {0x3809, 0x40, 0, 0}, // ; H output size L
+ {0x380a, 0x06, 0, 0}, // ; V output size H
+ {0x380b, 0x20, 0, 0}, // ; V output size L
+ {0x380c, 0x25, 0, 0}, // ; HTS H
+ {0x380d, 0x80, 0, 0}, // ; HTS L
+ {0x380e, 0x06, 0, 0}, // ; VTS H
+ {0x380f, 0x80, 0, 0}, // ; VTS L
+ {0x3810, 0x00, 0, 0}, // ; H win off H
+ {0x3811, 0x04, 0, 0}, // ; H win off L
+ {0x3812, 0x00, 0, 0}, // ; V win off H
+ {0x3813, 0x02, 0, 0}, // ; V win off L
+ {0x3814, 0x31, 0, 0}, // ; H inc
+ {0x3815, 0x31, 0, 0}, // ; V inc
+ {0x3820, 0x02, 0, 0}, // ; V flip off, V bin on
+ {0x3821, 0x05, 0, 0}, // ; H mirror on, H bin on
+ {0x3834, 0x00, 0, 0}, //
+ {0x3835, 0x1c, 0, 0}, // ; cut_en, vts_auto, blk_col_dis
+ {0x3836, 0x08, 0, 0}, //
+ {0x3837, 0x02, 0, 0}, //
+ {0x4000, 0xf1, 0, 0},//c1}, // ; BLC offset trig en, format change trig en, gain trig en, exp trig en, median en
+ {0x4001, 0x00, 0, 0}, // ; BLC
+ {0x400b, 0x0c, 0, 0}, // ; BLC
+ {0x4011, 0x00, 0, 0}, // ; BLC
+ {0x401a, 0x00, 0, 0}, // ; BLC
+ {0x401b, 0x00, 0, 0}, // ; BLC
+ {0x401c, 0x00, 0, 0}, // ; BLC
+ {0x401d, 0x00, 0, 0}, // ; BLC
+ {0x4020, 0x00, 0, 0}, // ; BLC
+ {0x4021, 0xe4, 0, 0}, // ; BLC
+ {0x4022, 0x07, 0, 0}, // ; BLC
+ {0x4023, 0x5f, 0, 0}, // ; BLC
+ {0x4024, 0x08, 0, 0}, // ; BLC
+ {0x4025, 0x44, 0, 0}, // ; BLC
+ {0x4026, 0x08, 0, 0}, // ; BLC
+ {0x4027, 0x47, 0, 0}, // ; BLC
+ {0x4028, 0x00, 0, 0}, // ; BLC
+ {0x4029, 0x02, 0, 0}, // ; BLC
+ {0x402a, 0x04, 0, 0}, // ; BLC
+ {0x402b, 0x08, 0, 0}, // ; BLC
+ {0x402c, 0x02, 0, 0}, // ; BLC
+ {0x402d, 0x02, 0, 0}, // ; BLC
+ {0x402e, 0x0c, 0, 0}, // ; BLC
+ {0x402f, 0x08, 0, 0}, // ; BLC
+ {0x403d, 0x2c, 0, 0}, //
+ {0x403f, 0x7f, 0, 0}, //
+ {0x4500, 0x82, 0, 0}, // ; BLC
+ {0x4501, 0x38, 0, 0}, // ; BLC
+ {0x4601, 0x04, 0, 0}, //
+ {0x4602, 0x22, 0, 0}, //
+ {0x4603, 0x01, 0, 0}, //; VFIFO
+ {0x4837, 0x19, 0, 0}, //; MIPI global timing
+ {0x4d00, 0x04, 0, 0}, // ; temperature monitor
+ {0x4d01, 0x42, 0, 0}, // ; temperature monitor
+ {0x4d02, 0xd1, 0, 0}, // ; temperature monitor
+ {0x4d03, 0x90, 0, 0}, // ; temperature monitor
+ {0x4d04, 0x66, 0, 0}, // ; temperature monitor
+ {0x4d05, 0x65, 0, 0}, // ; temperature monitor
+ {0x5000, 0x0e, 0, 0}, // ; windowing enable, BPC on, WPC on, Lenc on
+ {0x5001, 0x03, 0, 0}, // ; BLC enable, MWB on
+ {0x5002, 0x07, 0, 0}, //
+ {0x5013, 0x40, 0, 0},
+ {0x501c, 0x00, 0, 0},
+ {0x501d, 0x10, 0, 0},
+ //{0x5057, 0x56, 0, 0},//add
+ {0x5056, 0x08, 0, 0},
+ {0x5058, 0x08, 0, 0},
+ {0x505a, 0x08, 0, 0},
+ {0x5242, 0x00, 0, 0},
+ {0x5243, 0xb8, 0, 0},
+ {0x5244, 0x00, 0, 0},
+ {0x5245, 0xf9, 0, 0},
+ {0x5246, 0x00, 0, 0},
+ {0x5247, 0xf6, 0, 0},
+ {0x5248, 0x00, 0, 0},
+ {0x5249, 0xa6, 0, 0},
+ {0x5300, 0xfc, 0, 0},
+ {0x5301, 0xdf, 0, 0},
+ {0x5302, 0x3f, 0, 0},
+ {0x5303, 0x08, 0, 0},
+ {0x5304, 0x0c, 0, 0},
+ {0x5305, 0x10, 0, 0},
+ {0x5306, 0x20, 0, 0},
+ {0x5307, 0x40, 0, 0},
+ {0x5308, 0x08, 0, 0},
+ {0x5309, 0x08, 0, 0},
+ {0x530a, 0x02, 0, 0},
+ {0x530b, 0x01, 0, 0},
+ {0x530c, 0x01, 0, 0},
+ {0x530d, 0x0c, 0, 0},
+ {0x530e, 0x02, 0, 0},
+ {0x530f, 0x01, 0, 0},
+ {0x5310, 0x01, 0, 0},
+ {0x5400, 0x00, 0, 0},
+ {0x5401, 0x61, 0, 0},
+ {0x5402, 0x00, 0, 0},
+ {0x5403, 0x00, 0, 0},
+ {0x5404, 0x00, 0, 0},
+ {0x5405, 0x40, 0, 0},
+ {0x540c, 0x05, 0, 0},
+ {0x5b00, 0x00, 0, 0},
+ {0x5b01, 0x00, 0, 0},
+ {0x5b02, 0x01, 0, 0},
+ {0x5b03, 0xff, 0, 0},
+ {0x5b04, 0x02, 0, 0},
+ {0x5b05, 0x6c, 0, 0},
+ {0x5b09, 0x02, 0, 0}, //
+ //{0x5e00, 0x00, 0, 0}, // ; test pattern disable
+ //{0x5e00, 0x80, 0, 0}, // ; test pattern enable
+ {0x5e10, 0x1c, 0, 0}, // ; ISP test disable
+
+ //{0x0300, 0x01, 0, 0},// ; PLL
+ //{0x0302, 0x28, 0, 0},// ; PLL
+ //{0xffff, 50, 0, 0},
+ {0x3501, 0x67, 0, 0},// ; Exposure H
+ {0x370a, 0x26, 0, 0},//
+ {0x372a, 0x00, 0, 0},
+ {0x372f, 0x90, 0, 0},
+ {0x3801, 0x08, 0, 0}, //; H crop start L
+ {0x3803, 0x04, 0, 0}, //; V crop start L
+ {0x3805, 0x97, 0, 0}, //; H crop end L
+ {0x3807, 0x4b, 0, 0}, //; V crop end L
+ {0x3808, 0x08, 0, 0}, //; H output size H
+ {0x3809, 0x40, 0, 0}, //; H output size L
+ {0x380a, 0x06, 0, 0}, //; V output size H
+ {0x380b, 0x20, 0, 0}, //; V output size L
+ {0x380c, 0x25, 0, 0}, //; HTS H
+ {0x380d, 0x80, 0, 0}, //; HTS L
+ {0x380e, 0x0a, 0, 0},//6}, //; VTS H
+ {0x380f, 0x80, 0, 0}, //; VTS L
+ {0x3813, 0x02, 0, 0}, //; V win off
+ {0x3814, 0x31, 0, 0}, //; H inc
+ {0x3815, 0x31, 0, 0}, //; V inc
+ {0x3820, 0x02, 0, 0}, //; V flip off, V bin on
+ {0x3821, 0x05, 0, 0}, //; H mirror on, H bin on
+ {0x3836, 0x08, 0, 0}, //
+ {0x3837, 0x02, 0, 0}, //
+ {0x4020, 0x00, 0, 0}, //
+ {0x4021, 0xe4, 0, 0}, //
+ {0x4022, 0x07, 0, 0}, //
+ {0x4023, 0x5f, 0, 0}, //
+ {0x4024, 0x08, 0, 0}, //
+ {0x4025, 0x44, 0, 0}, //
+ {0x4026, 0x08, 0, 0}, //
+ {0x4027, 0x47, 0, 0}, //
+ {0x4603, 0x01, 0, 0}, //; VFIFO
+ {0x4837, 0x19, 0, 0}, //; MIPI global timing
+ {0x4802, 0x42, 0, 0}, //default 0x00
+ {0x481a, 0x00, 0, 0},
+ {0x481b, 0x1c, 0, 0}, //default 0x3c prepare
+ {0x4826, 0x12, 0, 0}, //default 0x32 trail
+ {0x5401, 0x61, 0, 0}, //
+ {0x5405, 0x40, 0, 0}, //
+
+ //{0xffff, 200, 0, 0},
+ //{0xffff, 200, 0, 0},
+ //{0xffff, 200, 0, 0},
+
+ //{0x0100, 0x01, 0, 0}, //; wake up, streaming
+};
+
+/* power-on sensor init reg table */
+static const struct ov13850_mode_info ov13850_mode_init_data = {
+ OV13850_MODE_1080P_1920_1080, SCALING,
+ 1920, 0x6e0, 1080, 0x470,
+ ov13850_init_setting_30fps_1080P,
+ ARRAY_SIZE(ov13850_init_setting_30fps_1080P),
+ OV13850_30_FPS,
+};
+
+static const struct ov13850_mode_info
+ov13850_mode_data[OV13850_NUM_MODES] = {
+ {OV13850_MODE_1080P_1920_1080, SCALING,
+ 1920, 0x6e0, 1080, 0x470,
+ ov13850_setting_1080P_1920_1080,
+ ARRAY_SIZE(ov13850_setting_1080P_1920_1080),
+ OV13850_30_FPS},
+};
+
+static int ov13850_write_reg(struct ov13850_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 ov13850_read_reg(struct ov13850_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 ov13850_read_reg16(struct ov13850_dev *sensor, u16 reg, u16 *val)
+{
+ u8 hi, lo;
+ int ret;
+
+ ret = ov13850_read_reg(sensor, reg, &hi);
+ if (ret)
+ return ret;
+ ret = ov13850_read_reg(sensor, reg + 1, &lo);
+ if (ret)
+ return ret;
+
+ *val = ((u16)hi << 8) | (u16)lo;
+ return 0;
+}
+
+static int ov13850_write_reg16(struct ov13850_dev *sensor, u16 reg, u16 val)
+{
+ int ret;
+
+ ret = ov13850_write_reg(sensor, reg, val >> 8);
+ if (ret)
+ return ret;
+
+ return ov13850_write_reg(sensor, reg + 1, val & 0xff);
+}
+
+static int ov13850_mod_reg(struct ov13850_dev *sensor, u16 reg,
+ u8 mask, u8 val)
+{
+ u8 readval;
+ int ret;
+
+ ret = ov13850_read_reg(sensor, reg, &readval);
+ if (ret)
+ return ret;
+
+ readval &= ~mask;
+ val &= mask;
+ val |= readval;
+
+ return ov13850_write_reg(sensor, reg, val);
+}
+
+static int ov13850_set_timings(struct ov13850_dev *sensor,
+ const struct ov13850_mode_info *mode)
+{
+ int ret;
+
+ ret = ov13850_write_reg16(sensor, OV13850_REG_H_OUTPUT_SIZE, mode->hact);
+ if (ret < 0)
+ return ret;
+
+ ret = ov13850_write_reg16(sensor, OV13850_REG_V_OUTPUT_SIZE, mode->vact);
+ if (ret < 0)
+ return ret;
+
+ return 0;
+}
+
+static int ov13850_load_regs(struct ov13850_dev *sensor,
+ const struct ov13850_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 = ov13850_mod_reg(sensor, reg_addr, mask, val);
+ else
+ ret = ov13850_write_reg(sensor, reg_addr, val);
+ if (ret)
+ break;
+
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+
+ return ov13850_set_timings(sensor, mode);
+}
+
+
+
+static int ov13850_get_gain(struct ov13850_dev *sensor)
+{
+ u32 gain = 0;
+ return gain;
+}
+
+static int ov13850_set_gain(struct ov13850_dev *sensor, int gain)
+{
+ return 0;
+}
+
+static int ov13850_set_stream_mipi(struct ov13850_dev *sensor, bool on)
+{
+ return 0;
+}
+
+static int ov13850_get_sysclk(struct ov13850_dev *sensor)
+{
+ return 0;
+}
+
+static int ov13850_set_night_mode(struct ov13850_dev *sensor)
+{
+ return 0;
+}
+
+static int ov13850_get_hts(struct ov13850_dev *sensor)
+{
+ /* read HTS from register settings */
+ u16 hts;
+ int ret;
+
+ ret = ov13850_read_reg16(sensor, OV13850_REG_TIMING_HTS, &hts);
+ if (ret)
+ return ret;
+ return hts;
+}
+
+static int ov13850_set_hts(struct ov13850_dev *sensor, int hts)
+{
+ return ov13850_write_reg16(sensor, OV13850_REG_TIMING_HTS, hts);
+}
+
+
+static int ov13850_get_vts(struct ov13850_dev *sensor)
+{
+ u16 vts;
+ int ret;
+
+ ret = ov13850_read_reg16(sensor, OV13850_REG_TIMING_VTS, &vts);
+ if (ret)
+ return ret;
+ return vts;
+}
+
+static int ov13850_set_vts(struct ov13850_dev *sensor, int vts)
+{
+ return ov13850_write_reg16(sensor, OV13850_REG_TIMING_VTS, vts);
+}
+
+static int ov13850_get_light_freq(struct ov13850_dev *sensor)
+{
+ return 0;
+}
+
+static int ov13850_set_bandingfilter(struct ov13850_dev *sensor)
+{
+ return 0;
+}
+
+static int ov13850_set_ae_target(struct ov13850_dev *sensor, int target)
+{
+ return 0;
+}
+
+static int ov13850_get_binning(struct ov13850_dev *sensor)
+{
+ return 0;
+}
+
+static int ov13850_set_binning(struct ov13850_dev *sensor, bool enable)
+{
+ return 0;
+}
+
+static const struct ov13850_mode_info *
+ov13850_find_mode(struct ov13850_dev *sensor, enum ov13850_frame_rate fr,
+ int width, int height, bool nearest)
+{
+ const struct ov13850_mode_info *mode;
+
+ mode = v4l2_find_nearest_size(ov13850_mode_data,
+ ARRAY_SIZE(ov13850_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 (ov13850_framerates[fr] > ov13850_framerates[mode->max_fps])
+ return NULL;
+
+ return mode;
+}
+
+static u64 ov13850_calc_pixel_rate(struct ov13850_dev *sensor)
+{
+ u64 rate;
+
+ rate = sensor->current_mode->vact * sensor->current_mode->hact;
+ rate *= ov13850_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
+ **
+ */
+
+/*
+ * ov13850_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.
+ */
+
+
+
+static int ov13850_set_mipi_pclk(struct ov13850_dev *sensor,
+ unsigned long rate)
+{
+
+ return 0;
+}
+
+/*
+ * if sensor changes inside scaling or subsampling
+ * change mode directly
+ */
+static int ov13850_set_mode_direct(struct ov13850_dev *sensor,
+ const struct ov13850_mode_info *mode)
+{
+ if (!mode->reg_data)
+ return -EINVAL;
+
+ /* Write capture setting */
+ return ov13850_load_regs(sensor, mode);
+}
+
+static int ov13850_set_mode(struct ov13850_dev *sensor)
+{
+ const struct ov13850_mode_info *mode = sensor->current_mode;
+ const struct ov13850_mode_info *orig_mode = sensor->last_mode;
+ int ret = 0;
+
+ ret = ov13850_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 = ov13850_set_mipi_pclk(sensor, 0);
+
+ if (ret < 0)
+ return 0;
+
+ sensor->pending_mode_change = false;
+ sensor->last_mode = mode;
+ return 0;
+}
+
+static int ov13850_set_framefmt(struct ov13850_dev *sensor,
+ struct v4l2_mbus_framefmt *format);
+
+/* restore the last set video mode after chip power-on */
+static int ov13850_restore_mode(struct ov13850_dev *sensor)
+{
+ int ret;
+
+ /* first load the initial register values */
+ ret = ov13850_load_regs(sensor, &ov13850_mode_init_data);
+ if (ret < 0)
+ return ret;
+ sensor->last_mode = &ov13850_mode_init_data;
+
+ /* now restore the last capture mode */
+ ret = ov13850_set_mode(sensor);
+ if (ret < 0)
+ return ret;
+
+ return ov13850_set_framefmt(sensor, &sensor->fmt);
+}
+
+static void ov13850_power(struct ov13850_dev *sensor, bool enable)
+{
+ if (!sensor->pwdn_gpio)
+ return;
+ if (enable) {
+ gpiod_set_value_cansleep(sensor->pwdn_gpio, 0);
+ gpiod_set_value_cansleep(sensor->pwdn_gpio, 1);
+ } else {
+ gpiod_set_value_cansleep(sensor->pwdn_gpio, 0);
+ }
+
+ mdelay(100);
+}
+
+static void ov13850_reset(struct ov13850_dev *sensor)
+{
+ if (!sensor->reset_gpio)
+ return;
+
+ gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+ gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+ mdelay(100);
+}
+
+static int ov13850_set_power_on(struct ov13850_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(OV13850_NUM_SUPPLIES,
+ sensor->supplies);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to enable regulators\n",
+ __func__);
+ goto xclk_off;
+ }
+
+ ov13850_reset(sensor);
+ ov13850_power(sensor, true);
+
+ return 0;
+
+xclk_off:
+ clk_disable_unprepare(sensor->xclk);
+ return ret;
+}
+
+static void ov13850_set_power_off(struct ov13850_dev *sensor)
+{
+ ov13850_power(sensor, false);
+ regulator_bulk_disable(OV13850_NUM_SUPPLIES, sensor->supplies);
+ clk_disable_unprepare(sensor->xclk);
+}
+
+static int ov13850_set_power_mipi(struct ov13850_dev *sensor, bool on)
+{
+ return 0;
+}
+
+static int ov13850_set_power(struct ov13850_dev *sensor, bool on)
+{
+ int ret = 0;
+ u16 chip_id;
+
+ if (on) {
+ ret = ov13850_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+#ifdef UNUSED_CODE
+ ret = ov13850_read_reg16(sensor, OV13850_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 != OV13850_CHIP_ID) {
+ dev_err(&sensor->i2c_client->dev,
+ "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+ __func__, OV13850_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);
+#endif
+
+ ret = ov13850_restore_mode(sensor);
+ if (ret)
+ goto power_off;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov13850_set_power_mipi(sensor, on);
+ if (ret)
+ goto power_off;
+
+ if (!on)
+ ov13850_set_power_off(sensor);
+
+ return 0;
+
+power_off:
+ ov13850_set_power_off(sensor);
+ return ret;
+}
+
+static int ov13850_s_power(struct v4l2_subdev *sd, int on)
+{
+ struct ov13850_dev *sensor = to_ov13850_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 = ov13850_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 ov13850_try_frame_interval(struct ov13850_dev *sensor,
+ struct v4l2_fract *fi,
+ u32 width, u32 height)
+{
+ const struct ov13850_mode_info *mode;
+ enum ov13850_frame_rate rate = OV13850_15_FPS;
+ int minfps, maxfps, best_fps, fps;
+ int i;
+
+ minfps = ov13850_framerates[OV13850_15_FPS];
+ maxfps = ov13850_framerates[OV13850_NUM_FRAMERATES - 1];
+
+ if (fi->numerator == 0) {
+ fi->denominator = maxfps;
+ fi->numerator = 1;
+ rate = OV13850_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(ov13850_framerates); i++) {
+ int curr_fps = ov13850_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 = ov13850_find_mode(sensor, rate, width, height, false);
+ return mode ? rate : -EINVAL;
+}
+
+static int ov13850_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(ov13850_formats))
+ return -EINVAL;
+
+ code->code = ov13850_formats[code->index].code;
+ return 0;
+}
+
+static int ov13850_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct ov13850_dev *sensor = to_ov13850_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 ov13850_try_fmt_internal(struct v4l2_subdev *sd,
+ struct v4l2_mbus_framefmt *fmt,
+ enum ov13850_frame_rate fr,
+ const struct ov13850_mode_info **new_mode)
+{
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+ const struct ov13850_mode_info *mode;
+ int i;
+
+ mode = ov13850_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(ov13850_formats); i++)
+ if (ov13850_formats[i].code == fmt->code)
+ break;
+ if (i >= ARRAY_SIZE(ov13850_formats))
+ i = 0;
+
+ fmt->code = ov13850_formats[i].code;
+ fmt->colorspace = ov13850_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 ov13850_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+ const struct ov13850_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 = ov13850_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 =
+ ov13850_framerates[sensor->current_fr];
+ sensor->current_mode = new_mode;
+ sensor->pending_mode_change = true;
+ }
+
+ __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
+ ov13850_calc_pixel_rate(sensor));
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int ov13850_set_framefmt(struct ov13850_dev *sensor,
+ struct v4l2_mbus_framefmt *format)
+{
+ u8 fmt;
+
+ switch (format->code) {
+ /* Raw, BGBG... / GRGR... */
+ case MEDIA_BUS_FMT_SBGGR8_1X8:
+ case MEDIA_BUS_FMT_SGBRG8_1X8:
+ case MEDIA_BUS_FMT_SGRBG8_1X8:
+ case MEDIA_BUS_FMT_SRGGB8_1X8:
+ fmt = 0x0;
+ break;
+ case MEDIA_BUS_FMT_SBGGR10_1X10:
+ case MEDIA_BUS_FMT_SGBRG10_1X10:
+ case MEDIA_BUS_FMT_SGRBG10_1X10:
+ case MEDIA_BUS_FMT_SRGGB10_1X10:
+ fmt = 0x1;
+ case MEDIA_BUS_FMT_SBGGR12_1X12:
+ case MEDIA_BUS_FMT_SGBRG12_1X12:
+ case MEDIA_BUS_FMT_SGRBG12_1X12:
+ case MEDIA_BUS_FMT_SRGGB12_1X12:
+ fmt = 0x2;
+ default:
+ return -EINVAL;
+ }
+
+ return ov13850_mod_reg(sensor, OV13850_REG_MIPI_SC,
+ BIT(1) | BIT(0), fmt);
+}
+
+/*
+ * Sensor Controls.
+ */
+
+static int ov13850_set_ctrl_hue(struct ov13850_dev *sensor, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov13850_set_ctrl_contrast(struct ov13850_dev *sensor, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov13850_set_ctrl_saturation(struct ov13850_dev *sensor, int value)
+{
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov13850_set_ctrl_white_balance(struct ov13850_dev *sensor, int awb)
+{
+ struct ov13850_ctrls *ctrls = &sensor->ctrls;
+ int ret = 0;
+
+ return ret;
+}
+
+static int ov13850_set_ctrl_exposure(struct ov13850_dev *sensor,
+ enum v4l2_exposure_auto_type auto_exposure)
+{
+ struct ov13850_ctrls *ctrls = &sensor->ctrls;
+ int ret = 0;
+
+ return ret;
+}
+
+static const s64 link_freq_menu_items[] = {
+ OV13850_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",
+};
+
+static int ov13850_set_ctrl_test_pattern(struct ov13850_dev *sensor, int value)
+{
+ return 0;
+}
+
+static int ov13850_set_ctrl_light_freq(struct ov13850_dev *sensor, int value)
+{
+ return 0;
+}
+
+static int ov13850_set_ctrl_hflip(struct ov13850_dev *sensor, int value)
+{
+ return 0;
+}
+
+static int ov13850_set_ctrl_vflip(struct ov13850_dev *sensor, int value)
+{
+ return 0;
+}
+
+static int ov13850_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+ int val;
+
+ /* v4l2_ctrl_lock() locks our own mutex */
+
+ switch (ctrl->id) {
+ case V4L2_CID_ANALOGUE_GAIN:
+ val = ov13850_get_gain(sensor);
+ break;
+ }
+
+ return 0;
+}
+
+static int ov13850_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+ struct ov13850_dev *sensor = to_ov13850_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 = ov13850_set_gain(sensor, ctrl->val);
+ break;
+ case V4L2_CID_EXPOSURE:
+ ret = ov13850_set_ctrl_exposure(sensor, V4L2_EXPOSURE_MANUAL);
+ break;
+ case V4L2_CID_AUTO_WHITE_BALANCE:
+ ret = ov13850_set_ctrl_white_balance(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ ret = ov13850_set_ctrl_hue(sensor, ctrl->val);
+ break;
+ case V4L2_CID_CONTRAST:
+ ret = ov13850_set_ctrl_contrast(sensor, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ ret = ov13850_set_ctrl_saturation(sensor, ctrl->val);
+ break;
+ case V4L2_CID_TEST_PATTERN:
+ ret = ov13850_set_ctrl_test_pattern(sensor, ctrl->val);
+ break;
+ case V4L2_CID_POWER_LINE_FREQUENCY:
+ ret = ov13850_set_ctrl_light_freq(sensor, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ ret = ov13850_set_ctrl_hflip(sensor, ctrl->val);
+ break;
+ case V4L2_CID_VFLIP:
+ ret = ov13850_set_ctrl_vflip(sensor, ctrl->val);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+ return ret;
+}
+
+static const struct v4l2_ctrl_ops ov13850_ctrl_ops = {
+ .g_volatile_ctrl = ov13850_g_volatile_ctrl,
+ .s_ctrl = ov13850_s_ctrl,
+};
+
+static int ov13850_init_controls(struct ov13850_dev *sensor)
+{
+ const struct v4l2_ctrl_ops *ops = &ov13850_ctrl_ops;
+ struct ov13850_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,
+ ov13850_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 ov13850_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 >= OV13850_NUM_MODES)
+ return -EINVAL;
+
+ fse->min_width =
+ ov13850_mode_data[fse->index].hact;
+ fse->max_width = fse->min_width;
+ fse->min_height =
+ ov13850_mode_data[fse->index].vact;
+ fse->max_height = fse->min_height;
+
+ return 0;
+}
+
+static int ov13850_enum_frame_interval(
+ struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_interval_enum *fie)
+{
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+ struct v4l2_fract tpf;
+ int ret;
+
+ if (fie->pad != 0)
+ return -EINVAL;
+ if (fie->index >= OV13850_NUM_FRAMERATES)
+ return -EINVAL;
+
+ tpf.numerator = 1;
+ tpf.denominator = ov13850_framerates[fie->index];
+
+/* ret = ov13850_try_frame_interval(sensor, &tpf,
+ * fie->width, fie->height);
+ * if (ret < 0)
+ * return -EINVAL;
+ */
+ fie->interval = tpf;
+
+ return 0;
+}
+
+static int ov13850_g_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+
+ mutex_lock(&sensor->lock);
+ fi->interval = sensor->frame_interval;
+ mutex_unlock(&sensor->lock);
+
+ return 0;
+}
+
+static int ov13850_s_frame_interval(struct v4l2_subdev *sd,
+ struct v4l2_subdev_frame_interval *fi)
+{
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+ const struct ov13850_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 = ov13850_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 = ov13850_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,
+ ov13850_calc_pixel_rate(sensor));
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static int ov13850_stream_start(struct ov13850_dev *sensor, int enable)
+{
+ int ret;
+
+ if (enable) { //stream on
+ mdelay(1000);
+ ret = ov13850_write_reg(sensor, OV13850_STREAM_CTRL, enable);
+ } else { //stream off
+ ret = ov13850_write_reg(sensor, OV13850_STREAM_CTRL, enable);
+ mdelay(100);
+ }
+
+ return ret;
+}
+
+static int ov13850_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct ov13850_dev *sensor = to_ov13850_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming == !enable) {
+ if (enable && sensor->pending_mode_change) {
+ ret = ov13850_set_mode(sensor);
+ if (ret)
+ goto out;
+ }
+
+ if (enable && sensor->pending_fmt_change) {
+ ret = ov13850_set_framefmt(sensor, &sensor->fmt);
+ if (ret)
+ goto out;
+ sensor->pending_fmt_change = false;
+ }
+
+ if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
+ ret = ov13850_set_stream_mipi(sensor, enable);
+
+ ret = ov13850_stream_start(sensor, enable);
+
+ if (!ret)
+ sensor->streaming = enable;
+ }
+out:
+ mutex_unlock(&sensor->lock);
+ return ret;
+}
+
+static const struct v4l2_subdev_core_ops ov13850_core_ops = {
+ .s_power = ov13850_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 ov13850_video_ops = {
+ .g_frame_interval = ov13850_g_frame_interval,
+ .s_frame_interval = ov13850_s_frame_interval,
+ .s_stream = ov13850_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops ov13850_pad_ops = {
+ .enum_mbus_code = ov13850_enum_mbus_code,
+ .get_fmt = ov13850_get_fmt,
+ .set_fmt = ov13850_set_fmt,
+ .enum_frame_size = ov13850_enum_frame_size,
+ .enum_frame_interval = ov13850_enum_frame_interval,
+};
+
+static const struct v4l2_subdev_ops ov13850_subdev_ops = {
+ .core = &ov13850_core_ops,
+ .video = &ov13850_video_ops,
+ .pad = &ov13850_pad_ops,
+};
+
+static int ov13850_get_regulators(struct ov13850_dev *sensor)
+{
+ int i;
+
+ for (i = 0; i < OV13850_NUM_SUPPLIES; i++)
+ sensor->supplies[i].supply = ov13850_supply_name[i];
+
+ return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+ OV13850_NUM_SUPPLIES,
+ sensor->supplies);
+}
+
+static int ov13850_check_chip_id(struct ov13850_dev *sensor)
+{
+ struct i2c_client *client = sensor->i2c_client;
+ int ret = 0;
+ u16 chip_id;
+
+ ret = ov13850_set_power_on(sensor);
+ if (ret)
+ return ret;
+
+#ifdef UNUSED_CODE
+ ret = ov13850_read_reg16(sensor, OV13850_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 != OV13850_CHIP_ID) {
+ dev_err(&client->dev, "%s: wrong chip identifier, expected 0x%x, got 0x%x\n",
+ __func__, OV13850_CHIP_ID, chip_id);
+ ret = -ENXIO;
+ }
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+#endif
+
+power_off:
+ ov13850_set_power_off(sensor);
+ return ret;
+}
+
+static int ov13850_probe(struct i2c_client *client)
+{
+ struct device *dev = &client->dev;
+ struct fwnode_handle *endpoint;
+ struct ov13850_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 = ov13850_framerates[OV13850_30_FPS];
+ sensor->current_fr = OV13850_30_FPS;
+ sensor->current_mode =
+ &ov13850_mode_data[OV13850_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 < OV13850_XCLK_MIN ||
+ sensor->xclk_freq > OV13850_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, &ov13850_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 = ov13850_get_regulators(sensor);
+ if (ret)
+ return ret;
+
+ mutex_init(&sensor->lock);
+
+ ret = ov13850_check_chip_id(sensor);
+ if (ret)
+ goto entity_cleanup;
+
+ ret = ov13850_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 ov13850_remove(struct i2c_client *client)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct ov13850_dev *sensor = to_ov13850_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 ov13850_id[] = {
+ {"ov13850", 0},
+ {},
+};
+MODULE_DEVICE_TABLE(i2c, ov13850_id);
+
+static const struct of_device_id ov13850_dt_ids[] = {
+ { .compatible = "ovti,ov13850" },
+ { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov13850_dt_ids);
+
+static struct i2c_driver ov13850_i2c_driver = {
+ .driver = {
+ .name = "ov13850",
+ .of_match_table = ov13850_dt_ids,
+ },
+ .id_table = ov13850_id,
+ .probe_new = ov13850_probe,
+ .remove = ov13850_remove,
+};
+
+module_i2c_driver(ov13850_i2c_driver);
+
+MODULE_DESCRIPTION("OV13850 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/ov4689_mipi.c
@@ -0,0 +1,2975 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2021-2023 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/pm_runtime.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 4
+
+#define OV4689_LINK_FREQ_500MHZ 500000000LL
+
+/* min/typical/max system clock (xclk) frequencies */
+#define OV4689_XCLK_MIN 6000000
+#define OV4689_XCLK_MAX 64000000
+
+#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
+#define OV4689_REG_STREAM_ON 0x0100
+
+#define OV4689_REG_MIPI_SC_CTRL_HI 0x3018
+#define OV4689_REG_MIPI_SC_CTRL_LOW 0x3019
+
+enum ov4689_mode_id {
+ //OV4689_MODE_720P_1280_720 = 0,
+ OV4689_MODE_1080P_1920_1080 = 0,
+ //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;
+
+ 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;
+
+ bool pending_mode_change;
+ int 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[] = {
+/* ov4689_1080p_30fps_4d */
+ {0x0103, 0x01, 0, 0},
+ {0x3638, 0x00, 0, 0},
+ {0x0300, 0x02, 0, 0},
+ {0x0302, 0x32, 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},
+ {0x3020, 0x93, 0, 0},
+ {0x3021, 0x03, 0, 0},
+ {0x3022, 0x01, 0, 0},
+ {0x3031, 0x0a, 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},
+ {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},
+ {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, 0x0d, 0, 0},
+ {0x380d, 0x70, 0, 0},
+ {0x380e, 0x04, 0, 0},
+ {0x380f, 0x8A, 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_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},
+ {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},
+ {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},
+#ifndef UNUSED_CODE
+ {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},
+ {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},
+ {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 = {
+
+};
+
+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},
+
+ {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 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)
+{
+ 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);
+}
+
+#ifdef UNUSED_CODE
+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;
+}
+#endif
+
+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)
+{
+ 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;
+}
+
+#ifdef UNUSED_CODE
+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;
+}
+#endif
+
+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;
+}
+
+#ifdef UNUSED_CODE
+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;
+}
+#endif
+
+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]
+
+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 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_REG_TIMING_HTS, htot);
+
+ ret = ov4689_read_reg(sensor, OV4689_REG_TIMING_HTS, &val);
+ val16 = val << 8;
+ ret = ov4689_read_reg(sensor, OV4689_REG_TIMING_HTS + 1, &val);
+ val16 |= val;
+
+ st_info(ST_SENSOR, "fps = %d, max_fps = %d\n", fps, mode->max_fps);
+ st_info(ST_SENSOR, "mode->htot = 0x%x, htot = 0x%x\n", mode->htot,
+ htot);
+ st_info(ST_SENSOR, "reg: 0x%x = 0x%x\n", OV4689_REG_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;
+
+ int ret = 0;
+
+ ret = ov4689_set_mode_direct(sensor, mode);
+ if (ret < 0)
+ return ret;
+
+ 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 device *dev)
+{
+ struct i2c_client *client = to_i2c_client(dev);
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ 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 int ov4689_set_power_off(struct device *dev)
+{
+ struct i2c_client *client = to_i2c_client(dev);
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+
+ ov4689_power(sensor, false);
+ regulator_bulk_disable(OV4689_NUM_SUPPLIES, sensor->supplies);
+ clk_disable_unprepare(sensor->xclk);
+
+ return 0;
+}
+
+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_state *state,
+ 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_state *state,
+ 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, state,
+ 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;
+
+ 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_state *state,
+ 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, state, 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 */
+
+ if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+ return 0;
+
+ switch (ctrl->id) {
+ case V4L2_CID_ANALOGUE_GAIN:
+ val = ov4689_get_gain(sensor);
+ break;
+ }
+
+ pm_runtime_put(&sensor->i2c_client->dev);
+
+ 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 at start streaming time.
+ */
+ if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+ 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;
+ }
+
+ pm_runtime_put(&sensor->i2c_client->dev);
+
+ 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_state *state,
+ 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_state *state,
+ struct v4l2_subdev_frame_interval_enum *fie)
+{
+ //struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ struct v4l2_fract tpf;
+ int i = 0;
+
+ 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;
+
+ pr_debug("fie->width = %d, fie->height = %d\n", fie->width, fie->height);
+ for (i = 0; i < OV4689_NUM_MODES; i++) {
+ if (fie->width == ov4689_mode_data[i].hact &&
+ fie->height == ov4689_mode_data[i].vact)
+ break;
+ }
+ if (i == OV4689_NUM_MODES)
+ return -ENOTTY;
+
+ fie->interval = tpf;
+ fie->width = ov4689_mode_data[i].hact;
+ fie->height = ov4689_mode_data[i].vact;
+
+ 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)
+{
+ return ov4689_write_reg(sensor, OV4689_REG_STREAM_ON, enable);
+}
+
+static int ov4689_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct ov4689_dev *sensor = to_ov4689_dev(sd);
+ int ret = 0;
+
+ if (enable) {
+ pm_runtime_get_sync(&sensor->i2c_client->dev);
+
+ ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+ if (ret) {
+ pm_runtime_put_sync(&sensor->i2c_client->dev);
+ return ret;
+ }
+ }
+
+ mutex_lock(&sensor->lock);
+
+ if (sensor->streaming == !enable) {
+ if (enable) {
+ ret = ov4689_restore_mode(sensor);
+ if (ret)
+ goto out;
+ }
+
+ if (enable && sensor->pending_mode_change) {
+ ret = ov4689_set_mode(sensor);
+ if (ret)
+ goto out;
+ }
+
+ if (sensor->ep.bus.mipi_csi2.num_data_lanes == 2) {
+ ov4689_write_reg(sensor, OV4689_REG_MIPI_SC_CTRL_HI, 0x32);
+ ov4689_write_reg(sensor, OV4689_REG_MIPI_SC_CTRL_LOW, 0x0c);
+ } else if (sensor->ep.bus.mipi_csi2.num_data_lanes == 4) {
+ ov4689_write_reg(sensor, OV4689_REG_MIPI_SC_CTRL_HI, 0x72);
+ ov4689_write_reg(sensor, OV4689_REG_MIPI_SC_CTRL_LOW, 0x00);
+ } else {
+ dev_err(&sensor->i2c_client->dev, "Unsupport lane num\n");
+ }
+
+ ret = ov4689_stream_start(sensor, enable);
+ if (ret)
+ goto out;
+ }
+ sensor->streaming += enable ? 1 : -1;
+ WARN_ON(sensor->streaming < 0);
+out:
+ mutex_unlock(&sensor->lock);
+
+ if (!enable || ret)
+ pm_runtime_put_sync(&sensor->i2c_client->dev);
+
+ return ret;
+}
+
+static const struct v4l2_subdev_core_ops ov4689_core_ops = {
+ .log_status = v4l2_ctrl_subdev_log_status,
+ .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+ .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops 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_read_reg16(sensor, OV4689_REG_CHIP_ID, &chip_id);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to read chip identifier\n",
+ __func__);
+ return ret;
+ }
+
+ 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);
+ return -ENXIO;
+ }
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+
+ return 0;
+}
+
+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;
+
+ 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;
+
+
+ /* 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_CSI2_DPHY) {
+ 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_set_power_on(dev);
+ if (ret) {
+ dev_err(dev, "failed to power on\n");
+ goto entity_cleanup;
+ }
+
+ ret = ov4689_check_chip_id(sensor);
+ if (ret)
+ goto error_power_off;
+
+ ret = ov4689_init_controls(sensor);
+ if (ret)
+ goto error_power_off;
+
+ ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+ if (ret)
+ goto free_ctrls;
+
+ pm_runtime_set_active(dev);
+ pm_runtime_enable(dev);
+
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+error_power_off:
+ ov4689_set_power_off(dev);
+entity_cleanup:
+ media_entity_cleanup(&sensor->sd.entity);
+ mutex_destroy(&sensor->lock);
+ return ret;
+}
+
+static void 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);
+
+ pm_runtime_disable(&client->dev);
+ if (!pm_runtime_status_suspended(&client->dev))
+ ov4689_set_power_off(&client->dev);
+ pm_runtime_set_suspended(&client->dev);
+}
+
+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 const struct dev_pm_ops ov4689_pm_ops = {
+ SET_RUNTIME_PM_OPS(ov4689_set_power_off, ov4689_set_power_on, NULL)
+};
+
+static struct i2c_driver ov4689_i2c_driver = {
+ .driver = {
+ .name = "ov4689",
+ .of_match_table = ov4689_dt_ids,
+ .pm = &ov4689_pm_ops,
+ },
+ .id_table = ov4689_id,
+ .probe = ov4689_probe,
+ .remove = ov4689_remove,
+};
+
+module_i2c_driver(ov4689_i2c_driver);
+
+MODULE_DESCRIPTION("OV4689 MIPI Camera Subdev Driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/ov5640.c
@@ -0,0 +1,3227 @@
+// 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.
+ *
+ */
+
+#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;
+ int 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_state *state,
+ 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, state,
+ 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_state *state,
+ 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, 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 =
+ 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_state *state,
+ 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_state *state,
+ 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_state *state,
+ 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)
+ goto out;
+ }
+ sensor->streaming += enable ? 1 : -1;
+ WARN_ON(sensor->streaming < 0);
+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(&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
+++ b/drivers/media/platform/starfive/v4l2_driver/sc2235.c
@@ -0,0 +1,1914 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2021-2023 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/pm_runtime.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 27000000
+
+#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
+#define SC2235_REG_STREAM_ON 0x0100
+
+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_NUM_FRAMERATES,
+};
+
+struct sc2235_pixfmt {
+ u32 code;
+ u32 colorspace;
+};
+
+static const struct sc2235_pixfmt sc2235_formats[] = {
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB, },
+};
+
+static const int sc2235_framerates[] = {
+ [SC2235_15_FPS] = 15,
+ [SC2235_30_FPS] = 30,
+};
+
+/* 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;
+
+ 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;
+
+ bool pending_mode_change;
+ int 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 30fps*/
+static struct reg_value sc2235_init_regs_tbl_1080[] = {
+ {0x0103, 0x01, 0, 50},
+ {0x0100, 0x00, 0, 0},
+ {0x3039, 0x80, 0, 0},
+ {0x3621, 0x28, 0, 0},
+
+ {0x3309, 0x60, 0, 0},
+ {0x331f, 0x4d, 0, 0},
+ {0x3321, 0x4f, 0, 0},
+ {0x33b5, 0x10, 0, 0},
+
+ {0x3303, 0x20, 0, 0},
+ {0x331e, 0x0d, 0, 0},
+ {0x3320, 0x0f, 0, 0},
+
+ {0x3622, 0x02, 0, 0},
+ {0x3633, 0x42, 0, 0},
+ {0x3634, 0x42, 0, 0},
+
+ {0x3306, 0x66, 0, 0},
+ {0x330b, 0xd1, 0, 0},
+
+ {0x3301, 0x0e, 0, 0},
+
+ {0x320c, 0x08, 0, 0},
+ {0x320d, 0x98, 0, 0},
+
+ {0x3364, 0x05, 0, 0}, // [2] 1: write at sampling ending
+
+ {0x363c, 0x28, 0, 0}, //bypass nvdd
+ {0x363b, 0x0a, 0, 0}, //HVDD
+ {0x3635, 0xa0, 0, 0}, //TXVDD
+
+ {0x4500, 0x59, 0, 0},
+ {0x3d08, 0x00, 0, 0},
+ {0x3908, 0x11, 0, 0},
+
+ {0x363c, 0x08, 0, 0},
+
+ {0x3e03, 0x03, 0, 0},
+ {0x3e01, 0x46, 0, 0},
+
+ //0703
+ {0x3381, 0x0a, 0, 0},
+ {0x3348, 0x09, 0, 0},
+ {0x3349, 0x50, 0, 0},
+ {0x334a, 0x02, 0, 0},
+ {0x334b, 0x60, 0, 0},
+
+ {0x3380, 0x04, 0, 0},
+ {0x3340, 0x06, 0, 0},
+ {0x3341, 0x50, 0, 0},
+ {0x3342, 0x02, 0, 0},
+ {0x3343, 0x60, 0, 0},
+
+ //0707
+
+ {0x3632, 0x88, 0, 0}, //anti sm
+ {0x3309, 0xa0, 0, 0},
+ {0x331f, 0x8d, 0, 0},
+ {0x3321, 0x8f, 0, 0},
+
+ {0x335e, 0x01, 0, 0}, //ana dithering
+ {0x335f, 0x03, 0, 0},
+ {0x337c, 0x04, 0, 0},
+ {0x337d, 0x06, 0, 0},
+ {0x33a0, 0x05, 0, 0},
+ {0x3301, 0x05, 0, 0},
+
+ {0x337f, 0x03, 0, 0},
+ {0x3368, 0x02, 0, 0},
+ {0x3369, 0x00, 0, 0},
+ {0x336a, 0x00, 0, 0},
+ {0x336b, 0x00, 0, 0},
+ {0x3367, 0x08, 0, 0},
+ {0x330e, 0x30, 0, 0},
+
+ {0x3366, 0x7c, 0, 0}, // div_rst gap
+
+ {0x3635, 0xc1, 0, 0},
+ {0x363b, 0x09, 0, 0},
+ {0x363c, 0x07, 0, 0},
+
+ {0x391e, 0x00, 0, 0},
+
+ {0x3637, 0x14, 0, 0}, //fullwell 7K
+
+ {0x3306, 0x54, 0, 0},
+ {0x330b, 0xd8, 0, 0},
+ {0x366e, 0x08, 0, 0}, // ofs auto en [3]
+ {0x366f, 0x2f, 0, 0},
+
+ {0x3631, 0x84, 0, 0},
+ {0x3630, 0x48, 0, 0},
+ {0x3622, 0x06, 0, 0},
+
+ //ramp by sc
+ {0x3638, 0x1f, 0, 0},
+ {0x3625, 0x02, 0, 0},
+ {0x3636, 0x24, 0, 0},
+
+ //0714
+ {0x3348, 0x08, 0, 0},
+ {0x3e03, 0x0b, 0, 0},
+
+ //7.17 fpn
+ {0x3342, 0x03, 0, 0},
+ {0x3343, 0xa0, 0, 0},
+ {0x334a, 0x03, 0, 0},
+ {0x334b, 0xa0, 0, 0},
+
+ //0718
+ {0x3343, 0xb0, 0, 0},
+ {0x334b, 0xb0, 0, 0},
+
+ //0720
+ //digital ctrl
+ {0x3802, 0x01, 0, 0},
+ {0x3235, 0x04, 0, 0},
+ {0x3236, 0x63, 0, 0}, // vts-2
+
+ //fpn
+ {0x3343, 0xd0, 0, 0},
+ {0x334b, 0xd0, 0, 0},
+ {0x3348, 0x07, 0, 0},
+ {0x3349, 0x80, 0, 0},
+
+ //0724
+ {0x391b, 0x4d, 0, 0},
+
+ {0x3342, 0x04, 0, 0},
+ {0x3343, 0x20, 0, 0},
+ {0x334a, 0x04, 0, 0},
+ {0x334b, 0x20, 0, 0},
+
+ //0804
+ {0x3222, 0x29, 0, 0},
+ {0x3901, 0x02, 0, 0},
+
+ //0808
+
+ // auto blc
+ {0x3900, 0xD5, 0, 0}, // Bit[0]: blc_enable
+ {0x3902, 0x45, 0, 0}, // Bit[6]: blc_auto_en
+
+ // blc target
+ {0x3907, 0x00, 0, 0},
+ {0x3908, 0x00, 0, 0},
+
+ // auto dpc
+ {0x5000, 0x00, 0, 0}, // Bit[2]: white dead pixel cancel enable, Bit[1]: black dead pixel cancel enable
+
+ //digital ctrl
+ {0x3f00, 0x07, 0, 0}, // bit[2] = 1
+ {0x3f04, 0x08, 0, 0},
+ {0x3f05, 0x74, 0, 0}, // hts - { 0x24
+
+ //0809
+ {0x330b, 0xc8, 0, 0},
+
+ //0817
+ {0x3306, 0x4a, 0, 0},
+ {0x330b, 0xca, 0, 0},
+ {0x3639, 0x09, 0, 0},
+
+ //manual DPC
+ {0x5780, 0xff, 0, 0},
+ {0x5781, 0x04, 0, 0},
+ {0x5785, 0x18, 0, 0},
+
+ //0822
+ {0x3039, 0x35, 0, 0}, //fps
+ {0x303a, 0x2e, 0, 0},
+ {0x3034, 0x05, 0, 0},
+ {0x3035, 0x2a, 0, 0},
+
+ {0x320c, 0x08, 0, 0},
+ {0x320d, 0xca, 0, 0},
+ {0x320e, 0x04, 0, 0},
+ {0x320f, 0xb0, 0, 0},
+
+ {0x3f04, 0x08, 0, 0},
+ {0x3f05, 0xa6, 0, 0}, // hts - { 0x24
+
+ {0x3235, 0x04, 0, 0},
+ {0x3236, 0xae, 0, 0}, // vts-2
+
+ //0825
+ {0x3313, 0x05, 0, 0},
+ {0x3678, 0x42, 0, 0},
+
+ //for AE control per frame
+ {0x3670, 0x00, 0, 0},
+ {0x3633, 0x42, 0, 0},
+
+ {0x3802, 0x00, 0, 0},
+
+ //20180126
+ {0x3677, 0x3f, 0, 0},
+ {0x3306, 0x44, 0, 0}, //20180126[3c },4a]
+ {0x330b, 0xca, 0, 0}, //20180126[c2 },d3]
+
+ //20180202
+ {0x3237, 0x08, 0, 0},
+ {0x3238, 0x9a, 0, 0}, //hts-0x30
+
+ //20180417
+ {0x3640, 0x01, 0, 0},
+ {0x3641, 0x02, 0, 0},
+
+ {0x3301, 0x12, 0, 0}, //[8 },15]20180126
+ {0x3631, 0x84, 0, 0},
+ {0x366f, 0x2f, 0, 0},
+ {0x3622, 0xc6, 0, 0}, //20180117
+
+ {0x3e03, 0x03, 0, 0}, // Bit[3]: AGC table mapping method, Bit[1]: AGC manual, BIt[0]: AEC manual
+
+ // {0x0100, 0x00, 0, 0},
+ // {0x4501, 0xc8, 0, 0}, //bar testing
+ // {0x3902, 0x45, 0, 0},
+};
+
+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_regs_tbl_1080,
+ ARRAY_SIZE(sc2235_init_regs_tbl_1080),
+ SC2235_30_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_30_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
+
+#ifdef UNUSED_CODE
+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;
+}
+#endif
+
+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;
+
+ 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));
+}
+
+#ifdef UNUSED_CODE
+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;
+}
+#endif
+
+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;
+}
+
+#ifdef UNUSED_CODE
+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;
+}
+
+#endif
+
+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;
+}
+
+#ifdef UNUSED_CODE
+/*
+ * 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)
+{
+ u8 prediv, mult, sysdiv;
+ int ret = 0;
+
+ sc2235_calc_sys_clk(sensor, rate, &prediv, &mult,
+ &sysdiv);
+
+
+ return ret;
+}
+
+/*
+ * 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);
+}
+#endif
+
+static int sc2235_set_mode(struct sc2235_dev *sensor)
+{
+#ifdef UNUSED_CODE
+ bool auto_exp = sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
+ const struct sc2235_mode_info *mode = sensor->current_mode;
+#endif
+ bool auto_gain = sensor->ctrls.auto_gain->val == 1;
+ 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;
+ }
+#ifdef UNUSED_CODE
+ /* 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);
+
+ 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;
+
+ /* 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;
+ /* 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 device *dev)
+{
+ struct i2c_client *client = to_i2c_client(dev);
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ 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 int sc2235_set_power_off(struct device *dev)
+{
+ struct i2c_client *client = to_i2c_client(dev);
+ struct v4l2_subdev *sd = i2c_get_clientdata(client);
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+
+ sc2235_power(sensor, false);
+ regulator_bulk_disable(SC2235_NUM_SUPPLIES, sensor->supplies);
+ clk_disable_unprepare(sensor->xclk);
+
+ return 0;
+}
+
+static int sc2235_set_power(struct sc2235_dev *sensor, bool on)
+{
+ int ret = 0;
+
+ if (on) {
+ pm_runtime_get_sync(&sensor->i2c_client->dev);
+
+ ret = sc2235_restore_mode(sensor);
+ if (ret)
+ goto power_off;
+ }
+
+ if (!on)
+ pm_runtime_put_sync(&sensor->i2c_client->dev);
+
+ return 0;
+
+power_off:
+ pm_runtime_put_sync(&sensor->i2c_client->dev);
+
+ 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);
+
+ ret = sc2235_set_power(sensor, !!on);
+ if (ret)
+ goto out;
+
+ mutex_unlock(&sensor->lock);
+ return 0;
+
+out:
+ mutex_unlock(&sensor->lock);
+ 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_30_FPS];
+
+ if (fi->numerator == 0) {
+ fi->denominator = maxfps;
+ fi->numerator = 1;
+ rate = SC2235_30_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.
+ */
+#ifdef UNUSED_CODE
+ 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 */
+
+ if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+ return 0;
+
+ 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;
+ }
+
+ pm_runtime_put(&sensor->i2c_client->dev);
+
+ 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 at start streaming time.
+ */
+ if (!pm_runtime_get_if_in_use(&sensor->i2c_client->dev))
+ 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;
+ }
+
+ pm_runtime_put(&sensor->i2c_client->dev);
+
+ 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 */
+#ifdef UNUSED_CODE
+ /*
+ *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, 720);
+ /* 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 v4l2_fract tpf;
+ int i;
+
+ if (fie->pad != 0)
+ return -EINVAL;
+
+ if (fie->index >= SC2235_NUM_FRAMERATES)
+ return -EINVAL;
+
+ tpf.numerator = 1;
+ tpf.denominator = sc2235_framerates[fie->index];
+
+ for (i = 0; i < SC2235_NUM_MODES; i++) {
+ if (fie->width == sc2235_mode_data[i].hact &&
+ fie->height == sc2235_mode_data[i].vact)
+ break;
+ }
+ if (i == SC2235_NUM_MODES)
+ return -ENOTTY;
+
+ 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_stream_start(struct sc2235_dev *sensor, int enable)
+{
+ return sc2235_mod_reg(sensor, SC2235_REG_STREAM_ON, BIT(0), !!enable);
+}
+
+static int sc2235_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct sc2235_dev *sensor = to_sc2235_dev(sd);
+ int ret = 0;
+
+ if (enable) {
+ ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
+ if (ret)
+ return ret;
+ }
+
+ 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;
+ }
+
+ ret = sc2235_stream_start(sensor, enable);
+ if (ret)
+ goto out;
+ }
+ sensor->streaming += enable ? 1 : -1;
+ WARN_ON(sensor->streaming < 0);
+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_read_reg16(sensor, SC2235_REG_CHIP_ID, &chip_id);
+ if (ret) {
+ dev_err(&client->dev, "%s: failed to read chip identifier\n",
+ __func__);
+ return ret;
+ }
+
+ 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);
+ return -ENXIO;
+ }
+ dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
+ __func__, chip_id);
+
+ return 0;
+}
+
+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;
+
+ 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 = 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;
+
+ /* 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) {
+ 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;
+ }
+
+ sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
+ GPIOD_OUT_HIGH);
+ if (IS_ERR(sensor->pwdn_gpio))
+ return PTR_ERR(sensor->pwdn_gpio);
+
+ 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, &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_set_power_on(dev);
+ if (ret) {
+ dev_err(dev, "failed to power on\n");
+ goto entity_cleanup;
+ }
+
+ ret = sc2235_check_chip_id(sensor);
+ if (ret)
+ goto entity_power_off;
+
+ ret = sc2235_init_controls(sensor);
+ if (ret)
+ goto entity_power_off;
+
+ ret = v4l2_async_register_subdev_sensor(&sensor->sd);
+ if (ret)
+ goto free_ctrls;
+
+ pm_runtime_set_active(dev);
+ pm_runtime_enable(dev);
+
+ return 0;
+
+free_ctrls:
+ v4l2_ctrl_handler_free(&sensor->ctrls.handler);
+entity_power_off:
+ sc2235_set_power_off(dev);
+entity_cleanup:
+ media_entity_cleanup(&sensor->sd.entity);
+ mutex_destroy(&sensor->lock);
+ return ret;
+}
+
+static void 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);
+
+ pm_runtime_disable(&client->dev);
+ if (!pm_runtime_status_suspended(&client->dev))
+ sc2235_set_power_off(&client->dev);
+ pm_runtime_set_suspended(&client->dev);
+}
+
+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 = "smartsens,sc2235" },
+ { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sc2235_dt_ids);
+
+static const struct dev_pm_ops sc2235_pm_ops = {
+ SET_RUNTIME_PM_OPS(sc2235_set_power_off, sc2235_set_power_on, NULL)
+};
+
+static struct i2c_driver sc2235_i2c_driver = {
+ .driver = {
+ .name = "sc2235",
+ .of_match_table = sc2235_dt_ids,
+ .pm = &sc2235_pm_ops,
+ },
+ .id_table = sc2235_id,
+ .probe = sc2235_probe,
+ .remove = sc2235_remove,
+};
+
+module_i2c_driver(sc2235_i2c_driver);
+
+MODULE_DESCRIPTION("SC2235 Camera Subdev Driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_common.h
@@ -0,0 +1,185 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 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_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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_csi.c
@@ -0,0 +1,465 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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>
+
+static const struct csi_format csi_formats_sink[] = {
+ { MEDIA_BUS_FMT_UYVY8_2X8, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+};
+
+/* this bpp need see csi controllor */
+static const struct csi_format csi_formats_src[] = {
+ { MEDIA_BUS_FMT_AYUV8_1X32, 32},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 16},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 16},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 16},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 16},
+};
+
+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)
+{
+ struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
+
+ csi_dev->s_type = SENSOR_VIN;
+ csi_dev->hw_ops = &csi_ops;
+ csi_dev->stfcamss = stfcamss;
+ csi_dev->formats_sink = csi_formats_sink;
+ csi_dev->nformats_sink = ARRAY_SIZE(csi_formats_sink);
+ csi_dev->formats_src = csi_formats_src;
+ csi_dev->nformats_src = ARRAY_SIZE(csi_formats_src);
+ 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);
+
+ csi_dev->hw_ops->csi_power_on(csi_dev, (u8)on);
+ 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 u32 code_to_data_type(int code)
+{
+ switch (code) {
+ case MEDIA_BUS_FMT_SRGGB10_1X10:
+ case MEDIA_BUS_FMT_SGRBG10_1X10:
+ case MEDIA_BUS_FMT_SGBRG10_1X10:
+ case MEDIA_BUS_FMT_SBGGR10_1X10:
+ return 0x2b;
+ case MEDIA_BUS_FMT_UYVY8_2X8:
+ return 0x1E;
+ default:
+ return 0x2b;
+ }
+}
+
+static int csi_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_csi_dev *csi_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+ int ret = 0;
+ u32 code, width, dt;
+ u8 bpp;
+
+ format = __csi_get_format(csi_dev, NULL, STF_CSI_PAD_SINK,
+ V4L2_SUBDEV_FORMAT_ACTIVE);
+ if (format == NULL)
+ return -EINVAL;
+
+ width = format->width;
+
+ ret = csi_find_format(format->code,
+ csi_dev->formats_sink,
+ csi_dev->nformats_sink);
+ if (ret < 0)
+ return ret;
+
+ code = csi_dev->formats_sink[ret].code;
+ bpp = csi_dev->formats_src[ret].bpp;
+ dt = code_to_data_type(code);
+
+ mutex_lock(&csi_dev->stream_lock);
+ if (enable) {
+ if (csi_dev->stream_count == 0) {
+ csi_dev->hw_ops->csi_clk_enable(csi_dev);
+ csi_dev->hw_ops->csi_stream_set(csi_dev, enable, dt, width, bpp);
+ }
+ 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, dt, width, bpp);
+ 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_sink; i++)
+ if (fmt->code == csi_dev->formats_sink[i].code)
+ break;
+
+ if (i >= csi_dev->nformats_sink)
+ fmt->code = csi_dev->formats_sink[0].code;
+
+ fmt->width = clamp_t(u32,
+ fmt->width,
+ STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height,
+ STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ break;
+
+ case STF_CSI_PAD_SRC:
+ /* Set format on src pad */
+
+ for (i = 0; i < csi_dev->nformats_src; i++)
+ if (fmt->code == csi_dev->formats_src[i].code)
+ break;
+
+ if (i >= csi_dev->nformats_src)
+ fmt->code = csi_dev->formats_src[0].code;
+
+ fmt->width = clamp_t(u32,
+ fmt->width,
+ STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height,
+ STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+
+ 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_sink)
+ return -EINVAL;
+ if (code->pad == STF_CSI_PAD_SINK) {
+ code->code = csi_dev->formats_sink[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;
+ struct v4l2_mbus_framefmt *format_src;
+ int ret;
+
+ format = __csi_get_format(csi_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ mutex_lock(&csi_dev->stream_lock);
+ if (csi_dev->stream_count) {
+ fmt->format = *format;
+ mutex_unlock(&csi_dev->stream_lock);
+ goto out;
+ } else {
+ csi_try_format(csi_dev, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&csi_dev->stream_lock);
+
+ if (fmt->pad == STF_CSI_PAD_SINK) {
+ format_src = __csi_get_format(csi_dev, state, STF_DVP_PAD_SRC,
+ fmt->which);
+
+ ret = csi_find_format(format->code, csi_dev->formats_sink,
+ csi_dev->nformats_sink);
+ if (ret < 0)
+ return ret;
+
+ format_src->code = csi_dev->formats_src[ret].code;
+ csi_try_format(csi_dev, state, STF_DVP_PAD_SRC, format_src,
+ fmt->which);
+ }
+out:
+ 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_pad_remote_pad_first(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 == ISP_DEV_TYPE)
+ csi_dev->s_type = SENSOR_ISP;
+ st_info(ST_CSI, "CSI device sensor type: %d\n", 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_pad_remote_pad_first(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);
+
+ st_info(ST_CSI, "CSI0 link to csiphy0\n");
+ }
+
+ 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;
+
+ csi_dev->mipirx_1p8 = devm_regulator_get(dev, "mipirx_1p8");
+ if (IS_ERR(csi_dev->mipirx_1p8))
+ return PTR_ERR(csi_dev->mipirx_1p8);
+
+ csi_dev->mipirx_0p9 = devm_regulator_get(dev, "mipirx_0p9");
+ if (IS_ERR(csi_dev->mipirx_0p9))
+ return PTR_ERR(csi_dev->mipirx_0p9);
+
+ 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, 0);
+ 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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_csi.h
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#ifndef STF_CSI_H
+#define STF_CSI_H
+
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-device.h>
+#include <media/media-entity.h>
+#include <video/stf-vin.h>
+
+#define STF_CSI_NAME "stf_csi"
+
+#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_power_on)(struct stf_csi_dev *csi_dev, u8 on);
+ int (*csi_clk_enable)(struct stf_csi_dev *csi_dev);
+ int (*csi_clk_disable)(struct stf_csi_dev *csi_dev);
+ int (*csi_stream_set)(struct stf_csi_dev *csi_dev, int on,
+ u32 dt, u32 width, u8 bpp);
+};
+
+struct stf_csi_dev {
+ struct stfcamss *stfcamss;
+ 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_sink;
+ unsigned int nformats_sink;
+ const struct csi_format *formats_src;
+ unsigned int nformats_src;
+ struct csi_hw_ops *hw_ops;
+ struct mutex stream_lock;
+ int stream_count;
+ struct regulator *mipirx_1p8;
+ struct regulator *mipirx_0p9;
+};
+
+extern int stf_csi_subdev_init(struct stfcamss *stfcamss);
+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 struct csi_hw_ops csi_ops;
+extern void dump_csi_reg(void *__iomem csibase);
+
+#endif /* STF_CSI_H */
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_csi_hw_ops.c
@@ -0,0 +1,310 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#include "stfcamss.h"
+#include <linux/regmap.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_EN_DATA_TYPE_0 BIT(7)
+#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 stf_csi_power_on(struct stf_csi_dev *csi_dev, u8 on)
+{
+ struct stfcamss *stfcamss = csi_dev->stfcamss;
+ int ret;
+
+ if (on) {
+ ret = regulator_enable(csi_dev->mipirx_1p8);
+ if (ret) {
+ st_err(ST_CSI, "Cannot enable mipirx_1p8 regulator\n");
+ goto err_1p8;
+ }
+
+ ret = regulator_enable(csi_dev->mipirx_0p9);
+ if (ret) {
+ st_err(ST_CSI, "Cannot enable mipirx_0p9 regulator\n");
+ goto err_0p9;
+ }
+ } else {
+ regulator_disable(csi_dev->mipirx_1p8);
+ regulator_disable(csi_dev->mipirx_0p9);
+ }
+
+ regmap_update_bits(stfcamss->stf_aon_syscon, stfcamss->aon_gp_reg,
+ BIT(31), BIT(31));
+
+ return 0;
+
+err_0p9:
+ regulator_disable(csi_dev->mipirx_1p8);
+err_1p8:
+ return ret;
+
+}
+
+static int stf_csi_clk_enable(struct stf_csi_dev *csi_dev)
+{
+ struct stfcamss *stfcamss = csi_dev->stfcamss;
+
+ clk_set_rate(stfcamss->sys_clk[STFCLK_MIPI_RX0_PXL].clk, 198000000);
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF0].clk);
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF1].clk);
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF2].clk);
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF3].clk);
+
+ reset_control_deassert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF0].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF1].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF2].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF3].rstc);
+
+ switch (csi_dev->s_type) {
+ case SENSOR_VIN:
+ reset_control_deassert(stfcamss->sys_rst[STFRST_AXIWR].rstc);
+ clk_set_parent(stfcamss->sys_clk[STFCLK_AXIWR].clk,
+ stfcamss->sys_clk[STFCLK_MIPI_RX0_PXL].clk);
+ break;
+ case SENSOR_ISP:
+ clk_set_parent(stfcamss->sys_clk[STFCLK_WRAPPER_CLK_C].clk,
+ stfcamss->sys_clk[STFCLK_MIPI_RX0_PXL].clk);
+ break;
+ }
+
+ return 0;
+}
+
+static int stf_csi_clk_disable(struct stf_csi_dev *csi_dev)
+{
+ struct stfcamss *stfcamss = csi_dev->stfcamss;
+
+ switch (csi_dev->s_type) {
+ case SENSOR_VIN:
+ reset_control_assert(stfcamss->sys_rst[STFRST_AXIWR].rstc);
+ break;
+ case SENSOR_ISP:
+ break;
+ }
+
+ reset_control_assert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF3].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF2].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF1].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_PIXEL_CLK_IF0].rstc);
+
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF3].clk);
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF2].clk);
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF1].clk);
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_PIXEL_CLK_IF0].clk);
+
+ 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 int csi2rx_start(struct stf_csi_dev *csi_dev, void *reg_base, u32 dt)
+{
+ struct stfcamss *stfcamss = csi_dev->stfcamss;
+ struct csi2phy_cfg *csiphy =
+ stfcamss->csiphy_dev->csiphy;
+ unsigned int i;
+ unsigned long lanes_used = 0;
+ u32 reg;
+
+ if (!csiphy) {
+ st_err(ST_CSI, "csiphy0 config not exist\n");
+ return -EINVAL;
+ }
+
+ 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); //data_clane
+#endif
+
+ reg |= 1 << 4 | 1 << 16; //clk_lane
+ writel(reg, reg_base + CSI2RX_DPHY_LANE_CONTROL);
+
+ /*
+ * 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) |
+ CSI2RX_STREAM_DATA_CFG_EN_DATA_TYPE_0 | dt,
+ 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(struct stf_csi_dev *csi_dev, void *reg_base)
+{
+ unsigned int i;
+
+ for (i = 0; i < CSI2RX_STREAMS_MAX; i++)
+ writel(0, reg_base + CSI2RX_STREAM_CTRL_REG(i));
+}
+
+static void csi_set_vin_axiwr_pix(struct stf_csi_dev *csi_dev, u32 width, u8 bpp)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+ u32 value = 0;
+ int cnfg_axiwr_pix_ct = 64 / bpp;
+
+ if (cnfg_axiwr_pix_ct == 2)
+ value = 0;
+ else if (cnfg_axiwr_pix_ct == 4)
+ value = 1;
+ else if (cnfg_axiwr_pix_ct == 8)
+ value = 2;
+
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ BIT(14)|BIT(13), value << 13); //u0_vin_cnfg_axiwr0_pix_ct
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ BIT(12)|BIT(11)|BIT(10)|BIT(9)|BIT(8)|BIT(7)|BIT(6)|BIT(5)|BIT(4)|BIT(3)|BIT(2),
+ (width / cnfg_axiwr_pix_ct - 1)<<2); //u0_vin_cnfg_axiwr0_pix_cnt_end
+}
+
+static int stf_csi_stream_set(struct stf_csi_dev *csi_dev,
+ int on, u32 dt, u32 width, u8 bpp)
+{
+ struct stf_vin_dev *vin = csi_dev->stfcamss->vin;
+ void __iomem *reg_base = vin->csi2rx_base;
+
+ switch (csi_dev->s_type) {
+ case SENSOR_VIN:
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_20,
+ BIT(3)|BIT(2)|BIT(1)|BIT(0),
+ 0<<0); //u0_vin_cnfg_axiwr0_channel_sel
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ BIT(16)|BIT(15),
+ 0<<15); //u0_vin_cnfg_axiwr0_pixel_high_bit_sel
+ csi_set_vin_axiwr_pix(csi_dev, width, bpp);
+ break;
+ case SENSOR_ISP:
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(7)|BIT(6),
+ 0<<6); //u0_vin_cnfg_mipi_byte_en_isp
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(11)|BIT(10)|BIT(9)|BIT(8),
+ 0<<8); //u0_vin_cnfg_mipi_channel_sel0
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(16)|BIT(15)|BIT(14)|BIT(13),
+ 0<<13); //u0_vin_cnfg_pix_num
+
+ if (dt == 0x2b)
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ BIT(12),
+ 1<<12); //u0_vin_cnfg_p_i_mipi_header_en0
+ break;
+ }
+
+ if (on)
+ csi2rx_start(csi_dev, reg_base, dt);
+ else
+ csi2rx_stop(csi_dev, reg_base);
+
+ return 0;
+}
+
+void dump_csi_reg(void *__iomem csibase)
+{
+ st_info(ST_CSI, "DUMP CSI register:\n");
+ 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_power_on = stf_csi_power_on,
+ .csi_clk_enable = stf_csi_clk_enable,
+ .csi_clk_disable = stf_csi_clk_disable,
+ .csi_stream_set = stf_csi_stream_set,
+};
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_csiphy.c
@@ -0,0 +1,357 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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>
+
+static const struct csiphy_format csiphy_formats_st7110[] = {
+ { MEDIA_BUS_FMT_UYVY8_2X8, 16},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+};
+
+int stf_csiphy_subdev_init(struct stfcamss *stfcamss)
+{
+ struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
+
+ 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)
+{
+ 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);
+
+ 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 = csiphy_dev->formats[0].code;
+
+ fmt->width = clamp_t(u32,
+ fmt->width,
+ STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height,
+ STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+
+ 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;
+
+ mutex_lock(&csiphy_dev->stream_lock);
+ if (csiphy_dev->stream_count) {
+ fmt->format = *format;
+ mutex_unlock(&csiphy_dev->stream_lock);
+ goto out;
+ } else {
+ csiphy_try_format(csiphy_dev, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&csiphy_dev->stream_lock);
+
+ /* 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);
+ }
+out:
+ 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_pad_remote_pad_first(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);
+ st_info(ST_CSIPHY, "CSIPHY0 link to CSI0\n");
+ }
+
+ 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, 0);
+ 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;
+ }
+
+ 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_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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_csiphy.h
@@ -0,0 +1,188 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 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_NAME "stf_csiphy"
+
+#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);
+};
+
+struct stf_csiphy_dev {
+ struct stfcamss *stfcamss;
+ struct csi2phy_cfg *csiphy;
+ 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);
+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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_csiphy_hw_ops.c
@@ -0,0 +1,335 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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 stfcamss *stfcamss = csiphy_dev->stfcamss;
+ 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) {
+ clk_set_rate(stfcamss->sys_clk[STFCLK_M31DPHY_CFGCLK_IN].clk,
+ 99000000);
+ clk_set_rate(stfcamss->sys_clk[STFCLK_M31DPHY_REFCLK_IN].clk,
+ 49500000);
+ clk_set_rate(stfcamss->sys_clk[STFCLK_M31DPHY_TXCLKESC_LAN0].clk,
+ 19800000);
+
+ reset_control_deassert(stfcamss->sys_rst[STFRST_M31DPHY_HW].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_M31DPHY_B09_ALWAYS_ON].rstc);
+
+ count++;
+ } else {
+ if (count == 0)
+ goto exit;
+ if (count == 1) {
+ reset_control_assert(stfcamss->sys_rst[STFRST_M31DPHY_HW].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_M31DPHY_B09_ALWAYS_ON].rstc);
+ }
+ 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);
+}
+
+#ifndef USE_CSIDPHY_ONE_CLK_MODE
+static int cmp_func(const void *x1, const void *x2)
+{
+ return *((unsigned char *)x1) - *((unsigned char *)x2);
+}
+#endif
+
+int try_cfg(struct csi2phy_cfg2 *cfg, struct csi2phy_cfg *cfg0,
+ struct csi2phy_cfg *cfg1)
+{
+ int i = 0;
+
+ cfg->clock_lane = 0;
+ cfg->clock1_lane = 5;
+ cfg->data_lanes[0] = 1;
+ cfg->data_lanes[1] = 2;
+ cfg->data_lanes[2] = 3;
+ cfg->data_lanes[3] = 4;
+
+ 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 csi2phy_cfg *phycfg = csiphy_dev->csiphy;
+
+ if (!phycfg)
+ return -EINVAL;
+
+ if (try_cfg(cfg, phycfg, NULL))
+ return -EINVAL;
+
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_4, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_8, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_12, 0xfff0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_16, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_20, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_24, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_28, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_32, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_36, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_40, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_44, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_48, 0x24000000);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_52, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_56, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_60, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_64, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_68, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_72, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_76, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_80, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_84, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_88, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_92, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_96, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_100, 0x02000000);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_104, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_108, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_112, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_116, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_120, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_124, 0xc);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_128, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_132, 0xcc500000);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_136, 0xcc);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_140, 0x0);
+ reg_write(vin->rstgen_base, M31DPHY_APBCFGSAIF__SYSCFG_144, 0x0);
+
+ reg_set_bit(vin->rstgen_base, //r100_ctrl0_2d1c_efuse_en
+ M31DPHY_APBCFGSAIF__SYSCFG_0,
+ BIT(6), 1<<6);
+ reg_set_bit(vin->rstgen_base, //r100_ctrl0_2d1c_efuse_in
+ M31DPHY_APBCFGSAIF__SYSCFG_0,
+ BIT(12)|BIT(11)|BIT(10)|BIT(9)|BIT(8)|BIT(7), 0x1b<<7);
+ reg_set_bit(vin->rstgen_base, //r100_ctrl1_2d1c_efuse_en
+ M31DPHY_APBCFGSAIF__SYSCFG_0,
+ BIT(13), 1<<13);
+ reg_set_bit(vin->rstgen_base, //r100_ctrl1_2d1c_efuse_in
+ M31DPHY_APBCFGSAIF__SYSCFG_0,
+ BIT(19)|BIT(18)|BIT(17)|BIT(16)|BIT(15)|BIT(14), 0x1b<<14);
+
+ reg_set_bit(vin->rstgen_base, //data_bus16_8
+ M31DPHY_APBCFGSAIF__SYSCFG_184,
+ BIT(8), 0<<8);
+
+ reg_set_bit(vin->rstgen_base, //debug_mode_sel
+ M31DPHY_APBCFGSAIF__SYSCFG_184,
+ BIT(15)|BIT(14)|BIT(13)|BIT(12)|BIT(11)|BIT(10)|BIT(9), 0x5a<<9);
+
+ reg_set_bit(vin->rstgen_base, //dpdn_swap_clk0
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(0), cfg->lane_polarities[0]<<0);
+ reg_set_bit(vin->rstgen_base, //dpdn_swap_clk1
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(1), cfg->lane_polarities[1]<<1);
+ reg_set_bit(vin->rstgen_base, //dpdn_swap_lan0
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(2), cfg->lane_polarities[2]<<2);
+ reg_set_bit(vin->rstgen_base, //dpdn_swap_lan1
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(3), cfg->lane_polarities[3]<<3);
+ reg_set_bit(vin->rstgen_base, //dpdn_swap_lan2
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(4), cfg->lane_polarities[4]<<4);
+ reg_set_bit(vin->rstgen_base, //dpdn_swap_lan3
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(5), cfg->lane_polarities[5]<<5);
+ reg_set_bit(vin->rstgen_base, //enable clk0
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(6), 1<<6);
+ reg_set_bit(vin->rstgen_base, //enable clk1
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(7), 1<<7);
+ reg_set_bit(vin->rstgen_base, //enable lan0
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(8), 1<<8);
+ reg_set_bit(vin->rstgen_base, //enable lan1
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(9), 1<<9);
+ reg_set_bit(vin->rstgen_base, //enable lan2
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(10), 1<<10);
+ reg_set_bit(vin->rstgen_base, //enable lan3
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(11), 1<<11);
+ reg_set_bit(vin->rstgen_base, //gpi_en
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(17)|BIT(16)|BIT(15)|BIT(14)|BIT(13)|BIT(12),
+ 0<<12);
+ reg_set_bit(vin->rstgen_base, //hs_freq_change_clk0
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(18), 0<<18);
+ reg_set_bit(vin->rstgen_base, //hs_freq_change_clk1
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(19), 0<<19);
+
+ reg_set_bit(vin->rstgen_base,
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(22)|BIT(21)|BIT(20), cfg->clock_lane<<20); //clock lane 0
+ reg_set_bit(vin->rstgen_base,
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(25)|BIT(24)|BIT(23), cfg->clock1_lane<<23); //clock lane 1
+
+ reg_set_bit(vin->rstgen_base,
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(28)|BIT(27)|BIT(26), cfg->data_lanes[0]<<26); //data lane 0
+ reg_set_bit(vin->rstgen_base,
+ M31DPHY_APBCFGSAIF__SYSCFG_188,
+ BIT(31)|BIT(30)|BIT(29), cfg->data_lanes[1]<<29); //data lane 1
+ reg_set_bit(vin->rstgen_base,
+ M31DPHY_APBCFGSAIF__SYSCFG_192,
+ BIT(2)|BIT(1)|BIT(0), cfg->data_lanes[2]<<0); //data lane 2
+ reg_set_bit(vin->rstgen_base,
+ M31DPHY_APBCFGSAIF__SYSCFG_192,
+ BIT(5)|BIT(4)|BIT(3), cfg->data_lanes[3]<<3); //data lane 3
+
+ reg_set_bit(vin->rstgen_base, //mp_test_en
+ M31DPHY_APBCFGSAIF__SYSCFG_192,
+ BIT(6), 0<<6);
+ reg_set_bit(vin->rstgen_base, //mp_test_mode_sel
+ M31DPHY_APBCFGSAIF__SYSCFG_192,
+ BIT(11)|BIT(10)|BIT(9)|BIT(8)|BIT(7), 0<<7);
+
+ reg_set_bit(vin->rstgen_base, //pll_clk_sel
+ M31DPHY_APBCFGSAIF__SYSCFG_192,
+ BIT(20)|BIT(19)|BIT(18)|BIT(17)|BIT(16)|BIT(15)|BIT(14)|BIT(13)|BIT(12),
+ 0x37c<<12);
+
+ reg_set_bit(vin->rstgen_base, //rx_1c2c_sel
+ M31DPHY_APBCFGSAIF__SYSCFG_200,
+ BIT(8), 0<<8);
+
+ reg_set_bit(vin->rstgen_base, //precounter in clk0
+ M31DPHY_APBCFGSAIF__SYSCFG_192,
+ BIT(29)|BIT(28)|BIT(27)|BIT(26)|BIT(25)|BIT(24)|BIT(23)|BIT(22),
+ 8<<22);
+ reg_set_bit(vin->rstgen_base, //precounter in clk1
+ M31DPHY_APBCFGSAIF__SYSCFG_196,
+ BIT(7)|BIT(6)|BIT(5)|BIT(4)|BIT(3)|BIT(2)|BIT(1)|BIT(0),
+ 8<<0);
+ reg_set_bit(vin->rstgen_base, //precounter in lan0
+ M31DPHY_APBCFGSAIF__SYSCFG_196,
+ BIT(15)|BIT(14)|BIT(13)|BIT(12)|BIT(11)|BIT(10)|BIT(9)|BIT(8),
+ 7<<8);
+ reg_set_bit(vin->rstgen_base, //precounter in lan1
+ M31DPHY_APBCFGSAIF__SYSCFG_196,
+ BIT(23)|BIT(22)|BIT(21)|BIT(20)|BIT(19)|BIT(18)|BIT(17)|BIT(16),
+ 7<<16);
+ reg_set_bit(vin->rstgen_base, //precounter in lan2
+ M31DPHY_APBCFGSAIF__SYSCFG_196,
+ BIT(31)|BIT(30)|BIT(29)|BIT(28)|BIT(27)|BIT(26)|BIT(25)|BIT(24),
+ 7<<24);
+ reg_set_bit(vin->rstgen_base, //precounter in lan3
+ M31DPHY_APBCFGSAIF__SYSCFG_200,
+ BIT(7)|BIT(6)|BIT(5)|BIT(4)|BIT(3)|BIT(2)|BIT(1)|BIT(0),
+ 7<<0);
+
+ return 0;
+}
+
+static int stf_csiphy_config_set(struct stf_csiphy_dev *csiphy_dev)
+{
+ struct stf_vin_dev *vin = csiphy_dev->stfcamss->vin;
+
+ csi2rx_dphy_config(vin, csiphy_dev);
+ return 0;
+}
+
+static int stf_csiphy_stream_set(struct stf_csiphy_dev *csiphy_dev, int on)
+{
+ return 0;
+}
+
+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,
+};
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_dmabuf.c
@@ -0,0 +1,123 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#include <linux/dma-buf.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "stf_isp_ioctl.h"
+#include "stf_dmabuf.h"
+
+#define TOTAL_SIZE_LIMIT (64 * 1024 * 1024)
+
+static size_t total_size;
+static struct vb2_queue vb2_queue = {
+ .dma_attrs = 0,
+ .gfp_flags = 0,
+ .dma_dir = DMA_TO_DEVICE,
+};
+static struct vb2_buffer vb = {
+ .vb2_queue = &vb2_queue,
+};
+
+static int dmabuf_create(struct device *dev,
+ struct dmabuf_create *head)
+{
+ struct dma_buf *dmabuf = NULL;
+ void *mem_priv = NULL;
+ dma_addr_t *paddr = NULL;
+ int ret = 0;
+
+ mem_priv = vb2_dma_contig_memops.alloc(&vb, dev, head->size);
+ if (IS_ERR_OR_NULL(mem_priv)) {
+ if (mem_priv)
+ ret = PTR_ERR(mem_priv);
+ goto exit;
+ }
+
+ dmabuf = vb2_dma_contig_memops.get_dmabuf(&vb, mem_priv, O_RDWR);
+ if (IS_ERR(dmabuf)) {
+ ret = PTR_ERR(dmabuf);
+ goto free;
+ }
+
+ head->fd = dma_buf_fd(dmabuf, O_CLOEXEC);
+ if (head->fd < 0) {
+ dma_buf_put(dmabuf);
+ ret = head->fd;
+ goto free;
+ }
+
+ paddr = vb2_dma_contig_memops.cookie(&vb, mem_priv);
+ head->paddr = *paddr;
+ return 0;
+free:
+ vb2_dma_contig_memops.put(mem_priv);
+exit:
+ return ret;
+}
+
+int stf_dmabuf_ioctl_alloc(struct device *dev, void *arg)
+{
+ struct dmabuf_create *head = arg;
+ int ret = -EINVAL;
+
+ if (IS_ERR_OR_NULL(head))
+ return -EFAULT;
+
+ head->size = PAGE_ALIGN(head->size);
+ if (!head->size)
+ return -EINVAL;
+ if ((head->size + total_size) > TOTAL_SIZE_LIMIT)
+ return -ENOMEM;
+
+ ret = dmabuf_create(dev, head);
+ if (ret)
+ return -EFAULT;
+
+ total_size += head->size;
+ return ret;
+}
+
+int stf_dmabuf_ioctl_free(struct device *dev, void *arg)
+{
+ struct dmabuf_create *head = arg;
+ struct dma_buf *dmabuf = NULL;
+ int ret = 0;
+
+ if (IS_ERR_OR_NULL(head))
+ return -EFAULT;
+ if (head->size != PAGE_ALIGN(head->size))
+ return -EINVAL;
+ if (head->size > total_size)
+ return -EINVAL;
+
+ dmabuf = dma_buf_get(head->fd);
+ if (IS_ERR_OR_NULL(dmabuf))
+ return -EINVAL;
+
+ dma_buf_put(dmabuf);
+ vb2_dma_contig_memops.put(dmabuf->priv);
+ total_size -= head->size;
+ return ret;
+}
+
+int stf_dmabuf_ioctl(struct device *dev, unsigned int cmd, void *arg)
+{
+ int ret = -ENOIOCTLCMD;
+
+ switch (cmd) {
+ case VIDIOC_STF_DMABUF_ALLOC:
+ ret = stf_dmabuf_ioctl_alloc(dev, arg);
+ break;
+ case VIDIOC_STF_DMABUF_FREE:
+ ret = stf_dmabuf_ioctl_free(dev, arg);
+ break;
+ default:
+ break;
+ }
+ return ret;
+}
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_dmabuf.h
@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#ifndef STF_DMABUF_H
+#define STF_DMABUF_H
+
+extern int stf_dmabuf_ioctl(struct device *dev, unsigned int cmd, void *arg);
+
+#endif /* STF_DMABUF_H */
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_dvp.c
@@ -0,0 +1,385 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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>
+
+static const struct dvp_format dvp_formats_st7110[] = {
+ { MEDIA_BUS_FMT_YUYV8_2X8, 8},
+ { MEDIA_BUS_FMT_RGB565_2X8_LE, 8},
+ { MEDIA_BUS_FMT_SRGGB8_1X8, 8},
+ { MEDIA_BUS_FMT_SGRBG8_1X8, 8},
+ { MEDIA_BUS_FMT_SGBRG8_1X8, 8},
+ { MEDIA_BUS_FMT_SBGGR8_1X8, 8},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 8},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 8},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 8},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 8},
+};
+
+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)
+{
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__dvp_get_format(struct stf_dvp_dev *dvp_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(
+ &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_enable(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->hw_ops->dvp_clk_disable(dvp_dev);
+ }
+ 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_state *state,
+ 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 = dvp_dev->formats[0].code;
+
+ fmt->width = clamp_t(u32,
+ fmt->width, STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height, STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+
+ 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, state, 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, 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, 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, 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, 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;
+
+ format = __dvp_get_format(dvp_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ mutex_lock(&dvp_dev->stream_lock);
+ if (dvp_dev->stream_count) {
+ fmt->format = *format;
+ mutex_unlock(&dvp_dev->stream_lock);
+ goto out;
+ } else {
+ dvp_try_format(dvp_dev, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&dvp_dev->stream_lock);
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_DVP_PAD_SINK) {
+ format = __dvp_get_format(dvp_dev, state, STF_DVP_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ dvp_try_format(dvp_dev, state, STF_DVP_PAD_SRC, format,
+ fmt->which);
+ }
+
+out:
+ 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_pad_remote_pad_first(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 == ISP_DEV_TYPE)
+ dvp_dev->s_type = SENSOR_ISP;
+ 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 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, 0);
+ 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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_dvp.h
@@ -0,0 +1,67 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 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_NAME "stf_dvp"
+
+#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_enable)(struct stf_dvp_dev *dvp_dev);
+ int (*dvp_clk_disable)(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;
+ 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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_dvp_hw_ops.c
@@ -0,0 +1,187 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#include "stfcamss.h"
+
+static int stf_dvp_clk_enable(struct stf_dvp_dev *dvp_dev)
+{
+ struct stfcamss *stfcamss = dvp_dev->stfcamss;
+
+ switch (dvp_dev->s_type) {
+ case SENSOR_VIN:
+ reset_control_deassert(stfcamss->sys_rst[STFRST_AXIWR].rstc);
+ clk_set_phase(stfcamss->sys_clk[STFCLK_DVP_INV].clk, 0);
+ clk_set_parent(stfcamss->sys_clk[STFCLK_AXIWR].clk,
+ stfcamss->sys_clk[STFCLK_DVP_INV].clk);
+ break;
+ case SENSOR_ISP:
+ clk_set_phase(stfcamss->sys_clk[STFCLK_DVP_INV].clk, 0);
+ clk_set_parent(stfcamss->sys_clk[STFCLK_WRAPPER_CLK_C].clk,
+ stfcamss->sys_clk[STFCLK_DVP_INV].clk);
+ break;
+ }
+
+ return 0;
+}
+
+static int stf_dvp_clk_disable(struct stf_dvp_dev *dvp_dev)
+{
+ struct stfcamss *stfcamss = dvp_dev->stfcamss;
+
+ switch (dvp_dev->s_type) {
+ case SENSOR_VIN:
+ clk_set_parent(stfcamss->sys_clk[STFCLK_AXIWR].clk,
+ stfcamss->sys_clk[STFCLK_MIPI_RX0_PXL].clk);
+ reset_control_assert(stfcamss->sys_rst[STFRST_AXIWR].rstc);
+ break;
+ case SENSOR_ISP:
+ clk_set_parent(stfcamss->sys_clk[STFCLK_WRAPPER_CLK_C].clk,
+ stfcamss->sys_clk[STFCLK_MIPI_RX0_PXL].clk);
+ break;
+ }
+
+ return 0;
+}
+
+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);
+
+ 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,
+ U0_VIN_CNFG_DVP_HS_POS
+ | U0_VIN_CNFG_DVP_VS_POS,
+ 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, SYSCONSAIF_SYSCFG_28);
+ 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, SYSCONSAIF_SYSCFG_28);
+
+ 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 = 1;
+ else if (cnfg_axiwr_pix_ct == 8)
+ value = 0;
+ else
+ return 0;
+
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCONSAIF_SYSCFG_28);
+ reg_set_bit(vin->sysctrl_base,
+ SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_PIX_CT,
+ value<<13);
+ print_reg(ST_DVP, vin->sysctrl_base, SYSCONSAIF_SYSCFG_28);
+
+ 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_set_bit(vin->sysctrl_base,
+ SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_PIX_CNT_END,
+ val << 2);
+ 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:
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ U0_VIN_CNFG_ISP_DVP_EN0,
+ 0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_0,
+ U0_VIN_CNFG_AXI_DVP_EN,
+ !!on<<2);
+ break;
+ case SENSOR_ISP:
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ U0_VIN_CNFG_ISP_DVP_EN0,
+ !!on<<5);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_0,
+ U0_VIN_CNFG_AXI_DVP_EN,
+ 0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ U0_VIN_CNFG_DVP_SWAP_EN,
+ 0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_36,
+ U0_VIN_CNFG_GEN_EN_AXIRD,
+ 0);
+ break;
+ }
+
+ return 0;
+}
+
+struct dvp_hw_ops dvp_ops = {
+ .dvp_clk_enable = stf_dvp_clk_enable,
+ .dvp_clk_disable = stf_dvp_clk_disable,
+ .dvp_config_set = stf_dvp_config_set,
+ .dvp_set_format = stf_dvp_set_format,
+ .dvp_stream_set = stf_dvp_stream_set,
+};
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_event.c
@@ -0,0 +1,36 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_isp.c
@@ -0,0 +1,1521 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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>
+#include <linux/jh7110-isp.h>
+#include "stf_isp_ioctl.h"
+#include "stf_dmabuf.h"
+
+static int user_config_isp;
+static int isp_set_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel);
+
+static struct v4l2_rect *
+__isp_get_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which);
+
+static struct v4l2_rect *
+__isp_get_crop(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which);
+
+static struct v4l2_rect *
+__isp_get_scale(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel);
+
+static struct v4l2_rect *
+__isp_get_itiws(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which);
+
+// sink format and raw format must one by one
+static const struct isp_format isp_formats_st7110_sink[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+};
+
+static const struct isp_format isp_formats_st7110_raw[] = {
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
+};
+
+static const struct isp_format isp_formats_st7110_compat_10bit_raw[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+};
+
+static const struct isp_format isp_formats_st7110_compat_8bit_raw[] = {
+ { MEDIA_BUS_FMT_SRGGB8_1X8, 8},
+ { MEDIA_BUS_FMT_SGRBG8_1X8, 8},
+ { MEDIA_BUS_FMT_SGBRG8_1X8, 8},
+ { MEDIA_BUS_FMT_SBGGR8_1X8, 8},
+};
+
+static const struct isp_format isp_formats_st7110_uo[] = {
+ { MEDIA_BUS_FMT_Y12_1X12, 8},
+};
+
+static const struct isp_format isp_formats_st7110_iti[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
+ { MEDIA_BUS_FMT_Y12_1X12, 8},
+ { MEDIA_BUS_FMT_YUV8_1X24, 8},
+};
+
+static const struct isp_format_table isp_formats_st7110[] = {
+ { isp_formats_st7110_sink, ARRAY_SIZE(isp_formats_st7110_sink) }, /* pad 0 */
+ { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) }, /* pad 1 */
+ { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) }, /* pad 2 */
+ { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) }, /* pad 3 */
+ { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) }, /* pad 4 */
+ { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) }, /* pad 5 */
+ { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) }, /* pad 6 */
+ { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) }, /* pad 7 */
+};
+
+int stf_isp_subdev_init(struct stfcamss *stfcamss)
+{
+ struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+
+ isp_dev->sdev_type = ISP_DEV_TYPE;
+ 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->power_lock);
+ mutex_init(&isp_dev->setfile_lock);
+ atomic_set(&isp_dev->shadow_count, 0);
+ 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)
+{
+ 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 = 0;
+
+ /*
+ * 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.
+ */
+ mutex_lock(&isp_dev->power_lock);
+ if (isp_dev->power_count == 0) {
+ mutex_unlock(&isp_dev->power_lock);
+ return 0;
+ }
+ mutex_unlock(&isp_dev->power_lock);
+
+ 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;
+ case V4L2_CID_USER_JH7110_ISP_WB_SETTING:
+ break;
+ case V4L2_CID_USER_JH7110_ISP_CAR_SETTING:
+ break;
+ case V4L2_CID_USER_JH7110_ISP_CCM_SETTING:
+ 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,
+};
+
+struct v4l2_ctrl_config isp_ctrl[] = {
+ [0] = {
+ .ops = &isp_ctrl_ops,
+ .type = V4L2_CTRL_TYPE_U8,
+ .def = 0,
+ .min = 0x00,
+ .max = 0xff,
+ .step = 1,
+ .name = "WB Setting",
+ .id = V4L2_CID_USER_JH7110_ISP_WB_SETTING,
+ .dims[0] = sizeof(struct jh7110_isp_wb_setting),
+ .flags = 0,
+ },
+ [1] = {
+ .ops = &isp_ctrl_ops,
+ .type = V4L2_CTRL_TYPE_U8,
+ .def = 0,
+ .min = 0x00,
+ .max = 0xff,
+ .step = 1,
+ .name = "Car Setting",
+ .id = V4L2_CID_USER_JH7110_ISP_CAR_SETTING,
+ .dims[0] = sizeof(struct jh7110_isp_car_setting),
+ .flags = 0,
+ },
+ [2] = {
+ .ops = &isp_ctrl_ops,
+ .type = V4L2_CTRL_TYPE_U8,
+ .def = 0,
+ .min = 0x00,
+ .max = 0xff,
+ .step = 1,
+ .name = "CCM Setting",
+ .id = V4L2_CID_USER_JH7110_ISP_CCM_SETTING,
+ .dims[0] = sizeof(struct jh7110_isp_ccm_setting),
+ .flags = 0,
+ },
+};
+
+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;
+ int i;
+
+ 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);
+
+ for (i = 0; i < ARRAY_SIZE(isp_ctrl); i++)
+ v4l2_ctrl_new_custom(hdl, &isp_ctrl[i], NULL);
+
+
+ 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);
+
+ st_debug(ST_ISP, "%s, %d\n", __func__, __LINE__);
+ mutex_lock(&isp_dev->power_lock);
+ if (on) {
+ if (isp_dev->power_count == 0)
+ st_debug(ST_ISP, "turn on isp\n");
+ isp_dev->power_count++;
+ } else {
+ if (isp_dev->power_count == 0)
+ goto exit;
+ isp_dev->power_count--;
+ }
+exit:
+ mutex_unlock(&isp_dev->power_lock);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__isp_get_format(struct stf_isp_dev *isp_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(&isp_dev->subdev, state, pad);
+
+ return &isp_dev->fmt[pad];
+}
+
+static int isp_get_interface_type(struct media_entity *entity)
+{
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad = &entity->pads[0];
+
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ return -EINVAL;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ return -EINVAL;
+
+ subdev = media_entity_to_v4l2_subdev(pad->entity);
+
+ st_debug(ST_ISP, "interface subdev name %s\n", subdev->name);
+ if (!strncmp(subdev->name, STF_CSI_NAME, strlen(STF_CSI_NAME)))
+ return CSI_SENSOR;
+ if (!strncmp(subdev->name, STF_DVP_NAME, strlen(STF_DVP_NAME)))
+ return DVP_SENSOR;
+ return -EINVAL;
+}
+
+static int isp_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ int ret = 0, interface_type;
+ struct v4l2_mbus_framefmt *fmt;
+ struct v4l2_event src_ch = { 0 };
+
+ 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);
+ if (!user_config_isp)
+ isp_dev->hw_ops->isp_config_set(isp_dev);
+ interface_type = isp_get_interface_type(&sd->entity);
+ if (interface_type < 0) {
+ st_err(ST_ISP, "%s, pipeline not config\n", __func__);
+ goto exit;
+ }
+ isp_dev->hw_ops->isp_set_format(isp_dev,
+ isp_dev->rect, fmt->code, interface_type);
+ isp_dev->hw_ops->isp_reset(isp_dev);
+ isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+ user_config_isp = 0;
+ }
+ 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--;
+ }
+ src_ch.type = V4L2_EVENT_SOURCE_CHANGE,
+ src_ch.u.src_change.changes = isp_dev->stream_count,
+
+ v4l2_subdev_notify_event(sd, &src_ch);
+exit:
+ mutex_unlock(&isp_dev->stream_lock);
+
+ mutex_lock(&isp_dev->power_lock);
+ /* restore controls */
+ if (enable && isp_dev->power_count == 1) {
+ mutex_unlock(&isp_dev->power_lock);
+ ret = v4l2_ctrl_handler_setup(&isp_dev->ctrls.handler);
+ } else
+ mutex_unlock(&isp_dev->power_lock);
+
+ return ret;
+}
+
+/*Try to match sensor format with sink, and then get the index as default.*/
+static int isp_match_sensor_format_get_index(struct stf_isp_dev *isp_dev)
+{
+ int ret, idx;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ struct v4l2_subdev_format fmt;
+ const struct isp_format_table *formats;
+
+ if (!isp_dev)
+ return -EINVAL;
+
+ sensor = stfcamss_find_sensor(&isp_dev->subdev.entity);
+ if (!sensor)
+ return -EINVAL;
+
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ st_debug(ST_ISP, "Found sensor = %s\n", sensor->name);
+
+ fmt.pad = 0;
+ fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+ if (ret) {
+ st_warn(ST_ISP, "Sonser get format failed !!\n");
+ return -EINVAL;
+ }
+
+ st_debug(ST_ISP, "Got sensor format 0x%x !!\n", fmt.format.code);
+
+ formats = &isp_dev->formats[0]; /* isp sink format */
+ for (idx = 0; idx < formats->nfmts; idx++) {
+ if (formats->fmts[idx].code == fmt.format.code) {
+ st_info(ST_ISP,
+ "Match sensor format to isp_formats_st7110_sink index %d !!\n",
+ idx);
+ return idx;
+ }
+ }
+ return -ERANGE;
+}
+
+static int isp_match_format_get_index(const struct isp_format_table *f_table,
+ __u32 mbus_code,
+ unsigned int pad)
+{
+ int i;
+
+ for (i = 0; i < f_table->nfmts; i++) {
+ if (mbus_code == f_table->fmts[i].code) {
+ break;
+ } else {
+ if (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y) {
+ if (mbus_code == (isp_formats_st7110_compat_10bit_raw[i].code ||
+ isp_formats_st7110_compat_8bit_raw[i].code))
+ break;
+ }
+ }
+ }
+
+ return i;
+}
+
+static void isp_try_format(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ const struct isp_format_table *formats;
+ unsigned int i;
+ u32 code = fmt->code;
+ u32 bpp;
+
+ if (pad == STF_ISP_PAD_SINK) {
+ /* Set format on sink pad */
+
+ formats = &isp_dev->formats[pad];
+ fmt->width = clamp_t(u32,
+ fmt->width, STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height, STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+ fmt->height &= ~0x1;
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+ } else {
+ formats = &isp_dev->formats[pad];
+ }
+
+ i = isp_match_format_get_index(formats, fmt->code, pad);
+ st_debug(ST_ISP, "%s pad=%d, code=%x isp_match_format_get_index = %d\n",
+ __func__, pad, code, i);
+
+ if (i >= formats->nfmts &&
+ (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y)) {
+ int sensor_idx;
+
+ sensor_idx = isp_match_sensor_format_get_index(isp_dev);
+ if (sensor_idx)
+ i = sensor_idx;
+ }
+
+ if (pad != STF_ISP_PAD_SINK)
+ *fmt = *__isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
+
+ if (i >= formats->nfmts) {
+ fmt->code = formats->fmts[0].code;
+ bpp = formats->fmts[0].bpp;
+ st_info(ST_ISP, "Use default index 0 format = 0x%x\n", fmt->code);
+ } else {
+ // sink format and raw format must one by one
+ if (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y) {
+ fmt->code = formats->fmts[i].code;
+ bpp = formats->fmts[i].bpp;
+ st_info(ST_ISP, "Use mapping format from sink index %d = 0x%x\n",
+ i, fmt->code);
+ } else {
+ fmt->code = code;
+ bpp = formats->fmts[i].bpp;
+ st_info(ST_ISP, "Use input format = 0x%x\n", fmt->code);
+ }
+ }
+
+ switch (pad) {
+ case STF_ISP_PAD_SINK:
+ break;
+ case STF_ISP_PAD_SRC:
+ isp_dev->rect[ISP_COMPOSE].bpp = bpp;
+ break;
+ case STF_ISP_PAD_SRC_SS0:
+ isp_dev->rect[ISP_SCALE_SS0].bpp = bpp;
+ break;
+ case STF_ISP_PAD_SRC_SS1:
+ isp_dev->rect[ISP_SCALE_SS1].bpp = bpp;
+ break;
+ case STF_ISP_PAD_SRC_ITIW:
+ case STF_ISP_PAD_SRC_ITIR:
+ isp_dev->rect[ISP_ITIWS].bpp = bpp;
+ break;
+ case STF_ISP_PAD_SRC_RAW:
+ isp_dev->rect[ISP_CROP].bpp = bpp;
+ break;
+ case STF_ISP_PAD_SRC_SCD_Y:
+ 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);
+ const struct isp_format_table *formats;
+
+ if (code->index >= isp_dev->formats[code->pad].nfmts)
+ return -EINVAL;
+
+ formats = &isp_dev->formats[code->pad];
+ code->code = formats->fmts[code->index].code;
+ 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, 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, 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, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+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;
+ struct v4l2_subdev_selection sel = { 0 };
+ struct v4l2_rect *rect = NULL;
+ int ret;
+
+ st_debug(ST_ISP, "%s pad=%d, code=%x, which=%d\n",
+ __func__, fmt->reserved[0], fmt->format.code, fmt->which);
+ format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ fmt->format = *format;
+ if (fmt->reserved[0] != 0) {
+ sel.which = fmt->which;
+ sel.pad = fmt->reserved[0];
+
+ switch (fmt->reserved[0]) {
+ case STF_ISP_PAD_SRC:
+ rect = __isp_get_compose(isp_dev, state, fmt->which);
+ break;
+ case STF_ISP_PAD_SRC_SS0:
+ case STF_ISP_PAD_SRC_SS1:
+ rect = __isp_get_scale(isp_dev, state, &sel);
+ break;
+ case STF_ISP_PAD_SRC_ITIW:
+ case STF_ISP_PAD_SRC_ITIR:
+ rect = __isp_get_itiws(isp_dev, state, fmt->which);
+ break;
+ case STF_ISP_PAD_SRC_RAW:
+ case STF_ISP_PAD_SRC_SCD_Y:
+ rect = __isp_get_crop(isp_dev, state, fmt->which);
+ break;
+ default:
+ break;
+ }
+ if (rect != NULL) {
+ fmt->format.width = rect->width;
+ fmt->format.height = rect->height;
+ }
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+ goto out;
+ } else {
+ isp_try_format(isp_dev, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_ISP_PAD_SINK) {
+ /* Reset sink pad compose selection */
+ sel.which = fmt->which;
+ sel.pad = STF_ISP_PAD_SINK;
+ sel.target = V4L2_SEL_TGT_CROP;
+ sel.r.width = fmt->format.width;
+ sel.r.height = fmt->format.height;
+ ret = isp_set_selection(sd, state, &sel);
+ if (ret < 0)
+ return ret;
+ }
+
+out:
+ 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->rect[ISP_COMPOSE].rect;
+}
+
+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_SINK);
+
+ return &isp_dev->rect[ISP_CROP].rect;
+}
+
+static struct v4l2_rect *
+__isp_get_scale(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ int pad;
+
+ if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
+ STF_ISP_PAD_SINK);
+ if (sel->pad != STF_ISP_PAD_SRC_SS0 && sel->pad != STF_ISP_PAD_SRC_SS1)
+ return NULL;
+
+ pad = sel->pad == STF_ISP_PAD_SRC_SS0 ? ISP_SCALE_SS0 : ISP_SCALE_SS1;
+ return &isp_dev->rect[pad].rect;
+}
+
+static struct v4l2_rect *
+__isp_get_itiws(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_SINK);
+
+ return &isp_dev->rect[ISP_ITIWS].rect;
+}
+
+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_mbus_framefmt *fmt;
+
+ fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
+
+ if (rect->width > fmt->width)
+ rect->width = fmt->width;
+
+ if (rect->width + rect->left > fmt->width)
+ rect->left = fmt->width - rect->width;
+
+ if (rect->height > fmt->height)
+ rect->height = fmt->height;
+
+ if (rect->height + rect->top > fmt->height)
+ rect->top = fmt->height - rect->height;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
+ rect->left = 0;
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+ }
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
+ rect->top = 0;
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ }
+ rect->height &= ~0x1;
+}
+
+static void isp_try_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_rect *crop;
+
+ crop = __isp_get_crop(isp_dev, state, which);
+
+ if (rect->width > crop->width)
+ rect->width = crop->width;
+
+ if (rect->height > crop->height)
+ rect->height = crop->height;
+
+ if (crop->width > rect->width * SCALER_RATIO_MAX)
+ rect->width = (crop->width + SCALER_RATIO_MAX - 1) /
+ SCALER_RATIO_MAX;
+
+ if (crop->height > rect->height * SCALER_RATIO_MAX)
+ rect->height = (crop->height + SCALER_RATIO_MAX - 1) /
+ SCALER_RATIO_MAX;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH)
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT)
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ rect->height &= ~0x1;
+}
+
+static void isp_try_scale(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;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
+ rect->left = 0;
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+ }
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
+ rect->top = 0;
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ }
+ rect->height &= ~0x1;
+}
+
+static void isp_try_itiws(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_rect *crop;
+
+ crop = __isp_get_crop(isp_dev, state, which);
+
+ if (rect->width > crop->width)
+ rect->width = crop->width;
+
+ if (rect->width + rect->left > crop->width)
+ rect->left = crop->width - rect->width;
+
+ if (rect->height > crop->height)
+ rect->height = crop->height;
+
+ if (rect->height + rect->top > crop->height)
+ rect->top = crop->height - rect->height;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
+ rect->left = 0;
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+ }
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
+ rect->top = 0;
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ }
+ rect->height &= ~0x1;
+}
+
+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_CROP_BOUNDS:
+ case V4L2_SEL_TGT_CROP_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_CROP:
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ sel->r = *rect;
+ break;
+ case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+ case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+ if (sel->pad > STF_ISP_PAD_SRC_ITIR)
+ return -EINVAL;
+ rect = __isp_get_crop(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_COMPOSE:
+ if (sel->pad > STF_ISP_PAD_SRC_ITIR)
+ return -EINVAL;
+ if (sel->pad == STF_ISP_PAD_SRC_SS0
+ || sel->pad == STF_ISP_PAD_SRC_SS1) {
+ rect = __isp_get_scale(isp_dev, state, sel);
+ if (rect == NULL)
+ return -EINVAL;
+ } else if (sel->pad == STF_ISP_PAD_SRC_ITIW
+ || sel->pad == STF_ISP_PAD_SRC_ITIR) {
+ rect = __isp_get_itiws(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+ } else {
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+ }
+ sel->r = *rect;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
+ __func__, sel->pad, 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 = 0;
+
+ if (sel->target == V4L2_SEL_TGT_COMPOSE &&
+ ((sel->pad == STF_ISP_PAD_SINK)
+ || (sel->pad == STF_ISP_PAD_SRC))) {
+ struct v4l2_subdev_format fmt = { 0 };
+ int i;
+
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_compose(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* 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);
+
+ /* Reset scale */
+ for (i = STF_ISP_PAD_SRC_SS0; i <= STF_ISP_PAD_SRC_ITIR; i++) {
+ struct v4l2_subdev_selection scale = { 0 };
+
+ scale.which = sel->which;
+ scale.target = V4L2_SEL_TGT_COMPOSE;
+ scale.r = *rect;
+ scale.pad = i;
+ ret = isp_set_selection(sd, state, &scale);
+ }
+ } else if (sel->target == V4L2_SEL_TGT_COMPOSE
+ && ((sel->pad == STF_ISP_PAD_SRC_SS0)
+ || (sel->pad == STF_ISP_PAD_SRC_SS1))) {
+ struct v4l2_subdev_format fmt = { 0 };
+
+ rect = __isp_get_scale(isp_dev, state, sel);
+ if (rect == NULL)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_scale(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Reset source pad format width and height */
+ fmt.which = sel->which;
+ fmt.pad = sel->pad;
+ 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 if (sel->target == V4L2_SEL_TGT_COMPOSE
+ && ((sel->pad == STF_ISP_PAD_SRC_ITIW)
+ || (sel->pad == STF_ISP_PAD_SRC_ITIR))) {
+ struct v4l2_subdev_format fmt = { 0 };
+
+ rect = __isp_get_itiws(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_itiws(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Reset source pad format width and height */
+ fmt.which = sel->which;
+ fmt.pad = sel->pad;
+ 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 if (sel->target == V4L2_SEL_TGT_CROP) {
+ struct v4l2_subdev_selection compose = { 0 };
+ int i;
+
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (rect == NULL)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_crop(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Reset source compose selection */
+ compose.which = sel->which;
+ compose.target = V4L2_SEL_TGT_COMPOSE;
+ compose.r.width = rect->width;
+ compose.r.height = rect->height;
+ compose.pad = STF_ISP_PAD_SINK;
+ ret = isp_set_selection(sd, state, &compose);
+
+ /* Reset source pad format width and height */
+ for (i = STF_ISP_PAD_SRC_RAW; i < STF_ISP_PAD_MAX; i++) {
+ struct v4l2_subdev_format fmt = { 0 };
+
+ fmt.which = sel->which;
+ fmt.pad = i;
+ 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, "%s pad = %d, left = %d, %d, %d, %d\n",
+ __func__, sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+out:
+ 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_pad_remote_pad_first(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 = (struct 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);
+ struct device *dev = isp_dev->stfcamss->dev;
+ 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;
+ }
+ case VIDIOC_STF_DMABUF_ALLOC:
+ case VIDIOC_STF_DMABUF_FREE:
+ ret = stf_dmabuf_ioctl(dev, cmd, arg);
+ break;
+ case VIDIOC_STFISP_GET_REG:
+ ret = isp_dev->hw_ops->isp_reg_read(isp_dev, arg);
+ break;
+ case VIDIOC_STFISP_SET_REG:
+ ret = isp_dev->hw_ops->isp_reg_write(isp_dev, arg);
+ break;
+ case VIDIOC_STFISP_SHADOW_LOCK:
+ if (atomic_add_unless(&isp_dev->shadow_count, 1, 1))
+ ret = 0;
+ else
+ ret = -EBUSY;
+ st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
+ break;
+ case VIDIOC_STFISP_SHADOW_UNLOCK:
+ if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
+ ret = -EINVAL;
+ else
+ ret = 0;
+ st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
+ break;
+ case VIDIOC_STFISP_SHADOW_UNLOCK_N_TRIGGER:
+ {
+ isp_dev->hw_ops->isp_shadow_trigger(isp_dev);
+ if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
+ ret = -EINVAL;
+ else
+ ret = 0;
+ st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
+ }
+ break;
+ case VIDIOC_STFISP_SET_USER_CONFIG_ISP:
+ st_debug(ST_ISP, "%s, %d set user_config_isp\n", __func__, __LINE__);
+ user_config_isp = 1;
+ break;
+ default:
+ break;
+ }
+ return ret;
+}
+
+int isp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+ st_debug(ST_ISP, "%s, %d\n", __func__, __LINE__);
+ while (atomic_dec_if_positive(&isp_dev->shadow_count) > 0)
+ st_warn(ST_ISP, "user not unlocked the shadow lock, driver unlock it!\n");
+
+ return 0;
+}
+
+static int stf_isp_subscribe_event(struct v4l2_subdev *sd,
+ struct v4l2_fh *fh,
+ struct v4l2_event_subscription *sub)
+{
+ switch (sub->type) {
+ case V4L2_EVENT_SOURCE_CHANGE:
+ return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+ case V4L2_EVENT_CTRL:
+ return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
+ default:
+ st_debug(ST_ISP, "unspport subscribe_event\n");
+ return -EINVAL;
+ }
+}
+
+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,
+ .subscribe_event = stf_isp_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,
+ .close = isp_close,
+};
+
+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 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, 0);
+ 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;
+ pads[STF_ISP_PAD_SRC_SS0].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_SS1].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_ITIW].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_ITIR].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_RAW].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_SCD_Y].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_PAD_MAX, 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;
+ }
+
+ 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->power_lock);
+ mutex_destroy(&isp_dev->setfile_lock);
+ return 0;
+}
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_isp.h
@@ -0,0 +1,222 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 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_NAME "stf_isp"
+#define STF_ISP_SETFILE "stf_isp0_fw.bin"
+
+#define ISP_SCD_BUFFER_SIZE (19 * 256 * 4) // align 128
+#define ISP_YHIST_BUFFER_SIZE (64 * 4)
+#define ISP_SCD_Y_BUFFER_SIZE (ISP_SCD_BUFFER_SIZE + ISP_YHIST_BUFFER_SIZE)
+#define ISP_RAW_DATA_BITS 12
+#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_SENSOR 0x00000014
+#define ISP_REG_DUMP_CFG_0 0x00000024
+#define ISP_REG_DUMP_CFG_1 0x00000028
+#define ISP_REG_SCD_CFG_0 0x00000098
+#define ISP_REG_SCD_CFG_1 0x0000009C
+#define ISP_REG_SC_CFG_1 0x000000BC
+#define ISP_REG_IESHD_ADDR 0x00000A50
+#define ISP_REG_SS0AY 0x00000A94
+#define ISP_REG_SS0AUV 0x00000A98
+#define ISP_REG_SS0S 0x00000A9C
+#define ISP_REG_SS0IW 0x00000AA8
+#define ISP_REG_SS1AY 0x00000AAC
+#define ISP_REG_SS1AUV 0x00000AB0
+#define ISP_REG_SS1S 0x00000AB4
+#define ISP_REG_SS1IW 0x00000AC0
+#define ISP_REG_YHIST_CFG_4 0x00000CD8
+#define ISP_REG_ITIIWSR 0x00000B20
+#define ISP_REG_ITIDWLSR 0x00000B24
+#define ISP_REG_ITIDWYSAR 0x00000B28
+#define ISP_REG_ITIDWUSAR 0x00000B2C
+#define ISP_REG_ITIDRYSAR 0x00000B30
+#define ISP_REG_ITIDRUSAR 0x00000B34
+#define ISP_REG_ITIPDFR 0x00000B38
+#define ISP_REG_ITIDRLSR 0x00000B3C
+#define ISP_REG_ITIBSR 0x00000B40
+#define ISP_REG_ITIAIR 0x00000B44
+#define ISP_REG_ITIDPSR 0x00000B48
+
+/* The output line of a isp controller */
+enum isp_line_id {
+ STF_ISP_LINE_INVALID = -1,
+ STF_ISP_LINE_SRC = 1,
+ STF_ISP_LINE_SRC_SS0 = 2,
+ STF_ISP_LINE_SRC_SS1 = 3,
+ STF_ISP_LINE_SRC_ITIW = 4,
+ STF_ISP_LINE_SRC_ITIR = 5,
+ STF_ISP_LINE_SRC_RAW = 6,
+ STF_ISP_LINE_SRC_SCD_Y = 7,
+ STF_ISP_LINE_MAX = STF_ISP_LINE_SRC_SCD_Y
+};
+
+/* pad id for media framework */
+enum isp_pad_id {
+ STF_ISP_PAD_SINK = 0,
+ STF_ISP_PAD_SRC = 1,
+ STF_ISP_PAD_SRC_SS0 = 2,
+ STF_ISP_PAD_SRC_SS1 = 3,
+ STF_ISP_PAD_SRC_ITIW = 4,
+ STF_ISP_PAD_SRC_ITIR = 5,
+ STF_ISP_PAD_SRC_RAW = 6,
+ STF_ISP_PAD_SRC_SCD_Y = 7,
+ STF_ISP_PAD_MAX = 8
+};
+
+enum {
+ EN_INT_NONE = 0,
+ EN_INT_ISP_DONE = (0x1 << 24),
+ EN_INT_CSI_DONE = (0x1 << 25),
+ EN_INT_SC_DONE = (0x1 << 26),
+ EN_INT_LINE_INT = (0x1 << 27),
+ EN_INT_ALL = (0xF << 24),
+};
+
+enum {
+ DVP_SENSOR = 0,
+ CSI_SENSOR,
+};
+
+#define ISP_AWB_OECF_SKIP_FRAME 0
+// 0x0BC [31:30] SEL - sc0 input mux for sc awb
+// 00 : after DEC, 01 : after OBC, 10 : after OECF, 11 : after AWB
+enum scd_type {
+ DEC_TYPE = 0,
+ OBC_TYPE,
+ OECF_TYPE,
+ AWB_TYPE
+};
+
+struct isp_format {
+ u32 code;
+ u8 bpp;
+};
+
+struct isp_format_table {
+ const struct isp_format *fmts;
+ int nfmts;
+};
+
+struct regval_t {
+ u32 addr;
+ u32 val;
+ u32 mask;
+ u32 delay_ms;
+};
+
+struct reg_table {
+ struct regval_t *regval;
+ int regval_num;
+};
+
+struct isp_stream_format {
+ struct v4l2_rect rect;
+ u32 bpp;
+};
+
+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 isp_stream_format *crop, u32 mcode,
+ int type);
+ // u32 width, u32 height);
+ int (*isp_stream_set)(struct stf_isp_dev *isp_dev, int on);
+ int (*isp_reg_read)(struct stf_isp_dev *isp_dev, void *arg);
+ int (*isp_reg_write)(struct stf_isp_dev *isp_dev, void *arg);
+ int (*isp_shadow_trigger)(struct stf_isp_dev *isp_dev);
+};
+
+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;
+};
+
+enum {
+ ISP_CROP = 0,
+ ISP_COMPOSE,
+ ISP_SCALE_SS0,
+ ISP_SCALE_SS1,
+ ISP_ITIWS,
+ ISP_RECT_MAX
+};
+
+struct stf_isp_dev {
+ enum subdev_type sdev_type; // must be frist
+ struct stfcamss *stfcamss;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_ISP_PAD_MAX];
+ struct v4l2_mbus_framefmt fmt[STF_ISP_PAD_MAX];
+ struct isp_stream_format rect[ISP_RECT_MAX];
+ const struct isp_format_table *formats;
+ unsigned int nformats;
+ struct isp_hw_ops *hw_ops;
+ struct mutex power_lock;
+ int power_count;
+ struct mutex stream_lock;
+ int stream_count;
+ atomic_t shadow_count;
+
+ struct isp_ctrls ctrls;
+ struct mutex setfile_lock;
+ struct isp_setfile setfile;
+ struct reg_table *context_regs;
+};
+
+extern int stf_isp_subdev_init(struct stfcamss *stfcamss);
+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);
+
+#endif /* STF_ISP_H */
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_isp_hw_ops.c
@@ -0,0 +1,1550 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#include "stfcamss.h"
+#include <linux/io.h>
+#include <linux/fb.h>
+#include <linux/module.h>
+#include <video/stf-vin.h>
+#include "stf_isp_ioctl.h"
+#include "stf_isp.h"
+#include <linux/delay.h>
+#include <linux/clk.h>
+#define USE_NEW_CONFIG_SETTING
+
+static struct regval_t isp_reg_init_config_list[] = {
+ /* config DC(0040H~0044H) */
+ {0x00000044, 0x00000000, 0, 0},
+ /* config DEC(0030H) */
+ {0x00000030, 0x00000000, 0, 0},
+ /* config OBC(0034H, 02E0H~02FCH) */
+ {0x00000034, 0x000000BB, 0, 0},
+ {0x000002E0, 0x40404040, 0, 0},
+ {0x000002E4, 0x40404040, 0, 0},
+ {0x000002E8, 0x40404040, 0, 0},
+ {0x000002EC, 0x40404040, 0, 0},
+ {0x000002F0, 0x00000000, 0, 0},
+ {0x000002F4, 0x00000000, 0, 0},
+ {0x000002F8, 0x00000000, 0, 0},
+ {0x000002FC, 0x00000000, 0, 0},
+ /* config LCBQ(0074H, 007CH, 0300H~039FH, and 0400H~049FH) */
+ {0x00000074, 0x00009900, 0, 0},
+ {0x0000007C, 0x01E40040, 0, 0},
+ {0x00000300, 0x01000100, 0, 0},
+ {0x00000304, 0x01000100, 0, 0},
+ {0x00000308, 0x01000100, 0, 0},
+ {0x0000030C, 0x01000100, 0, 0},
+ {0x00000310, 0x01000100, 0, 0},
+ {0x00000314, 0x01000100, 0, 0},
+ {0x00000318, 0x01000100, 0, 0},
+ {0x0000031C, 0x01000100, 0, 0},
+ {0x00000320, 0x01000100, 0, 0},
+ {0x00000324, 0x01000100, 0, 0},
+ {0x00000328, 0x01000100, 0, 0},
+ {0x0000032C, 0x01000100, 0, 0},
+ {0x00000330, 0x00000100, 0, 0},
+ {0x00000334, 0x01000100, 0, 0},
+ {0x00000338, 0x01000100, 0, 0},
+ {0x0000033C, 0x01000100, 0, 0},
+ {0x00000340, 0x01000100, 0, 0},
+ {0x00000344, 0x01000100, 0, 0},
+ {0x00000348, 0x01000100, 0, 0},
+ {0x0000034C, 0x01000100, 0, 0},
+ {0x00000350, 0x01000100, 0, 0},
+ {0x00000354, 0x01000100, 0, 0},
+ {0x00000358, 0x01000100, 0, 0},
+ {0x0000035C, 0x01000100, 0, 0},
+ {0x00000360, 0x01000100, 0, 0},
+ {0x00000364, 0x00000100, 0, 0},
+ {0x00000368, 0x01000100, 0, 0},
+ {0x0000036C, 0x01000100, 0, 0},
+ {0x00000370, 0x01000100, 0, 0},
+ {0x00000374, 0x01000100, 0, 0},
+ {0x00000378, 0x01000100, 0, 0},
+ {0x0000037C, 0x01000100, 0, 0},
+ {0x00000380, 0x01000100, 0, 0},
+ {0x00000384, 0x01000100, 0, 0},
+ {0x00000388, 0x01000100, 0, 0},
+ {0x0000038C, 0x01000100, 0, 0},
+ {0x00000390, 0x01000100, 0, 0},
+ {0x00000394, 0x01000100, 0, 0},
+ {0x00000398, 0x00000100, 0, 0},
+ {0x0000039C, 0x01000100, 0, 0},
+ {0x000003A0, 0x01000100, 0, 0},
+ {0x000003A4, 0x01000100, 0, 0},
+ {0x000003A8, 0x01000100, 0, 0},
+ {0x000003AC, 0x01000100, 0, 0},
+ {0x000003B0, 0x01000100, 0, 0},
+ {0x000003B4, 0x01000100, 0, 0},
+ {0x000003B8, 0x01000100, 0, 0},
+ {0x000003BC, 0x01000100, 0, 0},
+ {0x000003C0, 0x01000100, 0, 0},
+ {0x000003C4, 0x01000100, 0, 0},
+ {0x000003C8, 0x01000100, 0, 0},
+ {0x000003CC, 0x00000100, 0, 0},
+ {0x00000400, 0x00000000, 0, 0},
+ {0x00000404, 0x00000000, 0, 0},
+ {0x00000408, 0x00000000, 0, 0},
+ {0x0000040C, 0x00000000, 0, 0},
+ {0x00000410, 0x00000000, 0, 0},
+ {0x00000414, 0x00000000, 0, 0},
+ {0x00000418, 0x00000000, 0, 0},
+ {0x0000041C, 0x00000000, 0, 0},
+ {0x00000420, 0x00000000, 0, 0},
+ {0x00000424, 0x00000000, 0, 0},
+ {0x00000428, 0x00000000, 0, 0},
+ {0x0000042C, 0x00000000, 0, 0},
+ {0x00000430, 0x00000000, 0, 0},
+ {0x00000434, 0x00000000, 0, 0},
+ {0x00000438, 0x00000000, 0, 0},
+ {0x0000043C, 0x00000000, 0, 0},
+ {0x00000440, 0x00000000, 0, 0},
+ {0x00000444, 0x00000000, 0, 0},
+ {0x00000448, 0x00000000, 0, 0},
+ {0x0000044C, 0x00000000, 0, 0},
+ {0x00000450, 0x00000000, 0, 0},
+ {0x00000454, 0x00000000, 0, 0},
+ {0x00000458, 0x00000000, 0, 0},
+ {0x0000045C, 0x00000000, 0, 0},
+ {0x00000460, 0x00000000, 0, 0},
+ {0x00000464, 0x00000000, 0, 0},
+ {0x00000468, 0x00000000, 0, 0},
+ {0x0000046C, 0x00000000, 0, 0},
+ {0x00000470, 0x00000000, 0, 0},
+ {0x00000474, 0x00000000, 0, 0},
+ {0x00000478, 0x00000000, 0, 0},
+ {0x0000047C, 0x00000000, 0, 0},
+ {0x00000480, 0x00000000, 0, 0},
+ {0x00000484, 0x00000000, 0, 0},
+ {0x00000488, 0x00000000, 0, 0},
+ {0x0000048C, 0x00000000, 0, 0},
+ {0x00000490, 0x00000000, 0, 0},
+ {0x00000494, 0x00000000, 0, 0},
+ {0x00000498, 0x00000000, 0, 0},
+ {0x0000049C, 0x00000000, 0, 0},
+ {0x000004A0, 0x00000000, 0, 0},
+ {0x000004A4, 0x00000000, 0, 0},
+ {0x000004A8, 0x00000000, 0, 0},
+ {0x000004AC, 0x00000000, 0, 0},
+ {0x000004B0, 0x00000000, 0, 0},
+ {0x000004B4, 0x00000000, 0, 0},
+ {0x000004B8, 0x00000000, 0, 0},
+ {0x000004BC, 0x00000000, 0, 0},
+ {0x000004C0, 0x00000000, 0, 0},
+ {0x000004C4, 0x00000000, 0, 0},
+ {0x000004C8, 0x00000000, 0, 0},
+ {0x000004CC, 0x00000000, 0, 0},
+ /* config OECF(0100H~027CH) */
+ {0x00000100, 0x00100000, 0, 0},
+ {0x00000104, 0x00400020, 0, 0},
+ {0x00000108, 0x00800060, 0, 0},
+ {0x0000010C, 0x00C000A0, 0, 0},
+ {0x00000110, 0x010000E0, 0, 0},
+ {0x00000114, 0x02000180, 0, 0},
+ {0x00000118, 0x03000280, 0, 0},
+ {0x0000011C, 0x03FE0380, 0, 0},
+ {0x00000120, 0x00100000, 0, 0},
+ {0x00000124, 0x00400020, 0, 0},
+ {0x00000128, 0x00800060, 0, 0},
+ {0x0000012C, 0x00C000A0, 0, 0},
+ {0x00000130, 0x010000E0, 0, 0},
+ {0x00000134, 0x02000180, 0, 0},
+ {0x00000138, 0x03000280, 0, 0},
+ {0x0000013C, 0x03FE0380, 0, 0},
+ {0x00000140, 0x00100000, 0, 0},
+ {0x00000144, 0x00400020, 0, 0},
+ {0x00000148, 0x00800060, 0, 0},
+ {0x0000014C, 0x00C000A0, 0, 0},
+ {0x00000150, 0x010000E0, 0, 0},
+ {0x00000154, 0x02000180, 0, 0},
+ {0x00000158, 0x03000280, 0, 0},
+ {0x0000015C, 0x03FE0380, 0, 0},
+ {0x00000160, 0x00100000, 0, 0},
+ {0x00000164, 0x00400020, 0, 0},
+ {0x00000168, 0x00800060, 0, 0},
+ {0x0000016C, 0x00C000A0, 0, 0},
+ {0x00000170, 0x010000E0, 0, 0},
+ {0x00000174, 0x02000180, 0, 0},
+ {0x00000178, 0x03000280, 0, 0},
+ {0x0000017C, 0x03FE0380, 0, 0},
+ {0x00000180, 0x00100000, 0, 0},
+ {0x00000184, 0x00400020, 0, 0},
+ {0x00000188, 0x00800060, 0, 0},
+ {0x0000018C, 0x00C000A0, 0, 0},
+ {0x00000190, 0x010000E0, 0, 0},
+ {0x00000194, 0x02000180, 0, 0},
+ {0x00000198, 0x03000280, 0, 0},
+ {0x0000019C, 0x03FE0380, 0, 0},
+ {0x000001A0, 0x00100000, 0, 0},
+ {0x000001A4, 0x00400020, 0, 0},
+ {0x000001A8, 0x00800060, 0, 0},
+ {0x000001AC, 0x00C000A0, 0, 0},
+ {0x000001B0, 0x010000E0, 0, 0},
+ {0x000001B4, 0x02000180, 0, 0},
+ {0x000001B8, 0x03000280, 0, 0},
+ {0x000001BC, 0x03FE0380, 0, 0},
+ {0x000001C0, 0x00100000, 0, 0},
+ {0x000001C4, 0x00400020, 0, 0},
+ {0x000001C8, 0x00800060, 0, 0},
+ {0x000001CC, 0x00C000A0, 0, 0},
+ {0x000001D0, 0x010000E0, 0, 0},
+ {0x000001D4, 0x02000180, 0, 0},
+ {0x000001D8, 0x03000280, 0, 0},
+ {0x000001DC, 0x03FE0380, 0, 0},
+ {0x000001E0, 0x00100000, 0, 0},
+ {0x000001E4, 0x00400020, 0, 0},
+ {0x000001E8, 0x00800060, 0, 0},
+ {0x000001EC, 0x00C000A0, 0, 0},
+ {0x000001F0, 0x010000E0, 0, 0},
+ {0x000001F4, 0x02000180, 0, 0},
+ {0x000001F8, 0x03000280, 0, 0},
+ {0x000001FC, 0x03FE0380, 0, 0},
+ {0x00000200, 0x00800080, 0, 0},
+ {0x00000204, 0x00800080, 0, 0},
+ {0x00000208, 0x00800080, 0, 0},
+ {0x0000020C, 0x00800080, 0, 0},
+ {0x00000210, 0x00800080, 0, 0},
+ {0x00000214, 0x00800080, 0, 0},
+ {0x00000218, 0x00800080, 0, 0},
+ {0x0000021C, 0x00800080, 0, 0},
+ {0x00000220, 0x00800080, 0, 0},
+ {0x00000224, 0x00800080, 0, 0},
+ {0x00000228, 0x00800080, 0, 0},
+ {0x0000022C, 0x00800080, 0, 0},
+ {0x00000230, 0x00800080, 0, 0},
+ {0x00000234, 0x00800080, 0, 0},
+ {0x00000238, 0x00800080, 0, 0},
+ {0x0000023C, 0x00800080, 0, 0},
+ {0x00000240, 0x00800080, 0, 0},
+ {0x00000244, 0x00800080, 0, 0},
+ {0x00000248, 0x00800080, 0, 0},
+ {0x0000024C, 0x00800080, 0, 0},
+ {0x00000250, 0x00800080, 0, 0},
+ {0x00000254, 0x00800080, 0, 0},
+ {0x00000258, 0x00800080, 0, 0},
+ {0x0000025C, 0x00800080, 0, 0},
+ {0x00000260, 0x00800080, 0, 0},
+ {0x00000264, 0x00800080, 0, 0},
+ {0x00000268, 0x00800080, 0, 0},
+ {0x0000026C, 0x00800080, 0, 0},
+ {0x00000270, 0x00800080, 0, 0},
+ {0x00000274, 0x00800080, 0, 0},
+ {0x00000278, 0x00800080, 0, 0},
+ {0x0000027C, 0x00800080, 0, 0},
+ /* config OECFHM(03D0H~03E4H) */
+ {0x000003D0, 0x04000000, 0, 0},
+ {0x000003D4, 0x0C000800, 0, 0},
+ {0x000003D8, 0x00000FFF, 0, 0},
+ {0x000003DC, 0x08000800, 0, 0},
+ {0x000003E0, 0x08000800, 0, 0},
+ {0x000003E4, 0x00000800, 0, 0},
+ /* config LCCF(0050H, 0058H, 00E0H~00ECH) */
+ {0x00000050, 0x021C03C0, 0, 0},
+ {0x00000058, 0x0000000B, 0, 0},
+ {0x000000E0, 0x00000000, 0, 0},
+ {0x000000E4, 0x00000000, 0, 0},
+ {0x000000E8, 0x00000000, 0, 0},
+ {0x000000EC, 0x00000000, 0, 0},
+ /* config AWB(0280H~02DCH) */
+ {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, 0x00800080, 0, 0},
+ {0x000002C4, 0x00800080, 0, 0},
+ {0x000002C8, 0x00800080, 0, 0},
+ {0x000002CC, 0x00800080, 0, 0},
+ {0x000002D0, 0x00800080, 0, 0},
+ {0x000002D4, 0x00800080, 0, 0},
+ {0x000002D8, 0x00800080, 0, 0},
+ {0x000002DC, 0x00800080, 0, 0},
+ /* config CTC(0A10H) and DBC(0A14H) filter */
+ {0x00000A10, 0x41400040, 0, 0},
+ {0x00000A14, 0x02000200, 0, 0},
+ /* config CFA(0018H, 0A1CH) */
+ {0x00000018, 0x000011BB, 0, 0},
+ {0x00000A1C, 0x00000032, 0, 0},
+ /* config CCM(0C40H~0CA4H) */
+ {0x00000C40, 0x00060000, 0, 0},
+ {0x00000C44, 0x00000000, 0, 0},
+ {0x00000C48, 0x00000000, 0, 0},
+ {0x00000C4C, 0x00000000, 0, 0},
+ {0x00000C50, 0x00000000, 0, 0},
+ {0x00000C54, 0x00000000, 0, 0},
+ {0x00000C58, 0x00000000, 0, 0},
+ {0x00000C5C, 0x00000000, 0, 0},
+ {0x00000C60, 0x00000000, 0, 0},
+ {0x00000C64, 0x00000000, 0, 0},
+ {0x00000C68, 0x00000000, 0, 0},
+ {0x00000C6C, 0x00000000, 0, 0},
+ {0x00000C70, 0x00000080, 0, 0},
+ {0x00000C74, 0x00000000, 0, 0},
+ {0x00000C78, 0x00000000, 0, 0},
+ {0x00000C7C, 0x00000000, 0, 0},
+ {0x00000C80, 0x00000080, 0, 0},
+ {0x00000C84, 0x00000000, 0, 0},
+ {0x00000C88, 0x00000000, 0, 0},
+ {0x00000C8C, 0x00000000, 0, 0},
+ {0x00000C90, 0x00000080, 0, 0},
+ {0x00000C94, 0x00000000, 0, 0},
+ {0x00000C98, 0x00000000, 0, 0},
+ {0x00000C9C, 0x00000000, 0, 0},
+ {0x00000CA0, 0x00000700, 0, 0},
+ {0x00000CA4, 0x00000200, 0, 0},
+ /* config GMARGB(0E00H~0E38H) */
+ {0x00000E00, 0x24000000, 0, 0},
+ {0x00000E04, 0x08000020, 0, 0},
+ {0x00000E08, 0x08000040, 0, 0},
+ {0x00000E0C, 0x08000060, 0, 0},
+ {0x00000E10, 0x08000080, 0, 0},
+ {0x00000E14, 0x080000A0, 0, 0},
+ {0x00000E18, 0x080000C0, 0, 0},
+ {0x00000E1C, 0x080000E0, 0, 0},
+ {0x00000E20, 0x08000100, 0, 0},
+ {0x00000E24, 0x08000180, 0, 0},
+ {0x00000E28, 0x08000200, 0, 0},
+ {0x00000E2C, 0x08000280, 0, 0},
+ {0x00000E30, 0x08000300, 0, 0},
+ {0x00000E34, 0x08000380, 0, 0},
+ {0x00000E38, 0x080003FE, 0, 0},
+ /* config R2Y(0E40H~0E60H) */
+ {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},
+ /* config YCRV(0F00H~0FFCH) */
+ {0x00000F00, 0x00000000, 0, 0},
+ {0x00000F04, 0x00000010, 0, 0},
+ {0x00000F08, 0x00000020, 0, 0},
+ {0x00000F0C, 0x00000030, 0, 0},
+ {0x00000F10, 0x00000040, 0, 0},
+ {0x00000F14, 0x00000050, 0, 0},
+ {0x00000F18, 0x00000060, 0, 0},
+ {0x00000F1C, 0x00000070, 0, 0},
+ {0x00000F20, 0x00000080, 0, 0},
+ {0x00000F24, 0x00000090, 0, 0},
+ {0x00000F28, 0x000000A0, 0, 0},
+ {0x00000F2C, 0x000000B0, 0, 0},
+ {0x00000F30, 0x000000C0, 0, 0},
+ {0x00000F34, 0x000000D0, 0, 0},
+ {0x00000F38, 0x000000E0, 0, 0},
+ {0x00000F3C, 0x000000F0, 0, 0},
+ {0x00000F40, 0x00000100, 0, 0},
+ {0x00000F44, 0x00000110, 0, 0},
+ {0x00000F48, 0x00000120, 0, 0},
+ {0x00000F4C, 0x00000130, 0, 0},
+ {0x00000F50, 0x00000140, 0, 0},
+ {0x00000F54, 0x00000150, 0, 0},
+ {0x00000F58, 0x00000160, 0, 0},
+ {0x00000F5C, 0x00000170, 0, 0},
+ {0x00000F60, 0x00000180, 0, 0},
+ {0x00000F64, 0x00000190, 0, 0},
+ {0x00000F68, 0x000001A0, 0, 0},
+ {0x00000F6C, 0x000001B0, 0, 0},
+ {0x00000F70, 0x000001C0, 0, 0},
+ {0x00000F74, 0x000001D0, 0, 0},
+ {0x00000F78, 0x000001E0, 0, 0},
+ {0x00000F7C, 0x000001F0, 0, 0},
+ {0x00000F80, 0x00000200, 0, 0},
+ {0x00000F84, 0x00000210, 0, 0},
+ {0x00000F88, 0x00000220, 0, 0},
+ {0x00000F8C, 0x00000230, 0, 0},
+ {0x00000F90, 0x00000240, 0, 0},
+ {0x00000F94, 0x00000250, 0, 0},
+ {0x00000F98, 0x00000260, 0, 0},
+ {0x00000F9C, 0x00000270, 0, 0},
+ {0x00000FA0, 0x00000280, 0, 0},
+ {0x00000FA4, 0x00000290, 0, 0},
+ {0x00000FA8, 0x000002A0, 0, 0},
+ {0x00000FAC, 0x000002B0, 0, 0},
+ {0x00000FB0, 0x000002C0, 0, 0},
+ {0x00000FB4, 0x000002D0, 0, 0},
+ {0x00000FB8, 0x000002E0, 0, 0},
+ {0x00000FBC, 0x000002F0, 0, 0},
+ {0x00000FC0, 0x00000300, 0, 0},
+ {0x00000FC4, 0x00000310, 0, 0},
+ {0x00000FC8, 0x00000320, 0, 0},
+ {0x00000FCC, 0x00000330, 0, 0},
+ {0x00000FD0, 0x00000340, 0, 0},
+ {0x00000FD4, 0x00000350, 0, 0},
+ {0x00000FD8, 0x00000360, 0, 0},
+ {0x00000FDC, 0x00000370, 0, 0},
+ {0x00000FE0, 0x00000380, 0, 0},
+ {0x00000FE4, 0x00000390, 0, 0},
+ {0x00000FE8, 0x000003A0, 0, 0},
+ {0x00000FEC, 0x000003B0, 0, 0},
+ {0x00000FF0, 0x000003C0, 0, 0},
+ {0x00000FF4, 0x000003D0, 0, 0},
+ {0x00000FF8, 0x000003E0, 0, 0},
+ {0x00000FFC, 0x000003F0, 0, 0},
+ /* config Shrp(0E80H~0EE8H) */
+ {0x00000E80, 0x00070F00, 0, 0},
+ {0x00000E84, 0x00180F00, 0, 0},
+ {0x00000E88, 0x00800F00, 0, 0},
+ {0x00000E8C, 0x01000F00, 0, 0},
+ {0x00000E90, 0x00100F00, 0, 0},
+ {0x00000E94, 0x00600F00, 0, 0},
+ {0x00000E98, 0x01000F00, 0, 0},
+ {0x00000E9C, 0x01900F00, 0, 0},
+ {0x00000EA0, 0x00000F00, 0, 0},
+ {0x00000EA4, 0x00000F00, 0, 0},
+ {0x00000EA8, 0x00000F00, 0, 0},
+ {0x00000EAC, 0x00000F00, 0, 0},
+ {0x00000EB0, 0x00000F00, 0, 0},
+ {0x00000EB4, 0x00000F00, 0, 0},
+ {0x00000EB8, 0x00000F00, 0, 0},
+ {0x00000EBC, 0x10000000, 0, 0},
+ {0x00000EC0, 0x10000000, 0, 0},
+ {0x00000EC4, 0x10000000, 0, 0},
+ {0x00000EC8, 0x10000000, 0, 0},
+ {0x00000ECC, 0x10000000, 0, 0},
+ {0x00000ED0, 0x10000000, 0, 0},
+ {0x00000ED4, 0x88000D7C, 0, 0},
+ {0x00000ED8, 0x00C00040, 0, 0},
+ {0x00000EDC, 0xFF000000, 0, 0},
+ {0x00000EE0, 0x00A00040, 0, 0},
+ {0x00000EE4, 0x00000000, 0, 0},
+ {0x00000EE8, 0x00000000, 0, 0},
+ /* config DNYUV(0C00H~0C24H) */
+ {0x00000C00, 0x00777777, 0, 0},
+ {0x00000C04, 0x00007777, 0, 0},
+ {0x00000C08, 0x00777777, 0, 0},
+ {0x00000C0C, 0x00007777, 0, 0},
+ {0x00000C10, 0x00600040, 0, 0},
+ {0x00000C14, 0x00D80090, 0, 0},
+ {0x00000C18, 0x01E60144, 0, 0},
+ {0x00000C1C, 0x00600040, 0, 0},
+ {0x00000C20, 0x00D80090, 0, 0},
+ {0x00000C24, 0x01E60144, 0, 0},
+ /* config SAT(0A30H~0A40H, 0A54H~0A58H) */
+ {0x00000A30, 0x00000100, 0, 0},
+ {0x00000A34, 0x001F0001, 0, 0},
+ {0x00000A38, 0x00000000, 0, 0},
+ {0x00000A3C, 0x00000100, 0, 0},
+ {0x00000A40, 0x00000008, 0, 0},
+ {0x00000A54, 0x04010001, 0, 0},
+ {0x00000A58, 0x03FF0001, 0, 0},
+ /* config OBA(0090H~0094H) */
+ {0x00000090, 0x04380000, 0, 0},
+ {0x00000094, 0x04390780, 0, 0},
+ /* config SC(0098H~009CH, 00B8H~00BCH,
+ * 00C0H, 0C4H~0D4H, 04D0H~054CH, 5D0H~5D4H)
+ */
+ {0x0000009C, 0x01000000, 0, 0},
+ {0x000000B8, 0x000C0000, 0, 0},
+ {0x000000BC, 0xC010151D, 0, 0},
+ {0x000000C0, 0x01F1BF08, 0, 0},
+ {0x000000C4, 0xFF00FF00, 0, 0},
+ {0x000000C8, 0xFF00FF00, 0, 0},
+ {0x000000CC, 0xFFFF0000, 0, 0},
+ {0x000000D0, 0xFFFF0000, 0, 0},
+ {0x000000D4, 0xFFFF0000, 0, 0},
+ {0x000000D8, 0x01050107, 0, 0},
+ {0x000004D0, 0x00000000, 0, 0},
+ {0x000004D4, 0x00000000, 0, 0},
+ {0x000004D8, 0x00000000, 0, 0},
+ {0x000004DC, 0x00000000, 0, 0},
+ {0x000004E0, 0x00000000, 0, 0},
+ {0x000004E4, 0x00000000, 0, 0},
+ {0x000004E8, 0x00000000, 0, 0},
+ {0x000004EC, 0x00000000, 0, 0},
+ {0x000004F0, 0x00100000, 0, 0},
+ {0x000004F4, 0x00000000, 0, 0},
+ {0x000004F8, 0x03D20000, 0, 0},
+ {0x000004FC, 0x00000000, 0, 0},
+ {0x00000500, 0x00950000, 0, 0},
+ {0x00000504, 0x00000000, 0, 0},
+ {0x00000508, 0x00253000, 0, 0},
+ {0x0000050C, 0x00000000, 0, 0},
+ {0x00000510, 0x00000000, 0, 0},
+ {0x00000514, 0x00000000, 0, 0},
+ {0x00000518, 0x00000000, 0, 0},
+ {0x0000051C, 0x00000000, 0, 0},
+ {0x00000520, 0x00000000, 0, 0},
+ {0x00000524, 0x00000000, 0, 0},
+ {0x00000528, 0x00000000, 0, 0},
+ {0x0000052C, 0x00000000, 0, 0},
+ {0x00000530, 0x00000000, 0, 0},
+ {0x00000534, 0x00000000, 0, 0},
+ {0x00000538, 0xFFFFFFF0, 0, 0},
+ {0x0000053C, 0x8FFFFFFF, 0, 0},
+ {0x00000540, 0x0000001E, 0, 0},
+ {0x00000544, 0x00000000, 0, 0},
+ {0x00000548, 0x00000000, 0, 0},
+ {0x0000054C, 0xF0F20000, 0, 0},
+ {0x000005D0, 0xFF00FF00, 0, 0},
+ {0x000005D4, 0xFF00FF00, 0, 0},
+ /* config YHIST(0CC8H~0CD8H) */
+ {0x00000CC8, 0x00000000, 0, 0},
+ {0x00000CCC, 0x0437077F, 0, 0},
+ {0x00000CD0, 0x00010002, 0, 0},
+ {0x00000CD4, 0x00000000, 0, 0},
+ /* config CBAR(0600H-0653H) */
+ {0x00000600, 0x043E0782, 0, 0},
+ {0x00000604, 0x00000000, 0, 0},
+ {0x00000608, 0x0437077F, 0, 0},
+ {0x0000060C, 0x00443150, 0, 0},
+ {0x00000610, 0x00000000, 0, 0},
+ {0x00000614, 0x08880888, 0, 0},
+ {0x00000618, 0x02220222, 0, 0},
+ {0x0000061C, 0x04440444, 0, 0},
+ {0x00000620, 0x08880888, 0, 0},
+ {0x00000624, 0x0AAA0AAA, 0, 0},
+ {0x00000628, 0x0CCC0CCC, 0, 0},
+ {0x0000062C, 0x0EEE0EEE, 0, 0},
+ {0x00000630, 0x0FFF0FFF, 0, 0},
+ {0x00000634, 0x08880888, 0, 0},
+ {0x00000638, 0x02220222, 0, 0},
+ {0x0000063C, 0x04440444, 0, 0},
+ {0x00000640, 0x08880888, 0, 0},
+ {0x00000644, 0x0AAA0AAA, 0, 0},
+ {0x00000648, 0x0CCC0CCC, 0, 0},
+ {0x0000064C, 0x0EEE0EEE, 0, 0},
+ {0x00000650, 0x0FFF0FFF, 0, 0},
+ /* config sensor(0014H) */
+ {0x00000014, 0x0000000c, 0, 0},
+ /* config CROP(001CH, 0020H) */
+ {0x0000001C, 0x00000000, 0, 0},
+ {0x00000020, 0x0437077F, 0, 0},
+ /* config isp pileline X/Y size(A0CH) */
+ {0x00000A0C, 0x04380780, 0, 0},
+ /* config CSI dump (24H/28H) */
+ {0x00000028, 0x00030B80, 0, 0},
+ /* Video Output */
+ /* config UO(0A80H~0A90H) */
+ {0x00000A88, 0x00000780, 0, 0},
+ /* NV12 */
+ {0x00000A8C, 0x00000000, 0, 0},
+ /* NV21
+ *{0x00000A8C, 0x00000020, 0, 0},
+ */
+ {0x00000A90, 0x00000000, 0, 0},
+ {0x00000A9C, 0x00000780, 0, 0},
+ {0x00000AA0, 0x00000002, 0, 0},
+ {0x00000AA4, 0x00000002, 0, 0},
+ {0x00000AA8, 0x07800438, 0, 0},
+ {0x00000AB4, 0x00000780, 0, 0},
+ {0x00000AB8, 0x00000002, 0, 0},
+ {0x00000ABC, 0x00000002, 0, 0},
+ {0x00000AC0, 0x07800438, 0, 0},
+ {0x00000AC4, 0x00000000, 0, 0},
+ /* config TIL(0B20H~0B48H) */
+ {0x00000B20, 0x04380780, 0, 0},
+ {0x00000B24, 0x00000960, 0, 0},
+ {0x00000B38, 0x00030003, 0, 0},
+ {0x00000B3C, 0x00000960, 0, 0},
+ {0x00000B44, 0x00000000, 0, 0},
+ {0x00000B48, 0x00000000, 0, 0},
+ /* Enable DEC/OBC/OECF/LCCF/AWB/SC/DUMP */
+ {0x00000010, 0x000A00D6, 0x00000000, 0x00},
+ /* Enable CFA/CAR/CCM/GMARGB/R2Y/SHRP/SAT/DNYUV/YCRV/YHIST/CTC/DBC */
+ {0x00000A08, 0x107A01BE, 0x00000000, 0x00},
+};
+
+const struct reg_table isp_reg_init_settings[] = {
+ {isp_reg_init_config_list,
+ ARRAY_SIZE(isp_reg_init_config_list)},
+};
+
+static struct regval_t isp_reg_start_config_list[] = {
+#if defined(ENABLE_SS0_SS1)
+ /* ENABLE UO/SS0/SS1/Multi-Frame and Reset ISP */
+ {0x00000A00, 0x00121802, 0x00000000, 0x0A},
+ /* ENABLE UO/SS0/SS1/Multi-Frame and Leave ISP reset */
+ {0x00000A00, 0x00121800, 0x00000000, 0x0A},
+#else
+ /* ENABLE UO/Multi-Frame and Reset ISP */
+ {0x00000A00, 0x00120002, 0x00000000, 0x0A},
+ /* ENABLE UO/Multi-Frame and Leave ISP reset */
+ {0x00000A00, 0x00120000, 0x00000000, 0x0A},
+#endif
+ /* Config ISP shadow mode as next-vsync */
+ {0x00000A50, 0x00000002, 0x00000000, 0x00},
+#if defined(ENABLE_SS0_SS1)
+ /* ENABLE UO/SS0/SS1/Multi-Frame and Enable ISP */
+ {0x00000A00, 0x00121801, 0x00000000, 0x0A},
+#else
+ /* ENABLE UO/Multi-Frame and Enable ISP */
+ {0x00000A00, 0x00120001, 0x00000000, 0x0A},
+#endif
+ /* Config CSI shadow mode as immediate to fetch current setting */
+ {0x00000008, 0x00010004, 0x00000000, 0x0A},
+ /* Config CSI shadow mode as next-vsync */
+ {0x00000008, 0x00020004, 0x00000000, 0x00},
+ /* Enable CSI */
+ {0x00000000, 0x00000001, 0x00000000, 0x0A},
+};
+
+const struct reg_table isp_reg_start_settings[] = {
+ {isp_reg_start_config_list,
+ ARRAY_SIZE(isp_reg_start_config_list)},
+};
+
+static struct regval_t isp_imx_219_reg_config_list[] = {
+ /* MIPI sensor */
+ {0x00000014, 0x0000000D, 0, 0},
+ /* config CFA(0018H, 0A1CH) */
+ {0x00000A1C, 0x00000032, 0, 0},
+ {0x00000A8C, 0x00000000, 0, 0},
+ {0x00000A90, 0x00000000, 0, 0},
+ /* config R2Y(0E40H~0E60H) */
+ {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},
+ /* Config AWB(0280H~02DCH). Fixed WB gain for IMX-219 sensor. */
+ {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},
+ /* config GMARGB(0E00H~0E38H)
+ * Gamma RGB 1.9 for IMX-219 sensor
+ */
+ {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},
+ /* Enable CFA/GMARGB/R2Y */
+ {0x00000A08, 0x10000032, 0x0FFFFFFF, 0x00},
+ {0x00000A00, 0x00120002, 0, 0},
+ {0x00000A00, 0x00120000, 0, 0},
+ {0x00000A50, 0x00000002, 0, 0},
+ {0x00000008, 0x00010000, 0, 0},
+ {0x00000008, 0x0002000A, 0, 0},
+ {0x00000000, 0x00000001, 0, 0},
+};
+
+const struct reg_table isp_imx_219_settings[] = {
+ {isp_imx_219_reg_config_list,
+ ARRAY_SIZE(isp_imx_219_reg_config_list)},
+};
+
+static struct regval_t isp_format_reg_list[] = {
+ {0x0000001C, 0x00000000, 0x00000000, 0},
+ {0x00000020, 0x0437077F, 0x00000000, 0},
+ {0x00000A0C, 0x04380780, 0x00000000, 0},
+ {0x00000A88, 0x00000780, 0x00000000, 0},
+ {0x00000018, 0x000011BB, 0x00000000, 0},
+ {0x00000A08, 0x10000000, 0xF0000000, 0},
+ {0x00000028, 0x00030B80, 0x0003FFFF, 0},
+ {0x00000AA8, 0x07800438, 0x00000000, 0},
+ {0x00000A9C, 0x00000780, 0x00000000, 0},
+ {0x00000AC0, 0x07800438, 0x00000000, 0},
+ {0x00000AB4, 0x00000780, 0x00000000, 0},
+ {0x00000B20, 0x04380780, 0x00000000, 0},
+ {0x00000B24, 0x00000960, 0x00000000, 0},
+ {0x00000B3C, 0x00000960, 0x00000000, 0},
+ {0x00000014, 0x00000008, 0x00000000, 0},
+};
+
+const struct reg_table isp_format_settings[] = {
+ {isp_format_reg_list,
+ ARRAY_SIZE(isp_format_reg_list)},
+};
+
+#if defined(USE_NEW_CONFIG_SETTING)
+#else
+static struct reg_table *isp_settings = (struct reg_table *)isp_imx_219_settings;
+#endif
+
+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 void isp_load_regs_exclude_csi_isp_enable(
+ 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
+ || ((reg_addr == ISP_REG_CSI_INPUT_EN_AND_STATUS) && (val & 0x01))
+ || ((reg_addr == ISP_REG_ISP_CTRL_0) && (val & 0x01)))
+ 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 stfcamss *stfcamss = isp_dev->stfcamss;
+
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_WRAPPER_CLK_C].clk);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_WRAPPER_C].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_WRAPPER_P].rstc);
+
+ return 0;
+}
+
+static int stf_isp_clk_disable(struct stf_isp_dev *isp_dev)
+{
+ struct stfcamss *stfcamss = isp_dev->stfcamss;
+
+ reset_control_assert(stfcamss->sys_rst[STFRST_WRAPPER_C].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_WRAPPER_P].rstc);
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_WRAPPER_CLK_C].clk);
+
+ return 0;
+}
+
+static void __iomem *stf_isp_get_ispbase(struct stf_vin_dev *vin)
+{
+ void __iomem *base = vin->isp_base;
+
+ return base;
+}
+
+static int stf_isp_save_ctx_regs(struct stf_isp_dev *isp_dev)
+{
+ int j;
+ u32 addr, val;
+ void __iomem *ispbase;
+ struct device *dev = isp_dev->stfcamss->dev;
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ if (!isp_dev->context_regs) {
+ int regs_size =
+ sizeof(struct regval_t) * isp_reg_init_settings->regval_num;
+ isp_dev->context_regs =
+ devm_kzalloc(dev, sizeof(struct reg_table), GFP_KERNEL);
+ isp_dev->context_regs->regval =
+ devm_kzalloc(dev, regs_size, GFP_KERNEL);
+ isp_dev->context_regs->regval_num = isp_reg_init_settings->regval_num;
+ }
+
+ if (!isp_dev->context_regs || !isp_dev->context_regs->regval)
+ return -ENOMEM;
+
+ st_debug(ST_ISP, "Saving ISP context registers\n");
+ for (j = 0; j < isp_reg_init_settings->regval_num; j++) {
+ addr = isp_reg_init_settings->regval[j].addr;
+ val = ioread32(ispbase + addr);
+ isp_dev->context_regs->regval[j].addr = addr;
+ isp_dev->context_regs->regval[j].val = val;
+ }
+ st_debug(ST_ISP, "ISP context registers have been saved\n");
+
+ return 0;
+};
+
+static int stf_isp_restore_ctx_regs(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ if (isp_dev->context_regs) {
+ isp_load_regs(ispbase, isp_dev->context_regs);
+ st_debug(ST_ISP, "Restored ISP register: isp_reg_init_settings.\n");
+ }
+
+ return 0;
+}
+
+static int stf_isp_reset(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(1), BIT(1));
+ reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(1), 0);
+
+ 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;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ st_debug(ST_ISP, "%s\n", __func__);
+
+#if defined(USE_NEW_CONFIG_SETTING)
+ mutex_lock(&isp_dev->setfile_lock);
+
+ if (isp_dev->context_regs) {
+ stf_isp_restore_ctx_regs(isp_dev);
+ st_debug(ST_ISP, "%s context regs restore done\n", __func__);
+ } else {
+ isp_load_regs(ispbase, isp_reg_init_settings);
+ st_debug(ST_ISP, "%s isp_reg_init_settings done\n", __func__);
+ }
+ if (isp_dev->setfile.state) {
+ st_info(ST_ISP, "%s, Program extra ISP setting!\n", __func__);
+ isp_load_regs_exclude_csi_isp_enable(ispbase,
+ &isp_dev->setfile.settings);
+ }
+
+ mutex_unlock(&isp_dev->setfile_lock);
+#else
+ 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);
+#endif
+
+ return 0;
+}
+
+static int stf_isp_set_format(struct stf_isp_dev *isp_dev,
+ struct isp_stream_format *crop_array, u32 mcode,
+ int type)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ struct stf_dvp_dev *dvp_dev = isp_dev->stfcamss->dvp_dev;
+ struct v4l2_rect *crop = &crop_array[ISP_COMPOSE].rect;
+ u32 bpp = crop_array[ISP_COMPOSE].bpp;
+ void __iomem *ispbase;
+ u32 val, val1;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ st_debug(ST_ISP, "interface type is %d(%s)\n",
+ type, type == CSI_SENSOR ? "CSI" : "DVP");
+
+ if (type == DVP_SENSOR) {
+ unsigned int flags = dvp_dev->dvp->flags;
+
+ st_debug(ST_ISP, "dvp flags = 0x%x, hsync active is %s, vsync active is %s\n",
+ flags, flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH ? "high" : "low",
+ flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH ? "high" : "low");
+ }
+
+ 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 = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ 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, "src left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ crop = &crop_array[ISP_CROP].rect;
+ bpp = crop_array[ISP_CROP].bpp;
+ val = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_128);
+ isp_format_reg_list[6].addr = ISP_REG_DUMP_CFG_1;
+ isp_format_reg_list[6].val = val | 3 << 16;
+ isp_format_reg_list[6].mask = 0x0003FFFF;
+
+ st_info(ST_ISP, "raw left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ crop = &crop_array[ISP_SCALE_SS0].rect;
+ bpp = crop_array[ISP_SCALE_SS0].bpp;
+ isp_format_reg_list[7].addr = ISP_REG_SS0IW;
+ isp_format_reg_list[7].val = (crop->width << 16) + crop->height;
+ isp_format_reg_list[8].addr = ISP_REG_SS0S;
+ isp_format_reg_list[8].val = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ st_info(ST_ISP, "ss0 left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ crop = &crop_array[ISP_SCALE_SS1].rect;
+ bpp = crop_array[ISP_SCALE_SS1].bpp;
+ isp_format_reg_list[9].addr = ISP_REG_SS1IW;
+ isp_format_reg_list[9].val = (crop->width << 16) + crop->height;
+ isp_format_reg_list[10].addr = ISP_REG_SS1S;
+ isp_format_reg_list[10].val = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ crop = &crop_array[ISP_ITIWS].rect;
+ bpp = crop_array[ISP_ITIWS].bpp;
+ isp_format_reg_list[11].addr = ISP_REG_ITIIWSR;
+ isp_format_reg_list[11].val = (crop->height << 16) + crop->width;
+ isp_format_reg_list[12].addr = ISP_REG_ITIDWLSR;
+ isp_format_reg_list[12].val = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+ isp_format_reg_list[13].addr = ISP_REG_ITIDRLSR;
+ isp_format_reg_list[13].val = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ st_info(ST_ISP, "iti left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ isp_format_reg_list[14].addr = ISP_REG_SENSOR;
+ isp_format_reg_list[14].val = 0x00000000;
+ if (type == DVP_SENSOR) {
+ unsigned int flags = dvp_dev->dvp->flags;
+
+ if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+ isp_format_reg_list[14].val |= 0x08;
+ if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+ isp_format_reg_list[14].val |= 0x04;
+ } else {
+ isp_format_reg_list[14].val |= 0x01;
+ }
+
+ isp_load_regs(ispbase, isp_format_settings);
+ 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;
+
+ void __iomem *ispbase;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ if (on) {
+#if defined(USE_NEW_CONFIG_SETTING)
+ isp_load_regs(ispbase, isp_reg_start_settings);
+#else
+ reg_set_bit(ispbase, ISP_REG_CSIINTS_ADDR, 0x3FFFF, 0x3000a);
+ reg_set_bit(ispbase, ISP_REG_IESHD_ADDR, BIT(1) | BIT(0), 0x3);
+ reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(0), 1);
+#endif //#if defined(USE_NEW_CONFIG_SETTING)
+ } else {
+ /* NOTE: Clear bit 0 of ISP_REG_ISP_CTRL_0 here will get crash. */
+ stf_isp_save_ctx_regs(isp_dev);
+ }
+
+ return 0;
+}
+
+static union reg_buf reg_buf;
+static int stf_isp_reg_read(struct stf_isp_dev *isp_dev, void *arg)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+ struct isp_reg_param *reg_param = arg;
+ u32 size;
+ unsigned long r;
+
+ if (reg_param->reg_buf == NULL) {
+ st_err(ST_ISP, "Failed to access register. The pointer is NULL!!!\n");
+ return -EINVAL;
+ }
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ size = 0;
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ if (reg_param->reg_info.length > STF_ISP_REG_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_BUF_SIZE);
+ return -EINVAL;
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ /* This mode is not supported in the V4L2 version. */
+ st_err(ST_ISP, "Reg Read - Failed to access register. The method = \
+ STF_ISP_REG_METHOD_MODULE is not supported!!!\n");
+ return -ENOTTY;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_TBL_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_2_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_TBL_2_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 3;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_3_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_TBL_3_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 4;
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ st_err(ST_ISP, "Reg Read - Failed to access register. The method = \
+ STF_ISP_REG_METHOD_SMPL_PACK is not supported!!!\n");
+ return -ENOTTY;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ // This mode is not supported in the V4L2 version.
+ st_err(ST_ISP, "Reg Read - Failed to access register. The method = \
+ STF_ISP_REG_METHOD_SOFT_RDMA is not supported!!!\n");
+ return -ENOTTY;
+
+ default:
+ st_err(ST_ISP, "Failed to access register. The method=%d \
+ is not supported!!!\n", reg_param->reg_info.method);
+ return -ENOTTY;
+ }
+
+ memset(&reg_buf, 0, sizeof(union reg_buf));
+ if (size) {
+ r = copy_from_user((u8 *)reg_buf.buffer,
+ (u8 *)reg_param->reg_buf->buffer, size);
+ if (r) {
+ st_err(ST_ISP, "Failed to call copy_from_user for the \
+ reg_param->reg_buf value\n");
+ return -EIO;
+ }
+ }
+
+ size = 0;
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ reg_buf.buffer[0] = reg_read(ispbase, reg_param->reg_info.offset);
+ size = sizeof(u32);
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ reg_buf.buffer[r] = reg_read(ispbase,
+ reg_param->reg_info.offset + (r * 4));
+ }
+ size = sizeof(u32) * reg_param->reg_info.length;
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ reg_buf.reg_tbl[r].value = reg_read(ispbase,
+ reg_buf.reg_tbl[r].offset);
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf.reg_tbl2[r].mask) {
+ reg_buf.reg_tbl2[r].value = (reg_read(ispbase,
+ reg_buf.reg_tbl2[r].offset)
+ & reg_buf.reg_tbl2[r].mask);
+ } else {
+ reg_buf.reg_tbl2[r].value = reg_read(ispbase,
+ reg_buf.reg_tbl2[r].offset);
+ }
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 3;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf.reg_tbl3[r].mask) {
+ reg_buf.reg_tbl3[r].value = (reg_read(ispbase,
+ reg_buf.reg_tbl3[r].offset)
+ & reg_buf.reg_tbl3[r].mask);
+ } else {
+ reg_buf.reg_tbl3[r].value = reg_read(ispbase,
+ reg_buf.reg_tbl3[r].offset);
+ }
+ if (reg_buf.reg_tbl3[r].delay_ms) {
+ usleep_range(1000 * reg_buf.reg_tbl3[r].delay_ms,
+ 1000 * reg_buf.reg_tbl3[r].delay_ms + 100);
+ }
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 4;
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ break;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ break;
+
+ default:
+ break;
+ }
+
+ r = copy_to_user((u8 *)reg_param->reg_buf->buffer, (u8 *)reg_buf.buffer,
+ size);
+ if (r) {
+ st_err(ST_ISP, "Failed to call copy_to_user for the \
+ reg_param->buffer value\n");
+ return -EIO;
+ }
+
+ return 0;
+}
+
+static int stf_isp_soft_rdma(struct stf_isp_dev *isp_dev, u32 rdma_addr)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+ struct isp_rdma_info *rdma_info = NULL;
+ s32 len;
+ u32 offset;
+ int ret = 0;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ rdma_info = phys_to_virt(rdma_addr);
+ while (1) {
+ if (rdma_info->tag == RDMA_WR_ONE) {
+ reg_write(ispbase, rdma_info->offset, rdma_info->param);
+ rdma_info++;
+ } else if (rdma_info->tag == RDMA_WR_SRL) {
+ offset = rdma_info->offset;
+ len = rdma_info->param;
+ rdma_info++;
+ while (len > 0) {
+ reg_write(ispbase, offset, rdma_info->param);
+ offset += 4;
+ len--;
+ if (len > 0) {
+ reg_write(ispbase, offset, rdma_info->value);
+ len--;
+ }
+ offset += 4;
+ rdma_info++;
+ }
+ } else if (rdma_info->tag == RDMA_LINK) {
+ rdma_info = phys_to_virt(rdma_info->param);
+ } else if (rdma_info->tag == RDMA_SINT) {
+ /* Software not support this command. */
+ rdma_info++;
+ } else if (rdma_info->tag == RDMA_END) {
+ break;
+ } else
+ rdma_info++;
+ }
+
+ return ret;
+}
+
+static int stf_isp_reg_write(struct stf_isp_dev *isp_dev, void *arg)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+ struct isp_reg_param *reg_param = arg;
+ struct isp_rdma_info *rdma_info = NULL;
+ s32 len;
+ u32 offset;
+ u32 size;
+ unsigned long r;
+ int ret = 0;
+
+ if ((reg_param->reg_buf == NULL)
+ && (reg_param->reg_info.method != STF_ISP_REG_METHOD_SOFT_RDMA)) {
+ st_err(ST_ISP, "Failed to access register. \
+ The register buffer pointer is NULL!!!\n");
+ return -EINVAL;
+ }
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ size = 0;
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ size = sizeof(u32);
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ if (reg_param->reg_info.length > STF_ISP_REG_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length;
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ // This mode is not supported in the V4L2 version.
+ st_err(ST_ISP, "Reg Write - Failed to access register. \
+ The method = STF_ISP_REG_METHOD_MODULE is not supported!!!\n");
+ return -ENOTTY;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_TBL_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_2_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_TBL_2_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 3;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_3_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_TBL_3_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 4;
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ if (reg_param->reg_info.length > STF_ISP_REG_SMPL_PACK_BUF_SIZE) {
+ st_err(ST_ISP, "Failed to access register. \
+ The (length=0x%08X > 0x%08X) is out of size!!!\n",
+ reg_param->reg_info.length, STF_ISP_REG_SMPL_PACK_BUF_SIZE);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ break;
+
+ default:
+ st_err(ST_ISP, "Failed to access register. The method=%d \
+ is not supported!!!\n", reg_param->reg_info.method);
+ return -ENOTTY;
+ }
+
+ memset(&reg_buf, 0, sizeof(union reg_buf));
+ if (size) {
+ r = copy_from_user((u8 *)reg_buf.buffer,
+ (u8 *)reg_param->reg_buf->buffer, size);
+ if (r) {
+ st_err(ST_ISP, "Failed to call copy_from_user for the \
+ reg_param->reg_buf value\n");
+ return -EIO;
+ }
+ }
+
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ reg_write(ispbase, reg_param->reg_info.offset, reg_buf.buffer[0]);
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ reg_write(ispbase, reg_param->reg_info.offset + (r * 4),
+ reg_buf.buffer[r]);
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ /* This mode is not supported in the V4L2 version. */
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ reg_write(ispbase, reg_buf.reg_tbl[r].offset,
+ reg_buf.reg_tbl[r].value);
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf.reg_tbl2[r].mask) {
+ reg_set_bit(ispbase, reg_buf.reg_tbl2[r].offset,
+ reg_buf.reg_tbl2[r].mask, reg_buf.reg_tbl2[r].value);
+ } else {
+ reg_write(ispbase, reg_buf.reg_tbl2[r].offset,
+ reg_buf.reg_tbl2[r].value);
+ }
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf.reg_tbl3[r].mask) {
+ reg_set_bit(ispbase, reg_buf.reg_tbl3[r].offset,
+ reg_buf.reg_tbl3[r].mask, reg_buf.reg_tbl3[r].value);
+ } else {
+ reg_write(ispbase, reg_buf.reg_tbl3[r].offset,
+ reg_buf.reg_tbl3[r].value);
+ }
+ if (reg_buf.reg_tbl3[r].delay_ms) {
+ usleep_range(1000 * reg_buf.reg_tbl3[r].delay_ms,
+ 1000 * reg_buf.reg_tbl3[r].delay_ms + 100);
+ }
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ size = reg_param->reg_info.length;
+ rdma_info = &reg_buf.rdma_cmd[0];
+ while (size) {
+ if (rdma_info->tag == RDMA_WR_ONE) {
+ reg_write(ispbase, rdma_info->offset, rdma_info->param);
+ rdma_info++;
+ size--;
+ } else if (rdma_info->tag == RDMA_WR_SRL) {
+ offset = rdma_info->offset;
+ len = rdma_info->param;
+ rdma_info++;
+ size--;
+ while (size && (len > 0)) {
+ reg_write(ispbase, offset, rdma_info->param);
+ offset += 4;
+ len--;
+ if (len > 0) {
+ reg_write(ispbase, offset, rdma_info->value);
+ len--;
+ }
+ offset += 4;
+ rdma_info++;
+ size--;
+ }
+ } else if (rdma_info->tag == RDMA_END) {
+ break;
+ } else {
+ rdma_info++;
+ size--;
+ }
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ /*
+ * Simulation the hardware RDMA behavior to debug and verify
+ * the RDMA chain.
+ */
+ ret = stf_isp_soft_rdma(isp_dev, reg_param->reg_info.offset);
+ break;
+
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+static int stf_isp_shadow_trigger(struct stf_isp_dev *isp_dev)
+{
+ struct stf_vin_dev *vin = isp_dev->stfcamss->vin;
+ void __iomem *ispbase;
+
+ ispbase = stf_isp_get_ispbase(vin);
+
+ // shadow update
+ reg_set_bit(ispbase, ISP_REG_CSIINTS_ADDR, (BIT(17) | BIT(16)), 0x30000);
+ reg_set_bit(ispbase, ISP_REG_IESHD_ADDR, (BIT(1) | BIT(0)), 0x3);
+ return 0;
+}
+
+void dump_isp_reg(void *__iomem ispbase)
+{
+ int j;
+ u32 addr, val;
+
+ st_debug(ST_ISP, "DUMP ISP register:\n -- isp_reg_init_settings --\n");
+ for (j = 0; j < isp_reg_init_settings->regval_num; j++) {
+ addr = isp_reg_init_settings->regval[j].addr;
+ val = ioread32(ispbase + addr);
+ st_debug(ST_ISP, "{0x%08x, 0x%08x}\n", addr, val);
+ }
+
+ st_debug(ST_ISP, " --- isp_format_settings ---\n");
+ 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, "-- ISP_REG_Y_PLANE_START_ADDR --\n {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, "-- ISP_REG_UV_PLANE_START_ADDR --\n {0x%08x, 0x%08x}\n",
+ ISP_REG_UV_PLANE_START_ADDR, val);
+ val = ioread32(ispbase + ISP_REG_DUMP_CFG_0);
+ st_debug(ST_ISP, "-- ISP_REG_DUMP_CFG_0 --\n {0x%08x, 0x%08x}\n",
+ ISP_REG_DUMP_CFG_0, val);
+ val = ioread32(ispbase + ISP_REG_DUMP_CFG_1);
+ st_debug(ST_ISP, " --- ISP_REG_DUMP_CFG_1 ---\n {0x%08x, 0x%08x}\n",
+ ISP_REG_DUMP_CFG_1, val);
+
+ st_debug(ST_ISP, " --- isp_reg_start_settings ---\n");
+ for (j = 0; j < isp_reg_start_settings->regval_num; j++) {
+ addr = isp_reg_start_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,
+ .isp_reg_read = stf_isp_reg_read,
+ .isp_reg_write = stf_isp_reg_write,
+ .isp_shadow_trigger = stf_isp_shadow_trigger,
+};
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_isp_ioctl.h
@@ -0,0 +1,133 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#ifndef STF_ISP_IOCTL_H
+#define STF_ISP_IOCTL_H
+
+
+#include <media/v4l2-ctrls.h>
+
+
+#define FILENAME_MAX_LEN 30
+
+#define ISP_IOC ('V')
+#define STF_ISP_REG_BUF_SIZE (768)
+#define STF_ISP_REG_TBL_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 2)
+#define STF_ISP_REG_TBL_2_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 3)
+#define STF_ISP_REG_TBL_3_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 4)
+#define STF_ISP_REG_SMPL_PACK_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 2)
+#define RDMA_WR_ONE (0xA0)
+#define RDMA_WR_SRL (0xA1)
+#define RDMA_LINK (0xA2)
+#define RDMA_SINT (0xA3)
+#define RDMA_END (0xAF)
+#define ENABLE_SS0_SS1
+
+enum _STF_ISP_IOCTL {
+ STF_ISP_IOCTL_LOAD_FW = BASE_VIDIOC_PRIVATE + 1,
+ STF_ISP_IOCTL_DMABUF_ALLOC,
+ STF_ISP_IOCTL_DMABUF_FREE,
+ STF_ISP_IOCTL_GET_HW_VER,
+ STF_ISP_IOCTL_REG,
+ STF_ISP_IOCTL_SHADOW_LOCK,
+ STF_ISP_IOCTL_SHADOW_UNLOCK,
+ STF_ISP_IOCTL_SHADOW_UNLOCK_N_TRIGGER,
+ STF_ISP_IOCTL_SET_USER_CONFIG_ISP,
+ STF_ISP_IOCTL_MAX
+};
+
+enum _STF_ISP_REG_METHOD {
+ STF_ISP_REG_METHOD_ONE_REG = 0,
+ STF_ISP_REG_METHOD_SERIES,
+ STF_ISP_REG_METHOD_MODULE,
+ STF_ISP_REG_METHOD_TABLE,
+ STF_ISP_REG_METHOD_TABLE_2,
+ STF_ISP_REG_METHOD_TABLE_3,
+ STF_ISP_REG_METHOD_SMPL_PACK,
+ STF_ISP_REG_METHOD_SOFT_RDMA,
+ STF_ISP_REG_METHOD_MAX
+};
+
+
+struct stfisp_fw_info {
+ char __user filename[FILENAME_MAX_LEN];
+};
+
+struct dmabuf_create {
+ __u32 fd;
+ __u32 size;
+ __u32 paddr;
+};
+
+struct isp_rdma_info {
+ u32 param;
+ union {
+ u32 value;
+ struct {
+ u32 offset : 24;
+ u32 tag : 8;
+ };
+ };
+};
+
+struct isp_reg_info {
+ /** @brief [in] access method of register */
+ u8 method;
+ /** @brief [in] offset indicated which register will be read/write */
+ u32 offset;
+ /** @brief [in] length for indicated how much register will be read/write */
+ u32 length;
+};
+
+union reg_buf {
+ u32 buffer[STF_ISP_REG_BUF_SIZE];
+ struct {
+ u32 offset;
+ u32 value;
+ } reg_tbl[STF_ISP_REG_TBL_BUF_SIZE];
+ struct {
+ u32 offset;
+ u32 value;
+ u32 mask;
+ } reg_tbl2[STF_ISP_REG_TBL_2_BUF_SIZE];
+ struct {
+ u32 offset;
+ u32 value;
+ u32 mask;
+ u32 delay_ms;
+ } reg_tbl3[STF_ISP_REG_TBL_3_BUF_SIZE];
+ struct isp_rdma_info rdma_cmd[STF_ISP_REG_SMPL_PACK_BUF_SIZE];
+};
+
+struct isp_reg_param {
+ /** @brief [in, out] register read/write information */
+ struct isp_reg_info reg_info;
+ /** @brief [in, out] buffer */
+ union reg_buf *reg_buf;
+};
+
+
+#define VIDIOC_STFISP_LOAD_FW \
+ _IOW(ISP_IOC, STF_ISP_IOCTL_LOAD_FW, struct stfisp_fw_info)
+#define VIDIOC_STF_DMABUF_ALLOC \
+ _IOWR(ISP_IOC, STF_ISP_IOCTL_DMABUF_ALLOC, struct dmabuf_create)
+#define VIDIOC_STF_DMABUF_FREE \
+ _IOWR(ISP_IOC, STF_ISP_IOCTL_DMABUF_FREE, struct dmabuf_create)
+#define VIDIOC_STFISP_GET_REG \
+ _IOWR(ISP_IOC, STF_ISP_IOCTL_REG, struct isp_reg_param)
+#define VIDIOC_STFISP_SET_REG \
+ _IOW(ISP_IOC, STF_ISP_IOCTL_REG, struct isp_reg_param)
+#define VIDIOC_STFISP_SHADOW_LOCK \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SHADOW_LOCK)
+#define VIDIOC_STFISP_SHADOW_UNLOCK \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SHADOW_UNLOCK)
+#define VIDIOC_STFISP_SHADOW_UNLOCK_N_TRIGGER \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SHADOW_UNLOCK_N_TRIGGER)
+#define VIDIOC_STFISP_SET_USER_CONFIG_ISP \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SET_USER_CONFIG_ISP)
+
+
+#endif /* STF_ISP_IOCTL_H */
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_video.c
@@ -0,0 +1,1552 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#include "stfcamss.h"
+#include "stf_video.h"
+#include <media/media-entity.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
+
+static const struct stfcamss_format_info formats_pix_st7110_wr[] = {
+ { MEDIA_BUS_FMT_AYUV8_1X32, V4L2_PIX_FMT_AYUV32, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 32 } },
+ { 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 } },
+ { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 8 } },
+ { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 8 } },
+ { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 8 } },
+ { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 8 } },
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+};
+
+static const struct stfcamss_format_info formats_raw_st7110_isp[] = {
+ { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+};
+
+static const struct stfcamss_format_info formats_pix_st7110_isp[] = {
+ // { MEDIA_BUS_FMT_YUYV12_2X12, V4L2_PIX_FMT_NV12M, 2,
+ // { { 1, 1 }, { 1, 1 } }, { { 1, 1 }, { 1, 1 } }, { 8 , 4 } },
+ { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV21, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+};
+
+static const struct stfcamss_format_info formats_st7110_isp_iti[] = {
+ // raw format
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+
+ // YUV420
+ { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV21, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+
+ // YUV444
+ { MEDIA_BUS_FMT_YUV8_1X24, V4L2_PIX_FMT_NV24, 1,
+ { { 1, 1 } }, { { 1, 3 } }, { 8 } },
+ { MEDIA_BUS_FMT_VUY8_1X24, V4L2_PIX_FMT_NV42, 1,
+ { { 1, 1 } }, { { 1, 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, STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ pix_mp->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+ 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\n", pix_mp->width,
+ pix_mp->height, fi->bpp[0]);
+ st_info(ST_VIDEO, "i = %d, p = %d, s = 0x%x\n", 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, STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ pix->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+ 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\n", pix->width,
+ pix->height, fi->bpp[0]);
+ st_info(ST_VIDEO, "i = %d, s = 0x%x\n", 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 = video->type,
+ .fmt.pix = {
+ .width = 1920,
+ .height = 1080,
+ .pixelformat = V4L2_PIX_FMT_RGB565,
+ },
+ };
+
+ ret = __video_try_fmt(video, &format, is_mp);
+
+ if (ret < 0)
+ return ret;
+
+ video->active_fmt = 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__);
+ }
+ if ((stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC)
+ == STF_ISP_PAD_SRC_SCD_Y) &&
+ sizes[0] < ISP_SCD_Y_BUFFER_SIZE) {
+ sizes[0] = ISP_SCD_Y_BUFFER_SIZE;
+ }
+
+ 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;
+
+ buffer->sizeimage = 0;
+
+ if (video->is_mp) {
+ for (i = 0; i < fmt_mp->num_planes; i++) {
+ paddr = vb2_plane_cookie(vb, i);
+ buffer->addr[i] = *paddr;
+ buffer->sizeimage += vb2_plane_size(vb, i);
+ }
+
+ 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->plane_fmt[0].bytesperline *
+ fmt_mp->height;
+ } else {
+ paddr = vb2_plane_cookie(vb, 0);
+ buffer->sizeimage = vb2_plane_size(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->bytesperline *
+ fmt->height;
+ }
+
+ if (stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC)
+ == STF_ISP_PAD_SRC_SCD_Y) {
+ buffer->addr[1] = buffer->addr[0] + ISP_YHIST_BUFFER_SIZE;
+ buffer->vaddr_sc = vb2_plane_vaddr(vb, 0);
+ }
+
+ 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)) {
+ st_err(ST_VIDEO, "sizeimage = %d, plane size = %d\n",
+ fmt->sizeimage, (unsigned int)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_pad_remote_pad_first(&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_pix_format *pix = &video->active_fmt.fmt.pix;
+ struct v4l2_pix_format_mplane *pix_mp =
+ &video->active_fmt.fmt.pix_mp;
+ 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;
+
+ if (video->is_mp)
+ pixelformat = pix_mp->pixelformat;
+ else
+ pixelformat = pix->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;
+
+ ret = video_device_pipeline_start(vdev, &video->stfcamss->pipe);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to video_device_pipeline_start: %d\n", ret);
+ return ret;
+ }
+
+ 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_pad_remote_pad_first(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:
+ video_device_pipeline_stop(vdev);
+ 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_pad_remote_pad_first(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);
+ }
+
+ video_device_pipeline_stop(vdev);
+ 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 getcrop_pad_id(int video_id)
+{
+ return stf_vin_map_isp_pad(video_id, STF_ISP_PAD_SRC);
+}
+
+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 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;
+ struct media_pad *pad;
+ bool support_selection = false;
+ 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;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ if (subdev->ops->pad->set_selection) {
+ support_selection = true;
+ break;
+ }
+ }
+
+ if (support_selection) {
+ 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;
+ fsize->stepwise.step_width = 1;
+ fsize->stepwise.step_height = 1;
+ } else {
+ entity = &vdev->entity;
+ sensor = stfcamss_find_sensor(entity);
+ if (!sensor)
+ return -ENOTTY;
+
+ 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;
+ }
+
+ 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 -ENOTTY;
+ 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;
+
+ /* Don't care about the code, just find by pixelformat */
+ ret = video_find_format(0, fival->pixel_format,
+ video->formats, video->nformats);
+ if (ret < 0)
+ return -EINVAL;
+
+ 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,0x%x\n",
+ __func__, video->active_fmt.type,
+ video->active_fmt.fmt.pix.pixelformat);
+ *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_entity_s_fmt(struct stfcamss_video *video,
+ struct media_entity *entity,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_mbus_framefmt *mf = &fmt->format;
+ struct v4l2_subdev_format fmt_src = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ };
+ u32 width, height, code;
+ int ret, index = 0;
+
+ code = mf->code;
+ width = mf->width;
+ height = mf->height;
+ subdev = media_entity_to_v4l2_subdev(entity);
+ while (1) {
+ if (index >= entity->num_pads)
+ break;
+ pad = &entity->pads[index];
+ pad = media_pad_remote_pad_first(pad);
+ if (pad && is_media_entity_v4l2_subdev(pad->entity)) {
+ fmt->pad = index;
+ ret = v4l2_subdev_call(subdev, pad, set_fmt, state, fmt);
+ 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);
+ }
+ if (index) {
+ fmt_src.pad = index;
+ ret = v4l2_subdev_call(subdev, pad, get_fmt, state, &fmt_src);
+ if (ret)
+ return ret;
+
+ fmt->format.code = fmt_src.format.code;
+ ret = video_entity_s_fmt(video, pad->entity, state, fmt);
+ }
+ }
+
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ index++;
+ }
+ return ret;
+}
+
+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;
+ int ret, index;
+ struct v4l2_subdev_format fmt = {
+ .pad = 0,
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .reserved = {getcrop_pad_id(video->id)}
+ };
+ 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;
+ struct media_entity *sensor;
+ u32 width, height;
+ struct media_pad *pad;
+
+ /* 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;
+ } 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);
+ }
+
+ width = mf->width;
+ height = mf->height;
+
+ sensor = stfcamss_find_sensor(entity);
+ if (!sensor) {
+ st_err(ST_VIDEO, "Can't find sensor\n");
+ return -ENOTTY;
+ }
+
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ ret = v4l2_subdev_call(subdev, pad, get_fmt, state, &fmt);
+ if (ret)
+ return ret;
+
+ /*
+ * Starting from sensor subdevice, walk within
+ * pipeline and set format on each subdevice
+ */
+ pad = media_pad_remote_pad_first(&sensor->pads[0]);
+ ret = video_entity_s_fmt(video, pad->entity, state, &fmt);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ return ret;
+
+ index = video_find_format(mf->code,
+ video->formats[index].pixelformat,
+ video->formats, video->nformats);
+ st_debug(ST_VIDEO, "%s, code=%x, index=%d\n",
+ __func__, mf->code, index);
+
+ if (index < 0)
+ return index;
+
+ 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, v4l2fmt=%x\n",
+ __func__, f->type, f->fmt.pix.pixelformat);
+
+ 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);
+
+ st_debug(ST_VIDEO, "%s, pixelformat=0x%x, ret=%d\n",
+ __func__, f->fmt.pix.pixelformat, ret);
+ 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_pad_remote_pad_first(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_pad_remote_pad_first(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,
+ .pad = getcrop_pad_id(video->id),
+ .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_pad_remote_pad_first(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,
+ .pad = getcrop_pad_id(video->id),
+ .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_pad_remote_pad_first(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;
+}
+
+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,
+};
+
+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,
+};
+
+static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_out = {
+ .vidioc_querycap = video_querycap,
+ .vidioc_enum_fmt_vid_out = video_enum_fmt,
+ .vidioc_enum_framesizes = video_enum_framesizes,
+ .vidioc_enum_frameintervals = video_enum_frameintervals,
+ .vidioc_g_fmt_vid_out = video_g_fmt,
+ .vidioc_s_fmt_vid_out = video_s_fmt,
+ .vidioc_try_fmt_vid_out = 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,
+};
+
+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;
+
+ if (!video->pm_count) {
+ 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;
+ }
+ }
+
+ video->pm_count++;
+
+ mutex_unlock(&video->lock);
+
+ return 0;
+
+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);
+ struct stfcamss_video *video = video_drvdata(file);
+
+ vb2_fop_release(file);
+
+ video->pm_count--;
+
+ if (!video->pm_count)
+ v4l2_pipeline_pm_put(&vdev->entity);
+
+ 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;
+ enum isp_pad_id isp_pad;
+
+ 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->type = video->type;
+ 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;
+ q->min_buffers_needed = STFCAMSS_MIN_BUFFERS;
+ 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);
+
+ isp_pad = stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC);
+ if (video->id == VIN_LINE_WR) {
+ video->formats = formats_pix_st7110_wr;
+ video->nformats = ARRAY_SIZE(formats_pix_st7110_wr);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+ } else if (isp_pad == STF_ISP_PAD_SRC
+ || isp_pad == STF_ISP_PAD_SRC_SS0
+ || isp_pad == STF_ISP_PAD_SRC_SS1) {
+ video->formats = formats_pix_st7110_isp;
+ video->nformats = ARRAY_SIZE(formats_pix_st7110_isp);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+ } else if (isp_pad == STF_ISP_PAD_SRC_ITIW
+ || isp_pad == STF_ISP_PAD_SRC_ITIR) {
+ video->formats = formats_st7110_isp_iti;
+ video->nformats = ARRAY_SIZE(formats_st7110_isp_iti);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+ } else { // raw/scdump/yhist
+ video->formats = formats_raw_st7110_isp;
+ video->nformats = ARRAY_SIZE(formats_raw_st7110_isp);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_128;
+ }
+ 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;
+ if (isp_pad == STF_ISP_PAD_SRC_ITIR) {
+ vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT;
+ vdev->vfl_dir = VFL_DIR_TX;
+ } else {
+ vdev->device_caps = is_mp ? V4L2_CAP_VIDEO_CAPTURE_MPLANE :
+ V4L2_CAP_VIDEO_CAPTURE;
+ vdev->vfl_dir = VFL_DIR_RX;
+ }
+ vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE | V4L2_CAP_IO_MC;
+ if (video->type == V4L2_CAP_VIDEO_OUTPUT)
+ vdev->ioctl_ops = &stf_vid_ioctl_ops_out;
+ else
+ 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->queue = &video->vb2_q;
+ vdev->lock = &video->lock;
+ //strlcpy(vdev->name, name, sizeof(vdev->name));
+ strscpy(vdev->name, name, sizeof(vdev->name));
+
+ ret = video_register_device(vdev, VFL_TYPE_VIDEO, video->id);
+ 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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_video.h
@@ -0,0 +1,83 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 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 64
+#define STFCAMSS_FRAME_MAX_WIDTH 1920
+#define STFCAMSS_FRAME_MIN_HEIGHT 64
+#define STFCAMSS_FRAME_MAX_HEIGHT 1080
+#define STFCAMSS_FRAME_WIDTH_ALIGN_8 8
+#define STFCAMSS_FRAME_WIDTH_ALIGN_128 128
+#define STFCAMSS_MIN_BUFFERS 2
+
+#define STFCAMSS_MAX_ENTITY_NAME_LEN 27
+
+struct stfcamss_buffer {
+ struct vb2_v4l2_buffer vb;
+ dma_addr_t addr[3];
+ void *vaddr_sc; /* Use for isp sc data */
+ struct list_head queue;
+ int sizeimage;
+};
+
+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;
+ unsigned int pm_count;
+};
+
+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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_vin.c
@@ -0,0 +1,1515 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-event.h>
+
+#include "stfcamss.h"
+
+#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 90
+#define VIN_FRAME_DROP_MIN_VAL 4
+#define VIN_FRAME_PER_SEC_MAX_VAL 90
+
+/* ISP ctrl need 1 sec to let frames become stable. */
+#define VIN_FRAME_DROP_SEC_FOR_ISP_CTRL 1
+
+
+// #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_SRGGB8_1X8, 8},
+ { MEDIA_BUS_FMT_SGRBG8_1X8, 8},
+ { MEDIA_BUS_FMT_SGBRG8_1X8, 8},
+ { MEDIA_BUS_FMT_SBGGR8_1X8, 8},
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
+ { MEDIA_BUS_FMT_Y12_1X12, 8},
+ { MEDIA_BUS_FMT_YUV8_1X24, 8},
+ { MEDIA_BUS_FMT_AYUV8_1X32, 32},
+};
+
+static const struct vin2_format isp_formats_st7110_raw[] = {
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
+};
+
+static const struct vin2_format isp_formats_st7110_uo[] = {
+ { MEDIA_BUS_FMT_Y12_1X12, 8},
+};
+
+static const struct vin2_format isp_formats_st7110_iti[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
+ { MEDIA_BUS_FMT_Y12_1X12, 8},
+ { MEDIA_BUS_FMT_YUV8_1X24, 8},
+};
+
+static const struct vin2_format_table vin2_formats_table[] = {
+ /* VIN_LINE_WR */
+ { vin2_formats_st7110, ARRAY_SIZE(vin2_formats_st7110) },
+ /* VIN_LINE_ISP */
+ { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },
+ /* VIN_LINE_ISP_SS0 */
+ { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },
+ /* VIN_LINE_ISP_SS1 */
+ { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },
+ /* VIN_LINE_ISP_ITIW */
+ { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) },
+ /* VIN_LINE_ISP_ITIR */
+ { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) },
+ /* VIN_LINE_ISP_RAW */
+ { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) },
+ /* VIN_LINE_ISP_SCD_Y */
+ { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) },
+};
+
+static void vin_buffer_done(struct vin_line *line, struct vin_params *params);
+static void vin_change_buffer(struct vin_line *line);
+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 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);
+
+static char *get_line_subdevname(int line_id)
+{
+ char *name = NULL;
+
+ switch (line_id) {
+ case VIN_LINE_WR:
+ name = "wr";
+ break;
+ case VIN_LINE_ISP:
+ name = "isp0";
+ break;
+ case VIN_LINE_ISP_SS0:
+ name = "isp0_ss0";
+ break;
+ case VIN_LINE_ISP_SS1:
+ name = "isp0_ss1";
+ break;
+ case VIN_LINE_ISP_ITIW:
+ name = "isp0_itiw";
+ break;
+ case VIN_LINE_ISP_ITIR:
+ name = "isp0_itir";
+ break;
+ case VIN_LINE_ISP_RAW:
+ name = "isp0_raw";
+ break;
+ case VIN_LINE_ISP_SCD_Y:
+ name = "isp0_scd_y";
+ break;
+ default:
+ name = "unknow";
+ break;
+ }
+ return name;
+}
+
+static enum isp_line_id stf_vin_map_isp_line(enum vin_line_id line)
+{
+ enum isp_line_id line_id;
+
+ if ((line > VIN_LINE_WR) && (line < VIN_LINE_MAX)) {
+ line_id = line % STF_ISP_LINE_MAX;
+ if (line_id == 0)
+ line_id = STF_ISP_LINE_SRC_SCD_Y;
+ } else
+ line_id = STF_ISP_LINE_INVALID;
+
+ return line_id;
+}
+
+enum isp_pad_id stf_vin_map_isp_pad(enum vin_line_id line, enum isp_pad_id def)
+{
+ enum isp_pad_id pad_id;
+
+ if (line == VIN_LINE_WR)
+ pad_id = STF_ISP_PAD_SINK;
+ else if ((line > VIN_LINE_WR) && (line < VIN_LINE_MAX))
+ pad_id = stf_vin_map_isp_line(line);
+ else
+ pad_id = def;
+
+ return pad_id;
+}
+
+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 i, ret = 0;
+
+ vin_dev->stfcamss = stfcamss;
+ vin_dev->hw_ops = &vin_ops;
+ vin_dev->hw_ops->isr_buffer_done = vin_buffer_done;
+ vin_dev->hw_ops->isr_change_buffer = vin_change_buffer;
+
+ 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->isp_irq, vin_dev->hw_ops->vin_isp_irq_handler,
+ 0, "vin_isp_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp irq\n");
+ goto out;
+ }
+
+ st_info(ST_CAMSS, "%s, %d!\n", __func__, __LINE__);
+#ifdef ISP_USE_CSI_AND_SC_DONE_INTERRUPT
+ ret = devm_request_irq(dev,
+ vin->isp_csi_irq, vin_dev->hw_ops->vin_isp_csi_irq_handler,
+ 0, "vin_isp_csi_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp raw irq\n");
+ goto out;
+ }
+
+ ret = devm_request_irq(dev,
+ vin->isp_scd_irq, vin_dev->hw_ops->vin_isp_scd_irq_handler,
+ 0, "vin_isp_scd_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp scd irq\n");
+ goto out;
+ }
+#endif
+
+ ret = devm_request_irq(dev,
+ vin->isp_irq_csiline, vin_dev->hw_ops->vin_isp_irq_csiline_handler,
+ 0, "vin_isp_irq_csiline", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp irq csiline\n");
+ goto out;
+ }
+
+ mutex_init(&vin_dev->power_lock);
+ vin_dev->power_count = 0;
+
+ for (i = 0; i < STF_DUMMY_MODULE_NUMS; i++) {
+ struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[i];
+
+ mutex_init(&dummy_buffer->stream_lock);
+ dummy_buffer->nums = i == 0 ? VIN_DUMMY_BUFFER_NUMS : ISP_DUMMY_BUFFER_NUMS;
+ dummy_buffer->stream_count = 0;
+ dummy_buffer->buffer = devm_kzalloc(dev,
+ dummy_buffer->nums * sizeof(struct vin_dummy_buffer), GFP_KERNEL);
+ atomic_set(&dummy_buffer->frame_skip, 0);
+ }
+
+ for (i = VIN_LINE_WR;
+ i < STF_ISP_LINE_MAX + 1; i++) {
+ struct vin_line *l = &vin_dev->line[i];
+ int is_mp;
+
+ is_mp = i == VIN_LINE_WR ? false : true;
+ is_mp = false;
+ if (stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC_ITIR)
+ l->video_out.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+ else
+ 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_table[i].fmts;
+ l->nformats = vin2_formats_table[i].nfmts;
+ 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);
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ 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) {
+ pm_runtime_get_sync(stfcamss->dev);
+ 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);
+ pm_runtime_put_sync(stfcamss->dev);
+ }
+ vin_dev->power_count--;
+ }
+exit:
+
+ mutex_unlock(&vin_dev->power_lock);
+
+ return 0;
+}
+
+static unsigned int get_frame_skip(struct vin_line *line)
+{
+ unsigned int frame_skip = 0;
+ unsigned int isp_ctrl_skip_frames = 0;
+ struct media_entity *sensor;
+ struct v4l2_subdev_frame_interval fi;
+
+ sensor = stfcamss_find_sensor(&line->subdev.entity);
+ if (sensor) {
+ int fps = 0;
+ struct v4l2_subdev *subdev =
+ media_entity_to_v4l2_subdev(sensor);
+
+ if (subdev->ops->video->g_frame_interval) {
+ if (!subdev->ops->video->g_frame_interval(subdev, &fi))
+ fps = fi.interval.denominator;
+
+ if (fps > 0 && fps <= 90)
+ isp_ctrl_skip_frames = fps * VIN_FRAME_DROP_SEC_FOR_ISP_CTRL;
+ }
+ if (!fps)
+ st_debug(ST_VIN, "%s, Failed to get sensor fps !\n", __func__);
+
+ if (isp_ctrl_skip_frames <= VIN_FRAME_DROP_MIN_VAL)
+ isp_ctrl_skip_frames = VIN_FRAME_DROP_MIN_VAL;
+
+ v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
+
+ frame_skip += isp_ctrl_skip_frames;
+
+ 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);
+ }
+
+ return frame_skip;
+}
+
+static void vin_buf_l2cache_flush(struct vin_output *output)
+{
+ struct stfcamss_buffer *buffer = NULL;
+
+ if (!list_empty(&output->pending_bufs)) {
+ list_for_each_entry(buffer, &output->pending_bufs, queue) {
+ sifive_l2_flush64_range(buffer->addr[0], buffer->sizeimage);
+ }
+ }
+}
+
+static int vin_enable_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_IDLE;
+
+ vin_buf_l2cache_flush(output);
+
+ 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 u32 line_to_dummy_module(struct vin_line *line)
+{
+ u32 dummy_module = 0;
+
+ switch (line->id) {
+ case VIN_LINE_WR:
+ dummy_module = STF_DUMMY_VIN;
+ break;
+ case VIN_LINE_ISP:
+ case VIN_LINE_ISP_SS0:
+ case VIN_LINE_ISP_SS1:
+ case VIN_LINE_ISP_ITIW:
+ case VIN_LINE_ISP_ITIR:
+ case VIN_LINE_ISP_RAW:
+ case VIN_LINE_ISP_SCD_Y:
+ dummy_module = STF_DUMMY_ISP;
+ break;
+ default:
+ dummy_module = STF_DUMMY_VIN;
+ break;
+ }
+
+ return dummy_module;
+}
+
+static int vin_alloc_dummy_buffer(struct stf_vin2_dev *vin_dev,
+ struct v4l2_mbus_framefmt *fmt, int dummy_module)
+{
+ struct device *dev = vin_dev->stfcamss->dev;
+ struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
+ struct vin_dummy_buffer *buffer = NULL;
+ int ret = 0, i;
+ u32 aligns;
+
+ for (i = 0; i < dummy_buffer->nums; i++) {
+ buffer = &vin_dev->dummy_buffer[dummy_module].buffer[i];
+ buffer->width = fmt->width;
+ buffer->height = fmt->height;
+ buffer->mcode = fmt->code;
+ if (i == STF_VIN_PAD_SINK) {
+ aligns = ALIGN(fmt->width * 4, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+ buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height);
+ } else if (i == STF_ISP_PAD_SRC
+ || i == STF_ISP_PAD_SRC_SS0
+ || i == STF_ISP_PAD_SRC_SS1) {
+ aligns = ALIGN(fmt->width, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+ buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height * 3 / 2);
+ } else if (i == STF_ISP_PAD_SRC_ITIW
+ || i == STF_ISP_PAD_SRC_ITIR) {
+ aligns = ALIGN(fmt->width, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+ buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height * 3);
+ } else if (i == STF_ISP_PAD_SRC_RAW) {
+ aligns = ALIGN(fmt->width * ISP_RAW_DATA_BITS / 8,
+ STFCAMSS_FRAME_WIDTH_ALIGN_128);
+ buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height);
+ } else if (i == STF_ISP_PAD_SRC_SCD_Y)
+ buffer->buffer_size = PAGE_ALIGN(ISP_SCD_Y_BUFFER_SIZE);
+ else
+ continue;
+
+ buffer->vaddr = dma_alloc_coherent(dev, buffer->buffer_size,
+ &buffer->paddr[0], GFP_DMA | GFP_KERNEL);
+
+ if (buffer->vaddr) {
+ if (i == STF_ISP_PAD_SRC
+ || i == STF_ISP_PAD_SRC_SS0
+ || i == STF_ISP_PAD_SRC_SS1
+ || i == STF_ISP_PAD_SRC_ITIW
+ || i == STF_ISP_PAD_SRC_ITIR)
+ buffer->paddr[1] = (dma_addr_t)(buffer->paddr[0] +
+ aligns * fmt->height);
+ else if (i == STF_ISP_PAD_SRC_SCD_Y)
+ buffer->paddr[1] = (dma_addr_t)(buffer->paddr[0] +
+ ISP_YHIST_BUFFER_SIZE);
+ else
+ st_debug(ST_VIN, "signal plane\n");
+ }
+ {
+ char szPadName[][32] = {
+ "VIN_PAD_SINK",
+ "ISP_PAD_SRC",
+ "ISP_PAD_SRC_SS0",
+ "ISP_PAD_SRC_SS1",
+ "ISP_PAD_SRC_ITIW",
+ "ISP_PAD_SRC_ITIR",
+ "ISP_PAD_SRC_RAW",
+ "ISP_PAD_SRC_SCD_Y",
+ "Unknown Pad"
+ };
+
+ st_debug(ST_VIN, "%s: i = %d(%s) addr[0] = %llx, addr[1] = %llx, size = %u bytes\n",
+ __func__,
+ i,
+ szPadName[i],
+ buffer->paddr[0],
+ buffer->paddr[1],
+ buffer->buffer_size
+ );
+ }
+ }
+
+ return ret;
+}
+
+static void vin_free_dummy_buffer(struct stf_vin2_dev *vin_dev, int dummy_module)
+{
+ struct device *dev = vin_dev->stfcamss->dev;
+ struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
+ struct vin_dummy_buffer *buffer = NULL;
+ int i;
+
+ for (i = 0; i < dummy_buffer->nums; i++) {
+ buffer = &dummy_buffer->buffer[i];
+ if (buffer->vaddr)
+ dma_free_coherent(dev, buffer->buffer_size,
+ buffer->vaddr, buffer->paddr[0]);
+ memset(buffer, 0, sizeof(struct vin_dummy_buffer));
+ }
+}
+
+static void vin_set_dummy_buffer(struct vin_line *line, u32 pad)
+{
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ int dummy_module = line_to_dummy_module(line);
+ struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
+ struct vin_dummy_buffer *buffer = NULL;
+
+ switch (pad) {
+ case STF_VIN_PAD_SINK:
+ if (line->id == VIN_LINE_WR) {
+ buffer = &dummy_buffer->buffer[STF_VIN_PAD_SINK];
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, buffer->paddr[0]);
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, buffer->paddr[0]);
+ } else {
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC];
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS0];
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS1];
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIW];
+ vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIR];
+ vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_RAW];
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, buffer->paddr[0]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SCD_Y];
+ vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1], AWB_TYPE);
+ }
+ break;
+ case STF_ISP_PAD_SRC:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC];
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_SS0:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS0];
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_SS1:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS1];
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_ITIW:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIW];
+ vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_ITIR:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIR];
+ vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_RAW:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_RAW];
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, buffer->paddr[0]);
+ break;
+ case STF_ISP_PAD_SRC_SCD_Y:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SCD_Y];
+ vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
+ buffer->paddr[0], buffer->paddr[1], AWB_TYPE);
+ break;
+ default:
+ break;
+ }
+}
+
+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);
+ int dummy_module = line_to_dummy_module(line);
+ struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
+ struct v4l2_mbus_framefmt *fmt;
+
+ st_debug(ST_VIN, "%s, %d\n", __func__, __LINE__);
+ fmt = __vin_get_format(line, NULL, STF_VIN_PAD_SINK, V4L2_SUBDEV_FORMAT_ACTIVE);
+ mutex_lock(&dummy_buffer->stream_lock);
+ if (enable) {
+ if (dummy_buffer->stream_count == 0) {
+ vin_alloc_dummy_buffer(vin_dev, fmt, dummy_module);
+ vin_set_dummy_buffer(line, STF_VIN_PAD_SINK);
+ atomic_set(&dummy_buffer->frame_skip, get_frame_skip(line));
+ }
+ dummy_buffer->stream_count++;
+ } else {
+ if (dummy_buffer->stream_count == 1) {
+ vin_free_dummy_buffer(vin_dev, dummy_module);
+ // set buffer addr to zero
+ vin_set_dummy_buffer(line, STF_VIN_PAD_SINK);
+ } else
+ vin_set_dummy_buffer(line,
+ stf_vin_map_isp_pad(line->id, STF_ISP_PAD_SINK));
+
+ dummy_buffer->stream_count--;
+ }
+ mutex_unlock(&dummy_buffer->stream_lock);
+
+ mutex_lock(&line->stream_lock);
+ if (enable) {
+ if (line->stream_count == 0) {
+ if (line->id == VIN_LINE_WR) {
+ 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) {
+ if (line->id == VIN_LINE_WR) {
+ 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 = line->formats[0].code;
+
+ fmt->width = clamp_t(u32,
+ fmt->width, STFCAMSS_FRAME_MIN_WIDTH, STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height, STFCAMSS_FRAME_MIN_HEIGHT, STFCAMSS_FRAME_MAX_HEIGHT);
+
+ 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;
+
+ st_debug(ST_VIDEO, "%s, pad %d, fmt code %x\n",
+ __func__, fmt->pad, fmt->format.code);
+
+ format = __vin_get_format(line, state, fmt->pad, fmt->which);
+ if (format == NULL)
+ return -EINVAL;
+
+ mutex_lock(&line->stream_lock);
+ if (line->stream_count) {
+ fmt->format = *format;
+ mutex_unlock(&line->stream_lock);
+ goto out;
+ } else {
+ vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&line->stream_lock);
+
+ 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);
+ }
+
+out:
+ 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;
+
+ 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
+ return;
+
+ if (output->buf[1])
+ pong_addr = output->buf[1]->addr[0];
+ else
+ pong_addr = ping_addr;
+
+ switch (stf_vin_map_isp_line(line->id)) {
+ case STF_ISP_LINE_SRC:
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_SS0:
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_SS1:
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_ITIW:
+ vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
+ y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_ITIR:
+ vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
+ y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_RAW:
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, y_addr);
+ break;
+ case STF_ISP_LINE_SRC_SCD_Y:
+ output->frame_skip = ISP_AWB_OECF_SKIP_FRAME;
+ vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
+ y_addr, uv_addr, AWB_TYPE);
+ break;
+ default:
+ if (line->id == VIN_LINE_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;
+ }
+}
+
+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);
+ INIT_LIST_HEAD(&output->ready_bufs);
+}
+
+static void vin_buf_add_ready(struct vin_output *output,
+ struct stfcamss_buffer *buffer)
+{
+ INIT_LIST_HEAD(&buffer->queue);
+ list_add_tail(&buffer->queue, &output->ready_bufs);
+}
+
+static struct stfcamss_buffer *vin_buf_get_ready(struct vin_output *output)
+{
+ struct stfcamss_buffer *buffer = NULL;
+
+ if (!list_empty(&output->ready_bufs)) {
+ buffer = list_first_entry(&output->ready_bufs,
+ struct stfcamss_buffer,
+ queue);
+ list_del(&buffer->queue);
+ }
+
+ return buffer;
+}
+
+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;
+}
+
+#ifdef UNUSED_CODE
+static void vin_output_checkpending(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+
+ 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);
+ }
+}
+#endif
+
+static void vin_buf_update_on_last(struct vin_line *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 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)
+{
+#ifdef VIN_TWO_BUFFER
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ int inactive_idx;
+#endif
+
+ switch (output->state) {
+ case VIN_OUTPUT_SINGLE:
+#ifdef VIN_TWO_BUFFER
+ int 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(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);
+ }
+ list_for_each_entry_safe(buf, t, &output->ready_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;
+ unsigned long flags;
+ u64 ts = ktime_get_ns();
+ struct v4l2_event event = {
+ .type = V4L2_EVENT_FRAME_SYNC,
+ };
+
+ if (output->state == VIN_OUTPUT_OFF
+ || output->state == VIN_OUTPUT_RESERVED)
+ return;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ while ((ready_buf = vin_buf_get_ready(output))) {
+ if (line->id >= VIN_LINE_ISP && line->id <= VIN_LINE_ISP_SS1) {
+ event.u.frame_sync.frame_sequence = output->sequence;
+ v4l2_event_queue(line->subdev.devnode, &event);
+ }
+
+ ready_buf->vb.vb2_buf.timestamp = ts;
+ ready_buf->vb.sequence = output->sequence++;
+
+ /* The stf_isp_ctrl currently buffered with mmap,
+ * which will not update cache by default.
+ * Flush L2 cache to make sure data is updated.
+ */
+ if (ready_buf->vb.vb2_buf.memory == VB2_MEMORY_MMAP)
+ sifive_l2_flush64_range(ready_buf->addr[0], ready_buf->sizeimage);
+
+ vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+ }
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+}
+
+static void vin_change_buffer(struct vin_line *line)
+{
+ struct stfcamss_buffer *ready_buf;
+ struct vin_output *output = &line->output;
+ struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
+ dma_addr_t *new_addr;
+ unsigned long flags;
+ u32 active_index;
+ int scd_type;
+
+ if (output->state == VIN_OUTPUT_OFF
+ || output->state == VIN_OUTPUT_STOPPING
+ || output->state == VIN_OUTPUT_RESERVED
+ || output->state == VIN_OUTPUT_IDLE)
+ return;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ 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);
+ }
+
+ if (output->state == VIN_OUTPUT_STOPPING)
+ output->last_buffer = ready_buf;
+ else {
+ switch (stf_vin_map_isp_line(line->id)) {
+ case STF_ISP_LINE_SRC:
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ new_addr[0], new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_SS0:
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ new_addr[0], new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_SS1:
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ new_addr[0], new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_ITIW:
+ vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
+ new_addr[0], new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_ITIR:
+ vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
+ new_addr[0], new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_RAW:
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, new_addr[0]);
+ break;
+ case STF_ISP_LINE_SRC_SCD_Y:
+ scd_type = vin_dev->hw_ops->vin_isp_get_scd_type(vin_dev);
+ ready_buf->vb.flags &= ~(V4L2_BUF_FLAG_PFRAME | V4L2_BUF_FLAG_BFRAME);
+ if (scd_type == AWB_TYPE)
+ ready_buf->vb.flags |= V4L2_BUF_FLAG_PFRAME;
+ else
+ ready_buf->vb.flags |= V4L2_BUF_FLAG_BFRAME;
+ if (!output->frame_skip) {
+ output->frame_skip = ISP_AWB_OECF_SKIP_FRAME;
+ scd_type = scd_type == AWB_TYPE ? OECF_TYPE : AWB_TYPE;
+ } else {
+ output->frame_skip--;
+ scd_type = scd_type == AWB_TYPE ? AWB_TYPE : OECF_TYPE;
+ }
+ vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
+ new_addr[0], new_addr[1], scd_type);
+ break;
+ default:
+ if (line->id == VIN_LINE_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;
+ }
+
+ vin_buf_add_ready(output, ready_buf);
+ }
+
+ 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(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_pad_remote_pad_first(local))
+ return -EBUSY;
+ return 0;
+}
+
+static int stf_vin_subscribe_event(struct v4l2_subdev *sd,
+ struct v4l2_fh *fh,
+ struct v4l2_event_subscription *sub)
+{
+ switch (sub->type) {
+ case V4L2_EVENT_FRAME_SYNC:
+ return v4l2_event_subscribe(fh, sub, 0, NULL);
+ default:
+ st_debug(ST_VIN, "unsupport subscribe_event\n");
+ return -EINVAL;
+ }
+}
+
+static const struct v4l2_subdev_core_ops vin_core_ops = {
+ .s_power = vin_set_power,
+ .subscribe_event = stf_vin_subscribe_event,
+ .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+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 stfcamss_video *video_out;
+ struct media_pad *pads;
+ int ret;
+ int i;
+
+ for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
+ char name[32];
+ char *sub_name = get_line_subdevname(i);
+ int is_mp;
+
+#ifdef STF_CAMSS_SKIP_ITI
+ if ((stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC_ITIW) ||
+ (stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC_ITIR))
+ continue;
+#endif
+ is_mp = (stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC) ? 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 | V4L2_SUBDEV_FL_HAS_EVENTS;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
+ STF_VIN_NAME, 0, 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 < STF_DUMMY_MODULE_NUMS; i++)
+ mutex_destroy(&vin_dev->dummy_buffer[i].stream_lock);
+
+ for (i = 0; i < STF_ISP_LINE_MAX + 1; 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
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_vin.h
@@ -0,0 +1,182 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 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_NAME "stf_vin"
+
+#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;
+};
+
+struct vin2_format_table {
+ const struct vin2_format *fmts;
+ int nfmts;
+};
+
+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;
+ struct list_head ready_bufs;
+ enum vin_output_state state;
+ unsigned int sequence;
+ unsigned int frame_skip;
+};
+
+/* The vin output lines include all isp controller lines,
+ * and one vin_wr output line.
+ */
+enum vin_line_id {
+ VIN_LINE_NONE = -1,
+ VIN_LINE_WR = 0,
+ VIN_LINE_ISP = 1,
+ VIN_LINE_ISP_SS0 = 2,
+ VIN_LINE_ISP_SS1 = 3,
+ VIN_LINE_ISP_ITIW = 4,
+ VIN_LINE_ISP_ITIR = 5,
+ VIN_LINE_ISP_RAW = 6,
+ VIN_LINE_ISP_SCD_Y = 7,
+ VIN_LINE_MAX = 8,
+};
+
+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;
+#ifdef CONFIG_PM_SLEEP
+ int pm_stream_count;
+ int pm_power_count;
+#endif
+};
+
+struct stf_vin2_dev;
+
+struct vin_hw_ops {
+ 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,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_raw_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t raw_addr);
+ void (*vin_isp_set_ss0_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_ss1_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_itiw_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_itir_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_scd_addr)(struct stf_vin2_dev *vin_dev,
+ dma_addr_t yhist_addr,
+ dma_addr_t scd_addr, int scd_type);
+ int (*vin_isp_get_scd_type)(struct stf_vin2_dev *vin_dev);
+ irqreturn_t (*vin_wr_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_csi_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_scd_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_irq_csiline_handler)(int irq, void *priv);
+ void (*isr_buffer_done)(struct vin_line *line,
+ struct vin_params *params);
+ void (*isr_change_buffer)(struct vin_line *line);
+};
+
+#define ISP_DUMMY_BUFFER_NUMS STF_ISP_PAD_MAX
+#define VIN_DUMMY_BUFFER_NUMS 1
+
+enum {
+ STF_DUMMY_VIN,
+ STF_DUMMY_ISP,
+ STF_DUMMY_MODULE_NUMS,
+};
+
+struct vin_dummy_buffer {
+ dma_addr_t paddr[3];
+ void *vaddr;
+ u32 buffer_size;
+ u32 width;
+ u32 height;
+ u32 mcode;
+};
+
+struct dummy_buffer {
+ struct vin_dummy_buffer *buffer;
+ u32 nums;
+ struct mutex stream_lock;
+ int stream_count;
+ atomic_t frame_skip;
+};
+
+struct stf_vin2_dev {
+ struct stfcamss *stfcamss;
+ struct vin_line line[VIN_LINE_MAX];
+ struct dummy_buffer dummy_buffer[STF_DUMMY_MODULE_NUMS];
+ struct vin_hw_ops *hw_ops;
+ atomic_t ref_count;
+ struct mutex power_lock;
+ int power_count;
+};
+
+extern void sifive_l2_flush64_range(unsigned long start, unsigned long len);
+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);
+extern enum isp_pad_id stf_vin_map_isp_pad(enum vin_line_id line,
+ enum isp_pad_id def);
+
+#endif /* STF_VIN_H */
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stf_vin_hw_ops.c
@@ -0,0 +1,433 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 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 void vin_intr_clear(void __iomem *sysctrl_base)
+{
+ reg_set_bit(sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN,
+ 0x1);
+ reg_set_bit(sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN,
+ 0x0);
+}
+
+static irqreturn_t stf_vin_wr_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;
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[STF_DUMMY_VIN];
+
+ if (atomic_dec_if_positive(&dummy_buffer->frame_skip) < 0) {
+ vin_dev->hw_ops->isr_change_buffer(&vin_dev->line[VIN_LINE_WR]);
+ vin_dev->hw_ops->isr_buffer_done(&vin_dev->line[VIN_LINE_WR], &params);
+ }
+
+ vin_intr_clear(vin->sysctrl_base);
+
+ return IRQ_HANDLED;
+}
+
+static void __iomem *stf_vin_get_ispbase(struct stf_vin_dev *vin)
+{
+ void __iomem *base = vin->isp_base;
+
+ return base;
+}
+
+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;
+
+ ispbase = stf_vin_get_ispbase(vin);
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+ if (int_status & BIT(24)) {
+ if ((int_status & BIT(11)))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_SS0], &params);
+
+ if ((int_status & BIT(12)))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_SS1], &params);
+
+ if ((int_status & BIT(20)))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP], &params);
+
+ value = reg_read(ispbase, ISP_REG_ITIDPSR);
+ if ((value & BIT(17)))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_ITIW], &params);
+ if ((value & BIT(16)))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_ITIR], &params);
+
+#ifndef ISP_USE_CSI_AND_SC_DONE_INTERRUPT
+ if (int_status & BIT(25))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_RAW], &params);
+
+ if (int_status & BIT(26))
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_SCD_Y], &params);
+
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0, (int_status & ~EN_INT_ALL)
+ | EN_INT_ISP_DONE | EN_INT_CSI_DONE | EN_INT_SC_DONE);
+#else
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0,
+ (int_status & ~EN_INT_ALL) | EN_INT_ISP_DONE);
+#endif
+ } else
+ st_debug(ST_VIN, "%s, Unknown interrupt!!!\n", __func__);
+
+ return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_csi_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;
+
+ ispbase = stf_vin_get_ispbase(vin);
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+ if (int_status & BIT(25)) {
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_RAW], &params);
+
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0,
+ (int_status & ~EN_INT_ALL) | EN_INT_CSI_DONE);
+ } else
+ st_debug(ST_VIN, "%s, Unknown interrupt!!!\n", __func__);
+
+ return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_scd_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;
+
+ ispbase = stf_vin_get_ispbase(vin);
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+ if (int_status & BIT(26)) {
+ vin_dev->hw_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_SCD_Y], &params);
+
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0, (int_status & ~EN_INT_ALL) | EN_INT_SC_DONE);
+ } else
+ st_debug(ST_VIN, "%s, Unknown interrupt!!!\n", __func__);
+
+ return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_irq_csiline_handler(int irq, void *priv)
+{
+ struct stf_vin2_dev *vin_dev = priv;
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ struct stf_isp_dev *isp_dev;
+ void __iomem *ispbase;
+ u32 int_status, value;
+
+ ispbase = stf_vin_get_ispbase(vin);
+
+ isp_dev = vin_dev->stfcamss->isp_dev;
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+ if (int_status & BIT(27)) {
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[STF_DUMMY_ISP];
+
+ if (!atomic_read(&isp_dev->shadow_count)) {
+ if (atomic_dec_if_positive(&dummy_buffer->frame_skip) < 0) {
+ if ((int_status & BIT(11)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_SS0]);
+ if ((int_status & BIT(12)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_SS1]);
+ if ((int_status & BIT(20)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP]);
+
+ value = reg_read(ispbase, ISP_REG_ITIDPSR);
+ if ((value & BIT(17)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_ITIW]);
+ if ((value & BIT(16)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_ITIR]);
+
+ value = reg_read(ispbase, ISP_REG_CSI_MODULE_CFG);
+ if ((value & BIT(19)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_RAW]);
+ if ((value & BIT(17)))
+ vin_dev->hw_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_SCD_Y]);
+ }
+
+ // shadow update
+ reg_set_bit(ispbase, ISP_REG_CSIINTS_ADDR, 0x30000, 0x30000);
+ reg_set_bit(ispbase, ISP_REG_IESHD_ADDR, BIT(1) | BIT(0), 0x3);
+ } else {
+ st_warn(ST_VIN, "isp shadow_lock locked. skip this frame\n");
+ }
+
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0,
+ (int_status & ~EN_INT_ALL) | EN_INT_LINE_INT);
+ } else
+ st_debug(ST_VIN, "%s, Unknown interrupt!!!\n", __func__);
+
+ return IRQ_HANDLED;
+}
+
+static int stf_vin_clk_enable(struct stf_vin2_dev *vin_dev)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_PCLK].clk);
+ clk_set_rate(stfcamss->sys_clk[STFCLK_APB_FUNC].clk, 49500000);
+ clk_set_rate(stfcamss->sys_clk[STFCLK_SYS_CLK].clk, 297000000);
+
+ reset_control_deassert(stfcamss->sys_rst[STFRST_PCLK].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_SYS_CLK].rstc);
+
+ return 0;
+}
+
+
+static int stf_vin_clk_disable(struct stf_vin2_dev *vin_dev)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ reset_control_assert(stfcamss->sys_rst[STFRST_PCLK].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_SYS_CLK].rstc);
+
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_PCLK].clk);
+
+ return 0;
+}
+
+static int stf_vin_config_set(struct stf_vin2_dev *vin_dev)
+{
+ return 0;
+}
+
+static int stf_vin_wr_stream_set(struct stf_vin2_dev *vin_dev, int on)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ //make the axiwr alway on
+ if (on)
+ reg_set(vin->sysctrl_base, SYSCONSAIF_SYSCFG_20, U0_VIN_CNFG_AXIWR0_EN);
+
+ return 0;
+}
+
+static void stf_vin_wr_irq_enable(struct stf_vin2_dev *vin_dev,
+ int enable)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ unsigned int value = 0;
+
+ if (enable) {
+ value = ~(0x1 << 1);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_MASK,
+ value);
+ } else {
+ /* clear vin interrupt */
+ value = 0x1 << 1;
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN,
+ 0x1);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN,
+ 0x0);
+ reg_set_bit(vin->sysctrl_base, SYSCONSAIF_SYSCFG_28,
+ U0_VIN_CNFG_AXIWR0_MASK,
+ value);
+ }
+}
+
+static void stf_vin_wr_rd_set_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t wr_addr, dma_addr_t rd_addr)
+{
+#ifdef UNUSED_CODE
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ /* 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)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ /* set the start address */
+ reg_write(vin->sysctrl_base, SYSCONSAIF_SYSCFG_24, (long)addr);
+}
+
+void stf_vin_wr_set_pong_addr(struct stf_vin2_dev *vin_dev, dma_addr_t addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+
+ /* set the start address */
+ reg_write(vin->sysctrl_base, SYSCONSAIF_SYSCFG_32, (long)addr);
+}
+
+void stf_vin_isp_set_yuv_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_write(ispbase, ISP_REG_Y_PLANE_START_ADDR, y_addr);
+ reg_write(ispbase, ISP_REG_UV_PLANE_START_ADDR, uv_addr);
+ // reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(0), 1);
+}
+
+void stf_vin_isp_set_raw_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t raw_addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_write(ispbase, ISP_REG_DUMP_CFG_0, raw_addr);
+}
+
+void stf_vin_isp_set_ss0_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_write(ispbase, ISP_REG_SS0AY, y_addr);
+ reg_write(ispbase, ISP_REG_SS0AUV, uv_addr);
+}
+
+void stf_vin_isp_set_ss1_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_write(ispbase, ISP_REG_SS1AY, y_addr);
+ reg_write(ispbase, ISP_REG_SS1AUV, uv_addr);
+}
+
+void stf_vin_isp_set_itiw_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_write(ispbase, ISP_REG_ITIDWYSAR, y_addr);
+ reg_write(ispbase, ISP_REG_ITIDWUSAR, uv_addr);
+}
+
+void stf_vin_isp_set_itir_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_write(ispbase, ISP_REG_ITIDRYSAR, y_addr);
+ reg_write(ispbase, ISP_REG_ITIDRUSAR, uv_addr);
+}
+
+int stf_vin_isp_get_scd_type(struct stf_vin2_dev *vin_dev)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ return (reg_read(ispbase, ISP_REG_SC_CFG_1) & (0x3 << 30)) >> 30;
+}
+
+void stf_vin_isp_set_scd_addr(struct stf_vin2_dev *vin_dev,
+ dma_addr_t yhist_addr, dma_addr_t scd_addr, int scd_type)
+{
+ struct stf_vin_dev *vin = vin_dev->stfcamss->vin;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin);
+
+ reg_set_bit(ispbase, ISP_REG_SC_CFG_1, 0x3 << 30, scd_type << 30);
+ reg_write(ispbase, ISP_REG_SCD_CFG_0, scd_addr);
+ reg_write(ispbase, ISP_REG_YHIST_CFG_4, yhist_addr);
+}
+
+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);
+}
+
+struct vin_hw_ops vin_ops = {
+ .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,
+ .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_isp_set_ss0_addr = stf_vin_isp_set_ss0_addr,
+ .vin_isp_set_ss1_addr = stf_vin_isp_set_ss1_addr,
+ .vin_isp_set_itiw_addr = stf_vin_isp_set_itiw_addr,
+ .vin_isp_set_itir_addr = stf_vin_isp_set_itir_addr,
+ .vin_isp_set_scd_addr = stf_vin_isp_set_scd_addr,
+ .vin_isp_get_scd_type = stf_vin_isp_get_scd_type,
+ .vin_wr_irq_handler = stf_vin_wr_irq_handler,
+ .vin_isp_irq_handler = stf_vin_isp_irq_handler,
+ .vin_isp_csi_irq_handler = stf_vin_isp_csi_irq_handler,
+ .vin_isp_scd_irq_handler = stf_vin_isp_scd_irq_handler,
+ .vin_isp_irq_csiline_handler = stf_vin_isp_irq_csiline_handler,
+};
--- /dev/null
+++ b/drivers/media/platform/starfive/v4l2_driver/stfcamss.c
@@ -0,0 +1,1369 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#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/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/syscon.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
+EXPORT_SYMBOL_GPL(stdbg_level);
+EXPORT_SYMBOL_GPL(stdbg_mask);
+
+static const struct reg_name mem_reg_name[] = {
+ {"csi2rx"},
+ {"vclk"},
+ {"vrst"},
+ {"sctrl"},
+ {"isp"},
+ {"trst"},
+ {"pmu"},
+ {"syscrg"},
+};
+
+static struct clk_bulk_data stfcamss_clocks[] = {
+ { .id = "clk_apb_func" },
+ { .id = "clk_pclk" },
+ { .id = "clk_sys_clk" },
+ { .id = "clk_wrapper_clk_c" },
+ { .id = "clk_dvp_inv" },
+ { .id = "clk_axiwr" },
+ { .id = "clk_mipi_rx0_pxl" },
+ { .id = "clk_pixel_clk_if0" },
+ { .id = "clk_pixel_clk_if1" },
+ { .id = "clk_pixel_clk_if2" },
+ { .id = "clk_pixel_clk_if3" },
+ { .id = "clk_m31dphy_cfgclk_in" },
+ { .id = "clk_m31dphy_refclk_in" },
+ { .id = "clk_m31dphy_txclkesc_lan0" },
+ { .id = "clk_ispcore_2x" },
+ { .id = "clk_isp_axi" },
+};
+
+static struct reset_control_bulk_data stfcamss_resets[] = {
+ { .id = "rst_wrapper_p" },
+ { .id = "rst_wrapper_c" },
+ { .id = "rst_pclk" },
+ { .id = "rst_sys_clk" },
+ { .id = "rst_axird" },
+ { .id = "rst_axiwr" },
+ { .id = "rst_pixel_clk_if0" },
+ { .id = "rst_pixel_clk_if1" },
+ { .id = "rst_pixel_clk_if2" },
+ { .id = "rst_pixel_clk_if3" },
+ { .id = "rst_m31dphy_hw" },
+ { .id = "rst_m31dphy_b09_always_on" },
+ { .id = "rst_isp_top_n" },
+ { .id = "rst_isp_top_axi" },
+};
+
+int stfcamss_get_mem_res(struct platform_device *pdev, struct stf_vin_dev *vin)
+{
+ struct device *dev = &pdev->dev;
+ struct resource *res;
+ 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);
+
+ if (!res)
+ return -EINVAL;
+
+ if (!strcmp(name, "csi2rx")) {
+ vin->csi2rx_base = devm_ioremap_resource(dev, res);
+ if (IS_ERR(vin->csi2rx_base))
+ return PTR_ERR(vin->csi2rx_base);
+ } else if (!strcmp(name, "vclk")) {
+ vin->clkgen_base = ioremap(res->start, resource_size(res));
+ if (!vin->clkgen_base)
+ return -ENOMEM;
+ } else if (!strcmp(name, "vrst")) {
+ vin->rstgen_base = devm_ioremap_resource(dev, res);
+ if (IS_ERR(vin->rstgen_base))
+ return PTR_ERR(vin->rstgen_base);
+ } else if (!strcmp(name, "sctrl")) {
+ vin->sysctrl_base = devm_ioremap_resource(dev, res);
+ if (IS_ERR(vin->sysctrl_base))
+ return PTR_ERR(vin->sysctrl_base);
+ } else if (!strcmp(name, "isp")) {
+ vin->isp_base = devm_ioremap_resource(dev, res);
+ if (IS_ERR(vin->isp_base))
+ return PTR_ERR(vin->isp_base);
+ } else if (!strcmp(name, "trst")) {
+ vin->vin_top_rstgen_base = devm_ioremap_resource(dev, res);
+ if (IS_ERR(vin->vin_top_rstgen_base))
+ return PTR_ERR(vin->vin_top_rstgen_base);
+ } else if (!strcmp(name, "pmu")) {
+ vin->pmu_test = ioremap(res->start, resource_size(res));
+ if (!vin->pmu_test)
+ return -ENOMEM;
+ } else if (!strcmp(name, "syscrg")) {
+ vin->sys_crg = ioremap(res->start, resource_size(res));
+ if (!vin->sys_crg)
+ return -ENOMEM;
+ } 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) {
+ if (!entity->pads)
+ return NULL;
+
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ return NULL;
+
+ pad = media_pad_remote_pad_first(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_mbus_config_parallel *parallel_bus = &vep.bus.parallel;
+ struct v4l2_mbus_config_mipi_csi2 *csi2_bus = &vep.bus.mipi_csi2;
+ struct dvp_cfg *dvp = &csd->interface.dvp;
+ struct csi2phy_cfg *csiphy = &csd->interface.csiphy;
+
+ 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 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 CSI2RX_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_nf_add_fwnode(&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;
+
+ 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;
+ }
+
+ ret = stf_csiphy_subdev_init(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_csiphy sub-device: %d\n",
+ ret);
+ return ret;
+ }
+
+ ret = stf_csi_subdev_init(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_csi sub-device: %d\n",
+ ret);
+ return ret;
+ }
+
+ ret = stf_isp_subdev_init(stfcamss);
+ 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;
+ 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%d entity: %d\n",
+ 0, ret);
+ goto err_reg_dvp;
+ }
+
+ ret = stf_csiphy_register(csiphy_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf csiphy%d entity: %d\n",
+ 0, ret);
+ goto err_reg_csiphy;
+ }
+
+ ret = stf_csi_register(csi_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf csi%d entity: %d\n",
+ 0, ret);
+ goto err_reg_csi;
+ }
+
+ ret = stf_isp_register(isp_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf isp%d entity: %d\n",
+ 0, 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;
+ }
+
+ ret = media_create_pad_link(
+ &csi_dev->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->subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &csiphy_dev->subdev.entity,
+ STF_CSIPHY_PAD_SRC,
+ &csi_dev->subdev.entity,
+ STF_CSI_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",
+ csiphy_dev->subdev.entity.name,
+ csi_dev->subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC,
+ &vin_dev->line[VIN_LINE_ISP].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_SS0,
+ &vin_dev->line[VIN_LINE_ISP_SS0].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_SS0]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_SS1,
+ &vin_dev->line[VIN_LINE_ISP_SS1].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_SS1]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+#ifndef STF_CAMSS_SKIP_ITI
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_ITIW,
+ &vin_dev->line[VIN_LINE_ISP_ITIW].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_ITIW]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_ITIR,
+ &vin_dev->line[VIN_LINE_ISP_ITIR].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_ITIR]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+#endif
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_RAW,
+ &vin_dev->line[VIN_LINE_ISP_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->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_RAW]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_SCD_Y,
+ &vin_dev->line[VIN_LINE_ISP_SCD_Y].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_SCD_Y]
+ .subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &dvp_dev->subdev.entity,
+ STF_DVP_PAD_SRC,
+ &isp_dev->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->subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &csi_dev->subdev.entity,
+ STF_CSI_PAD_SRC,
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ csi_dev->subdev.entity.name,
+ isp_dev->subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ return ret;
+
+err_link:
+ stf_vin_unregister(stfcamss->vin_dev);
+err_reg_vin:
+ stf_isp_unregister(stfcamss->isp_dev);
+err_reg_isp:
+ stf_csi_unregister(stfcamss->csi_dev);
+err_reg_csi:
+ stf_csiphy_unregister(stfcamss->csiphy_dev);
+err_reg_csiphy:
+ stf_dvp_unregister(stfcamss->dvp_dev);
+err_reg_dvp:
+ return ret;
+}
+
+static void stfcamss_unregister_subdevices(struct stfcamss *stfcamss)
+{
+ stf_dvp_unregister(stfcamss->dvp_dev);
+ stf_csiphy_unregister(stfcamss->csiphy_dev);
+ stf_csi_unregister(stfcamss->csi_dev);
+ stf_isp_unregister(stfcamss->isp_dev);
+ 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);
+ 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_connection *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_dvp_dev *dvp_dev = stfcamss->dvp_dev;
+ struct stf_csiphy_dev *csiphy_dev = stfcamss->csiphy_dev;
+
+ switch (port) {
+ case DVP_SENSOR_PORT_NUMBER:
+ dvp_dev->dvp = &csd->interface.dvp;
+ subdev->host_priv = &dvp_dev->subdev.entity;
+ break;
+ case CSI2RX_SENSOR_PORT_NUMBER:
+ csiphy_dev->csiphy = &csd->interface.csiphy;
+ subdev->host_priv = &csiphy_dev->subdev.entity;
+ break;
+ default:
+ break;
+ };
+
+ stfcamss_register_mediadevice_subdevnodes(async, subdev);
+
+ return 0;
+}
+
+#ifdef UNUSED_CODE
+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);
+}
+#endif
+
+static const struct v4l2_async_notifier_operations
+stfcamss_subdev_notifier_ops = {
+ .bound = stfcamss_subdev_notifier_bound,
+};
+
+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,
+ ISP_MODULE,
+ CSI_MODULE,
+ CSIPHY_MODULE,
+ DVP_MODULE,
+ CLK_MODULE,
+};
+
+static enum module_id id_num = ISP_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_ISP_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)
+{
+ struct device *dev = file->private_data;
+ 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 *isp_dev = stfcamss->isp_dev;
+ struct stf_csi_dev *csi0_dev = stfcamss->csi_dev;
+
+ 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 ISP_MODULE:
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count > 0) {
+ reg_base = vin->isp_base;
+ dump_isp_reg(reg_base);
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+ break;
+ case CSI_MODULE:
+ mutex_lock(&csi0_dev->stream_lock);
+ if (csi0_dev->stream_count > 0) {
+ reg_base = vin->csi2rx_base;
+ dump_csi_reg(reg_base);
+ }
+ mutex_unlock(&csi0_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 *isp_dev = stfcamss->isp_dev;
+ struct stf_csi_dev *csi_dev = stfcamss->csi_dev;
+ 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 ISP_MODULE:
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count > 0) {
+ reg_base = vin->isp_base;
+ print_reg(ST_ISP, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_ISP, reg_base, offset);
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+ break;
+ case CSI_MODULE:
+ mutex_lock(&csi_dev->stream_lock);
+ if (csi_dev->stream_count > 0) {
+ reg_base = vin->csi2rx_base;
+ print_reg(ST_CSI, reg_base, offset);
+ reg_write(reg_base, offset, val);
+ print_reg(ST_CSI, reg_base, offset);
+ }
+ mutex_unlock(&csi_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);
+ 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: %lu, count: %lu\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 *dev = &pdev->dev;
+ struct of_phandle_args args;
+ int ret = 0, num_subdevs;
+
+ dev_info(dev, "stfcamss probe enter!\n");
+
+ stfcamss = devm_kzalloc(dev, sizeof(struct stfcamss), GFP_KERNEL);
+ if (!stfcamss)
+ return -ENOMEM;
+
+ 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,
+ sizeof(*stfcamss->csiphy_dev),
+ GFP_KERNEL);
+ if (!stfcamss->csiphy_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->csi_dev = devm_kzalloc(dev,
+ sizeof(*stfcamss->csi_dev),
+ GFP_KERNEL);
+ if (!stfcamss->csi_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->isp_dev = devm_kzalloc(dev,
+ 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->isp_irq = platform_get_irq(pdev, 1);
+ if (vin->isp_irq <= 0) {
+ st_err(ST_CAMSS, "Could not get isp irq\n");
+ goto err_cam;
+ }
+
+ vin->isp_csi_irq = platform_get_irq(pdev, 2);
+ if (vin->isp_csi_irq <= 0) {
+ st_err(ST_CAMSS, "Could not get isp csi irq\n");
+ goto err_cam;
+ }
+
+ vin->isp_scd_irq = platform_get_irq(pdev, 3);
+ if (vin->isp_scd_irq <= 0) {
+ st_err(ST_CAMSS, "Could not get isp scd irq\n");
+ goto err_cam;
+ }
+
+ vin->isp_irq_csiline = platform_get_irq(pdev, 4);
+ if (vin->isp_irq_csiline <= 0) {
+ st_err(ST_CAMSS, "Could not get isp irq csiline\n");
+ goto err_cam;
+ }
+
+ pm_runtime_enable(dev);
+
+ stfcamss->nclks = ARRAY_SIZE(stfcamss_clocks);
+ stfcamss->sys_clk = stfcamss_clocks;
+
+ ret = devm_clk_bulk_get(dev, stfcamss->nclks, stfcamss->sys_clk);
+ if (ret) {
+ st_err(ST_CAMSS, "Failed to get clk controls\n");
+ return ret;
+ }
+
+ stfcamss->nrsts = ARRAY_SIZE(stfcamss_resets);
+ stfcamss->sys_rst = stfcamss_resets;
+
+ ret = devm_reset_control_bulk_get_shared(dev, stfcamss->nrsts,
+ stfcamss->sys_rst);
+ if (ret) {
+ st_err(ST_CAMSS, "Failed to get reset controls\n");
+ return ret;
+ }
+
+ ret = of_parse_phandle_with_fixed_args(dev->of_node,
+ "starfive,aon-syscon", 1, 0, &args);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to parse starfive,aon-syscon\n");
+ return -EINVAL;
+ }
+
+ stfcamss->stf_aon_syscon = syscon_node_to_regmap(args.np);
+ of_node_put(args.np);
+ if (IS_ERR(stfcamss->stf_aon_syscon))
+ return PTR_ERR(stfcamss->stf_aon_syscon);
+
+ stfcamss->aon_gp_reg = args.args[0];
+
+ 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_nf_init(&stfcamss->notifier, &stfcamss->v4l2_dev);
+
+ 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));
+ strscpy(stfcamss->media_dev.serial, "0123456789ABCDEF",
+ sizeof(stfcamss->media_dev.serial));
+ snprintf(stfcamss->media_dev.bus_info, sizeof(stfcamss->media_dev.bus_info),
+ "%s:%s", dev_bus_name(dev), pdev->name);
+ stfcamss->media_dev.hw_revision = 0x01;
+ 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_nf_register(&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",
+ 0644, stfcamss->debugfs_entry,
+ (void *)dev, &vin_debug_fops);
+ debugfs_create_u32("dbg_level",
+ 0644, stfcamss->debugfs_entry,
+ &stdbg_level);
+ debugfs_create_u32("dbg_mask",
+ 0644, stfcamss->debugfs_entry,
+ &stdbg_mask);
+#endif
+ dev_info(dev, "stfcamss probe success!\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_nf_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);
+ pm_runtime_disable(&pdev->dev);
+
+ kfree(stfcamss);
+
+ return 0;
+}
+
+static const struct of_device_id stfcamss_of_match[] = {
+ { .compatible = "starfive,jh7110-vin" },
+ { /* end node */ },
+};
+
+MODULE_DEVICE_TABLE(of, stfcamss_of_match);
+
+#ifdef CONFIG_PM_SLEEP
+static int stfcamss_suspend(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ struct stfcamss_video *video;
+ struct video_device *vdev;
+ int i = 0;
+ int pm_power_count;
+ int pm_stream_count;
+
+ for (i = 0; i < VIN_LINE_MAX; i++) {
+ video = &vin_dev->line[i].video_out;
+ vdev = &vin_dev->line[i].video_out.vdev;
+ vin_dev->line[i].pm_power_count = vin_dev->line[i].power_count;
+ vin_dev->line[i].pm_stream_count = vin_dev->line[i].stream_count;
+ pm_power_count = vin_dev->line[i].pm_power_count;
+ pm_stream_count = vin_dev->line[i].pm_stream_count;
+
+ if (pm_stream_count) {
+ while (pm_stream_count--) {
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(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);
+ }
+ }
+ video_device_pipeline_stop(vdev);
+ video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
+ }
+
+ if (!pm_power_count)
+ continue;
+
+ v4l2_pipeline_pm_put(&vdev->entity);
+ }
+
+ return pm_runtime_force_suspend(dev);
+}
+
+static int stfcamss_resume(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+ struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ struct stfcamss_video *video;
+ struct video_device *vdev;
+ int i = 0;
+ int pm_power_count;
+ int pm_stream_count;
+ int ret = 0;
+
+ pm_runtime_force_resume(dev);
+
+ for (i = 0; i < VIN_LINE_MAX; i++) {
+ video = &vin_dev->line[i].video_out;
+ vdev = &vin_dev->line[i].video_out.vdev;
+ pm_power_count = vin_dev->line[i].pm_power_count;
+ pm_stream_count = vin_dev->line[i].pm_stream_count;
+
+ if (!pm_power_count)
+ continue;
+
+ ret = v4l2_pipeline_pm_get(&vdev->entity);
+ if (ret < 0)
+ goto err;
+
+ if (pm_stream_count) {
+ ret = video_device_pipeline_start(vdev, &video->stfcamss->pipe);
+ if (ret < 0)
+ goto err_pm_put;
+
+ while (pm_stream_count--) {
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(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 err_pipeline_stop;
+ }
+ }
+ }
+ }
+
+ return 0;
+
+err_pipeline_stop:
+ video_device_pipeline_stop(vdev);
+err_pm_put:
+ v4l2_pipeline_pm_put(&vdev->entity);
+err:
+ return ret;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM
+static int stfcamss_runtime_suspend(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+ reset_control_assert(stfcamss->sys_rst[STFRST_ISP_TOP_AXI].rstc);
+ reset_control_assert(stfcamss->sys_rst[STFRST_ISP_TOP_N].rstc);
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_ISP_AXI].clk);
+ clk_disable_unprepare(stfcamss->sys_clk[STFCLK_ISPCORE_2X].clk);
+
+ return 0;
+}
+
+static int stfcamss_runtime_resume(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_ISPCORE_2X].clk);
+ clk_prepare_enable(stfcamss->sys_clk[STFCLK_ISP_AXI].clk);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_ISP_TOP_N].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STFRST_ISP_TOP_AXI].rstc);
+
+ return 0;
+}
+#endif /* CONFIG_PM */
+
+static const struct dev_pm_ops stfcamss_pm_ops = {
+ SET_SYSTEM_SLEEP_PM_OPS(stfcamss_suspend, stfcamss_resume)
+ SET_RUNTIME_PM_OPS(stfcamss_runtime_suspend, stfcamss_runtime_resume, NULL)
+};
+
+static struct platform_driver stfcamss_driver = {
+ .probe = stfcamss_probe,
+ .remove = stfcamss_remove,
+ .driver = {
+ .name = DRV_NAME,
+ .pm = &stfcamss_pm_ops,
+ .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
+++ b/drivers/media/platform/starfive/v4l2_driver/stfcamss.h
@@ -0,0 +1,117 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+
+#ifndef STFCAMSS_H
+#define STFCAMSS_H
+
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/reset.h>
+#include <linux/clk.h>
+
+enum sensor_type {
+ SENSOR_VIN,
+ /* need replace sensor */
+ SENSOR_ISP,
+};
+
+enum subdev_type {
+ VIN_DEV_TYPE,
+ ISP_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
+
+#define STF_CAMSS_SKIP_ITI
+
+enum port_num {
+ DVP_SENSOR_PORT_NUMBER = 0,
+ CSI2RX_SENSOR_PORT_NUMBER
+};
+
+enum stf_clk_num {
+ STFCLK_APB_FUNC = 0,
+ STFCLK_PCLK,
+ STFCLK_SYS_CLK,
+ STFCLK_WRAPPER_CLK_C,
+ STFCLK_DVP_INV,
+ STFCLK_AXIWR,
+ STFCLK_MIPI_RX0_PXL,
+ STFCLK_PIXEL_CLK_IF0,
+ STFCLK_PIXEL_CLK_IF1,
+ STFCLK_PIXEL_CLK_IF2,
+ STFCLK_PIXEL_CLK_IF3,
+ STFCLK_M31DPHY_CFGCLK_IN,
+ STFCLK_M31DPHY_REFCLK_IN,
+ STFCLK_M31DPHY_TXCLKESC_LAN0,
+ STFCLK_ISPCORE_2X,
+ STFCLK_ISP_AXI,
+ STFCLK_NUM
+};
+
+enum stf_rst_num {
+ STFRST_WRAPPER_P = 0,
+ STFRST_WRAPPER_C,
+ STFRST_PCLK,
+ STFRST_SYS_CLK,
+ STFRST_AXIRD,
+ STFRST_AXIWR,
+ STFRST_PIXEL_CLK_IF0,
+ STFRST_PIXEL_CLK_IF1,
+ STFRST_PIXEL_CLK_IF2,
+ STFRST_PIXEL_CLK_IF3,
+ STFRST_M31DPHY_HW,
+ STFRST_M31DPHY_B09_ALWAYS_ON,
+ STFRST_ISP_TOP_N,
+ STFRST_ISP_TOP_AXI,
+ STFRST_NUM
+};
+
+struct stfcamss {
+ struct stf_vin_dev *vin; // stfcamss phy res
+ struct v4l2_device v4l2_dev;
+ struct media_device media_dev;
+ struct media_pipeline pipe;
+ struct device *dev;
+ struct stf_vin2_dev *vin_dev; // subdev
+ struct stf_dvp_dev *dvp_dev; // subdev
+ struct stf_csi_dev *csi_dev; // subdev
+ struct stf_csiphy_dev *csiphy_dev; // subdev
+ struct stf_isp_dev *isp_dev; // subdev
+ struct v4l2_async_notifier notifier;
+ struct clk_bulk_data *sys_clk;
+ int nclks;
+ struct reset_control_bulk_data *sys_rst;
+ int nrsts;
+ struct regmap *stf_aon_syscon;
+ uint32_t aon_gp_reg;
+#ifdef CONFIG_DEBUG_FS
+ struct dentry *debugfs_entry;
+ struct dentry *vin_debugfs;
+#endif
+};
+
+struct stfcamss_async_subdev {
+ struct v4l2_async_connection 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
+++ b/include/uapi/linux/jh7110-isp.h
@@ -0,0 +1,253 @@
+/* SPDX-License-Identifier: ((GPL-2.0+ WITH Linux-syscall-note) OR BSD-3-Clause) */
+/*
+ * jh7110-isp.h
+ *
+ * JH7110 ISP driver - user space header file.
+ *
+ * Copyright © 2023 Starfive Technology Co., Ltd.
+ *
+ * Author: Su Zejian (zejian.su@starfivetech.com)
+ *
+ */
+
+#ifndef __JH7110_ISP_H_
+#define __JH7110_ISP_H_
+
+#include <linux/v4l2-controls.h>
+
+#define V4L2_CID_USER_JH7110_ISP_WB_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0001)
+#define V4L2_CID_USER_JH7110_ISP_CAR_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0002)
+#define V4L2_CID_USER_JH7110_ISP_CCM_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0003)
+#define V4L2_CID_USER_JH7110_ISP_CFA_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0004)
+#define V4L2_CID_USER_JH7110_ISP_CTC_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0005)
+#define V4L2_CID_USER_JH7110_ISP_DBC_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0006)
+#define V4L2_CID_USER_JH7110_ISP_DNYUV_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0007)
+#define V4L2_CID_USER_JH7110_ISP_GMARGB_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0008)
+#define V4L2_CID_USER_JH7110_ISP_LCCF_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x0009)
+#define V4L2_CID_USER_JH7110_ISP_OBC_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x000a)
+#define V4L2_CID_USER_JH7110_ISP_OECF_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x000b)
+#define V4L2_CID_USER_JH7110_ISP_R2Y_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x000c)
+#define V4L2_CID_USER_JH7110_ISP_SAT_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x000d)
+#define V4L2_CID_USER_JH7110_ISP_SHRP_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x000e)
+#define V4L2_CID_USER_JH7110_ISP_YCRV_SETTING \
+ (V4L2_CID_USER_JH7110_ISP_BASE + 0x000f)
+
+struct jh7110_isp_wb_gain {
+ __u16 gain_r;
+ __u16 gain_g;
+ __u16 gain_b;
+};
+
+struct jh7110_isp_wb_setting {
+ __u32 enabled;
+ struct jh7110_isp_wb_gain gains;
+};
+
+struct jh7110_isp_car_setting {
+ __u32 enabled;
+};
+
+struct jh7110_isp_ccm_smlow {
+ __s32 ccm[3][3];
+ __s32 offsets[3];
+};
+
+struct jh7110_isp_ccm_setting {
+ __u32 enabled;
+ struct jh7110_isp_ccm_smlow ccm_smlow;
+};
+
+struct jh7110_isp_cfa_params {
+ __s32 hv_width;
+ __s32 cross_cov;
+};
+
+struct jh7110_isp_cfa_setting {
+ __u32 enabled;
+ struct jh7110_isp_cfa_params settings;
+};
+
+struct jh7110_isp_ctc_params {
+ __u8 saf_mode;
+ __u8 daf_mode;
+ __s32 max_gt;
+ __s32 min_gt;
+};
+
+struct jh7110_isp_ctc_setting {
+ __u32 enabled;
+ struct jh7110_isp_ctc_params settings;
+};
+
+struct jh7110_isp_dbc_params {
+ __s32 bad_gt;
+ __s32 bad_xt;
+};
+
+struct jh7110_isp_dbc_setting {
+ __u32 enabled;
+ struct jh7110_isp_dbc_params settings;
+};
+
+struct jh7110_isp_dnyuv_params {
+ __u8 y_sweight[10];
+ __u16 y_curve[7];
+ __u8 uv_sweight[10];
+ __u16 uv_curve[7];
+};
+
+struct jh7110_isp_dnyuv_setting {
+ __u32 enabled;
+ struct jh7110_isp_dnyuv_params settings;
+};
+
+struct jh7110_isp_gmargb_point {
+ __u16 g_val;
+ __u16 sg_val;
+};
+
+struct jh7110_isp_gmargb_setting {
+ __u32 enabled;
+ struct jh7110_isp_gmargb_point curve[15];
+};
+
+struct jh7110_isp_lccf_circle {
+ __s16 center_x;
+ __s16 center_y;
+ __u8 radius;
+};
+
+struct jh7110_isp_lccf_curve_param {
+ __s16 f1;
+ __s16 f2;
+};
+
+struct jh7110_isp_lccf_setting {
+ __u32 enabled;
+ struct jh7110_isp_lccf_circle circle;
+ struct jh7110_isp_lccf_curve_param r_param;
+ struct jh7110_isp_lccf_curve_param gr_param;
+ struct jh7110_isp_lccf_curve_param gb_param;
+ struct jh7110_isp_lccf_curve_param b_param;
+};
+
+struct jh7110_isp_blacklevel_win_size {
+ __u32 width;
+ __u32 height;
+};
+
+struct jh7110_isp_blacklevel_gain {
+ __u8 tl_gain;
+ __u8 tr_gain;
+ __u8 bl_gain;
+ __u8 br_gain;
+};
+
+struct jh7110_isp_blacklevel_offset {
+ __u8 tl_offset;
+ __u8 tr_offset;
+ __u8 bl_offset;
+ __u8 br_offset;
+};
+
+struct jh7110_isp_blacklevel_setting {
+ __u32 enabled;
+ struct jh7110_isp_blacklevel_win_size win_size;
+ struct jh7110_isp_blacklevel_gain gain[4];
+ struct jh7110_isp_blacklevel_offset offset[4];
+};
+
+struct jh7110_isp_oecf_point {
+ __u16 x;
+ __u16 y;
+ __s16 slope;
+};
+
+struct jh7110_isp_oecf_setting {
+ __u32 enabled;
+ struct jh7110_isp_oecf_point r_curve[16];
+ struct jh7110_isp_oecf_point gr_curve[16];
+ struct jh7110_isp_oecf_point gb_curve[16];
+ struct jh7110_isp_oecf_point b_curve[16];
+};
+
+struct jh7110_isp_r2y_matrix {
+ __s16 m[9];
+};
+
+struct jh7110_isp_r2y_setting {
+ __u32 enabled;
+ struct jh7110_isp_r2y_matrix matrix;
+};
+
+struct jh7110_isp_sat_curve {
+ __s16 yi_min;
+ __s16 yo_ir;
+ __s16 yo_min;
+ __s16 yo_max;
+};
+
+struct jh7110_isp_sat_hue_info {
+ __s16 sin;
+ __s16 cos;
+};
+
+struct jh7110_isp_sat_info {
+ __s16 gain_cmab;
+ __s16 gain_cmad;
+ __s16 threshold_cmb;
+ __s16 threshold_cmd;
+ __s16 offset_u;
+ __s16 offset_v;
+ __s16 cmsf;
+};
+
+struct jh7110_isp_sat_setting {
+ __u32 enabled;
+ struct jh7110_isp_sat_curve curve;
+ struct jh7110_isp_sat_hue_info hue_info;
+ struct jh7110_isp_sat_info sat_info;
+};
+
+struct jh7110_isp_sharp_weight {
+ __u8 weight[15];
+ __u32 recip_wei_sum;
+};
+
+struct jh7110_isp_sharp_strength {
+ __s16 diff[4];
+ __s16 f[4];
+};
+
+struct jh7110_isp_sharp_setting {
+ __u32 enabled;
+ struct jh7110_isp_sharp_weight weight;
+ struct jh7110_isp_sharp_strength strength;
+ __s8 pdirf;
+ __s8 ndirf;
+};
+
+struct jh7110_isp_ycrv_curve {
+ __s16 y[64];
+};
+
+struct jh7110_isp_ycrv_setting {
+ __u32 enabled;
+ struct jh7110_isp_ycrv_curve curve;
+};
+
+#endif
--- a/include/uapi/linux/v4l2-controls.h
+++ b/include/uapi/linux/v4l2-controls.h
@@ -203,6 +203,12 @@ enum v4l2_colorfx {
*/
#define V4L2_CID_USER_ASPEED_BASE (V4L2_CID_USER_BASE + 0x11a0)
+/*
+ * The base for the jh7110-isp driver controls.
+ * We reserve 16 controls for this driver.
+ */
+#define V4L2_CID_USER_JH7110_ISP_BASE (V4L2_CID_USER_BASE + 0x1170)
+
/* MPEG-class control IDs */
/* The MPEG controls are applicable to all codec controls
* and the 'MPEG' part of the define is historical */
--- /dev/null
+++ b/include/video/stf-vin.h
@@ -0,0 +1,443 @@
+/* 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 "jh7110-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_MIPI_RX0_PXL 0x4
+
+#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_VIN_PIXEL_CLK_IF0 0x20
+#define CLK_U0_VIN_PIXEL_CLK_IF1 0x24
+#define CLK_U0_VIN_PIXEL_CLK_IF2 0x28
+#define CLK_U0_VIN_PIXEL_CLK_IF3 0x2c
+
+#define CLK_U0_VIN_CLK_P_AXIWR 0x30
+
+#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 M31DPHY_APBCFGSAIF__SYSCFG_0 0x0
+#define M31DPHY_APBCFGSAIF__SYSCFG_4 0x4
+#define M31DPHY_APBCFGSAIF__SYSCFG_8 0x8
+#define M31DPHY_APBCFGSAIF__SYSCFG_12 0xc
+#define M31DPHY_APBCFGSAIF__SYSCFG_16 0x10
+#define M31DPHY_APBCFGSAIF__SYSCFG_20 0x14
+#define M31DPHY_APBCFGSAIF__SYSCFG_24 0x18
+#define M31DPHY_APBCFGSAIF__SYSCFG_28 0x1c
+#define M31DPHY_APBCFGSAIF__SYSCFG_32 0x20
+#define M31DPHY_APBCFGSAIF__SYSCFG_36 0x24
+#define M31DPHY_APBCFGSAIF__SYSCFG_40 0x28
+#define M31DPHY_APBCFGSAIF__SYSCFG_44 0x2c
+#define M31DPHY_APBCFGSAIF__SYSCFG_48 0x30
+#define M31DPHY_APBCFGSAIF__SYSCFG_52 0x34
+#define M31DPHY_APBCFGSAIF__SYSCFG_56 0x38
+#define M31DPHY_APBCFGSAIF__SYSCFG_60 0x3c
+#define M31DPHY_APBCFGSAIF__SYSCFG_64 0x40
+#define M31DPHY_APBCFGSAIF__SYSCFG_68 0x44
+#define M31DPHY_APBCFGSAIF__SYSCFG_72 0x48
+#define M31DPHY_APBCFGSAIF__SYSCFG_76 0x4c
+#define M31DPHY_APBCFGSAIF__SYSCFG_80 0x50
+#define M31DPHY_APBCFGSAIF__SYSCFG_84 0x54
+#define M31DPHY_APBCFGSAIF__SYSCFG_88 0x58
+#define M31DPHY_APBCFGSAIF__SYSCFG_92 0x5c
+#define M31DPHY_APBCFGSAIF__SYSCFG_96 0x60
+#define M31DPHY_APBCFGSAIF__SYSCFG_100 0x64
+#define M31DPHY_APBCFGSAIF__SYSCFG_104 0x68
+#define M31DPHY_APBCFGSAIF__SYSCFG_108 0x6c
+#define M31DPHY_APBCFGSAIF__SYSCFG_112 0x70
+#define M31DPHY_APBCFGSAIF__SYSCFG_116 0x74
+#define M31DPHY_APBCFGSAIF__SYSCFG_120 0x78
+#define M31DPHY_APBCFGSAIF__SYSCFG_124 0x7c
+#define M31DPHY_APBCFGSAIF__SYSCFG_128 0x80
+#define M31DPHY_APBCFGSAIF__SYSCFG_132 0x84
+#define M31DPHY_APBCFGSAIF__SYSCFG_136 0x88
+#define M31DPHY_APBCFGSAIF__SYSCFG_140 0x8c
+#define M31DPHY_APBCFGSAIF__SYSCFG_144 0x90
+#define M31DPHY_APBCFGSAIF__SYSCFG_184 0xb8
+
+//pmu registers
+#define SW_DEST_POWER_ON 0x0C
+#define SW_DEST_POWER_OFF 0x10
+#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_ISP_CTRL 0x3c
+#define CLK_ISP_2X_CTRL 0x40
+#define CLK_ISP_MIPI_CTRL 0x44
+#define CLK_C_ISP_CTRL 0x64
+#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 U0_VIN_SCFG_SRAM_CONFIG (BIT(0) | BIT(1))
+
+#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 U0_VIN_CNFG_AXIWR0_END_ADDR 0xFFFFFFFF
+
+#define SYSCONSAIF_SYSCFG_28 0x1c
+#define U0_VIN_CNFG_AXIWR0_INTR_CLEAN BIT(0)
+#define U0_VIN_CNFG_AXIWR0_MASK BIT(1)
+#define U0_VIN_CNFG_AXIWR0_PIX_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_PIX_CT (BIT(13) | BIT(14))
+#define UO_VIN_CNFG_AXIWR0_PIXEL_HEIGH_BIT_SEL (BIT(15) | BIT(16))
+#define SYSCONSAIF_SYSCFG_32 0x20
+
+#define SYSCONSAIF_SYSCFG_36 0x24
+#define UO_VIN_CNFG_COLOR_BAR_EN BIT(0)
+#define U0_VIN_CNFG_DVP_HS_POS (0x1<<1)
+#define U0_VIN_CNFG_DVP_SWAP_EN BIT(2)
+#define U0_VIN_CNFG_DVP_VS_POS (0x1<<3)
+#define U0_VIN_CNFG_GEN_EN_AXIRD BIT(4)
+#define U0_VIN_CNFG_ISP_DVP_EN0 BIT(5)
+#define U0_VIN_CNFG_MIPI_BYTE_EN_ISP0 (BIT(6) |BIT(7))
+#define U0_VIN_CNFG_P_I_MIPI_CHANNEL_SEL0 (BIT(8) |BIT(9) | BIT(10) | BIT(11))
+#define U0_VIN_CNFG_P_I_MIPI_HEADER_EN0 BIT(12)
+
+#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 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_CSI_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
+
+//m31_dphy registers
+#define M31DPHY_APBCFGSAIF__SYSCFG_188 0xbc
+#define M31DPHY_APBCFGSAIF__SYSCFG_192 0xc0
+#define M31DPHY_APBCFGSAIF__SYSCFG_196 0xc4
+#define M31DPHY_APBCFGSAIF__SYSCFG_200 0xc8
+
+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 *csi2rx_base;
+ void __iomem *clkgen_base;
+ void __iomem *rstgen_base;
+ void __iomem *sysctrl_base;
+ void __iomem *isp_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 isp;
+ int isp_irq;
+ int isp_csi_irq;
+ int isp_scd_irq;
+ int isp_irq_csiline;
+ 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
Reference in New Issue View Git Blame Copy Permalink