From 1543e317f1da31b75942316931e8f491a8920811 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Thu, 04 Jan 2024 10:08:02 +0000
Subject: [PATCH] disable FB
---
kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c | 151 ++++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 140 insertions(+), 11 deletions(-)
diff --git a/kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c b/kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c
index e0a9fd6..4aada19 100644
--- a/kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c
+++ b/kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c
@@ -54,6 +54,7 @@
#define CSI2_DPHY_DUAL_CAL_EN (0x80)
#define CSI2_DPHY_CLK_INV (0X84)
+#define CSI2_DPHY_CLK_CONTINUE_MODE (0x128)
#define CSI2_DPHY_CLK_WR_THS_SETTLE (0x160)
#define CSI2_DPHY_CLK_CALIB_EN (0x168)
#define CSI2_DPHY_LANE0_WR_THS_SETTLE (0x1e0)
@@ -64,6 +65,7 @@
#define CSI2_DPHY_LANE2_CALIB_EN (0x2e8)
#define CSI2_DPHY_LANE3_WR_THS_SETTLE (0x360)
#define CSI2_DPHY_LANE3_CALIB_EN (0x368)
+#define CSI2_DPHY_CLK1_CONTINUE_MODE (0x3a8)
#define CSI2_DPHY_CLK1_WR_THS_SETTLE (0x3e0)
#define CSI2_DPHY_CLK1_CALIB_EN (0x3e8)
@@ -213,6 +215,8 @@
CSI2PHY_PATH1_MODEL,
CSI2PHY_PATH1_LVDS_MODEL,
CSI2PHY_CLK_INV,
+ CSI2PHY_CLK_CONTINUE_MODE,
+ CSI2PHY_CLK1_CONTINUE_MODE,
};
#define HIWORD_UPDATE(val, mask, shift) \
@@ -232,9 +236,14 @@
static inline void write_sys_grf_reg(struct csi2_dphy_hw *hw,
int index, u8 value)
{
- const struct grf_reg *reg = &hw->grf_regs[index];
- unsigned int val = HIWORD_UPDATE(value, reg->mask, reg->shift);
+ const struct grf_reg *reg = NULL;
+ unsigned int val = 0;
+ if (index >= hw->drv_data->num_grf_regs)
+ return;
+
+ reg = &hw->grf_regs[index];
+ val = HIWORD_UPDATE(value, reg->mask, reg->shift);
if (reg->mask)
regmap_write(hw->regmap_sys_grf, reg->offset, val);
}
@@ -242,18 +251,27 @@
static inline void write_grf_reg(struct csi2_dphy_hw *hw,
int index, u8 value)
{
- const struct grf_reg *reg = &hw->grf_regs[index];
- unsigned int val = HIWORD_UPDATE(value, reg->mask, reg->shift);
+ const struct grf_reg *reg = NULL;
+ unsigned int val = 0;
+ if (index >= hw->drv_data->num_grf_regs)
+ return;
+
+ reg = &hw->grf_regs[index];
+ val = HIWORD_UPDATE(value, reg->mask, reg->shift);
if (reg->mask)
regmap_write(hw->regmap_grf, reg->offset, val);
}
static inline u32 read_grf_reg(struct csi2_dphy_hw *hw, int index)
{
- const struct grf_reg *reg = &hw->grf_regs[index];
+ const struct grf_reg *reg = NULL;
unsigned int val = 0;
+ if (index >= hw->drv_data->num_grf_regs)
+ return -EINVAL;
+
+ reg = &hw->grf_regs[index];
if (reg->mask) {
regmap_read(hw->regmap_grf, reg->offset, &val);
val = (val >> reg->shift) & reg->mask;
@@ -265,8 +283,12 @@
static inline void write_csi2_dphy_reg(struct csi2_dphy_hw *hw,
int index, u32 value)
{
- const struct csi2dphy_reg *reg = &hw->csi2dphy_regs[index];
+ const struct csi2dphy_reg *reg = NULL;
+ if (index >= hw->drv_data->num_csi2dphy_regs)
+ return;
+
+ reg = &hw->csi2dphy_regs[index];
if ((index == CSI2PHY_REG_CTRL_LANE_ENABLE) ||
(index == CSI2PHY_CLK_LANE_ENABLE) ||
(index != CSI2PHY_REG_CTRL_LANE_ENABLE &&
@@ -277,9 +299,13 @@
static inline void write_csi2_dphy_reg_mask(struct csi2_dphy_hw *hw,
int index, u32 value, u32 mask)
{
- const struct csi2dphy_reg *reg = &hw->csi2dphy_regs[index];
+ const struct csi2dphy_reg *reg = NULL;
u32 read_val = 0;
+ if (index >= hw->drv_data->num_csi2dphy_regs)
+ return;
+
+ reg = &hw->csi2dphy_regs[index];
read_val = readl(hw->hw_base_addr + reg->offset);
read_val &= ~mask;
read_val |= value;
@@ -289,8 +315,12 @@
static inline void read_csi2_dphy_reg(struct csi2_dphy_hw *hw,
int index, u32 *value)
{
- const struct csi2dphy_reg *reg = &hw->csi2dphy_regs[index];
+ const struct csi2dphy_reg *reg = NULL;
+ if (index >= hw->drv_data->num_csi2dphy_regs)
+ return;
+
+ reg = &hw->csi2dphy_regs[index];
if ((index == CSI2PHY_REG_CTRL_LANE_ENABLE) ||
(index == CSI2PHY_CLK_LANE_ENABLE) ||
(index != CSI2PHY_REG_CTRL_LANE_ENABLE &&
@@ -397,6 +427,8 @@
[CSI2PHY_CLK1_THS_SETTLE] = CSI2PHY_REG(CSI2_DPHY_CLK1_WR_THS_SETTLE),
[CSI2PHY_CLK1_CALIB_ENABLE] = CSI2PHY_REG(CSI2_DPHY_CLK1_CALIB_EN),
[CSI2PHY_CLK1_LANE_ENABLE] = CSI2PHY_REG(CSI2_DPHY_CLK1_LANE_EN),
+ [CSI2PHY_CLK_CONTINUE_MODE] = CSI2PHY_REG(CSI2_DPHY_CLK_CONTINUE_MODE),
+ [CSI2PHY_CLK1_CONTINUE_MODE] = CSI2PHY_REG(CSI2_DPHY_CLK1_CONTINUE_MODE),
};
static const struct grf_reg rv1106_grf_dphy_regs[] = {
@@ -709,19 +741,30 @@
val |= (GENMASK(sensor->lanes - 1, 0) <<
CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT) |
(0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
+ if (sensor->mbus.flags & V4L2_MBUS_CSI2_CONTINUOUS_CLOCK)
+ write_csi2_dphy_reg(hw, CSI2PHY_CLK_CONTINUE_MODE, 0x30);
} else {
if (!(pre_val & (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT)))
val |= (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
- if (dphy->phy_index % 3 == DPHY1)
+ if (dphy->phy_index % 3 == DPHY1) {
val |= (GENMASK(sensor->lanes - 1, 0) <<
CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT);
+ if (sensor->mbus.flags &
+ V4L2_MBUS_CSI2_CONTINUOUS_CLOCK)
+ write_csi2_dphy_reg(
+ hw, CSI2PHY_CLK_CONTINUE_MODE, 0x30);
+ }
if (dphy->phy_index % 3 == DPHY2) {
val |= (GENMASK(sensor->lanes - 1, 0) <<
CSI2_DPHY_CTRL_DATALANE_SPLIT_LANE2_3_OFFSET_BIT);
if (hw->drv_data->chip_id >= CHIP_ID_RK3588)
write_csi2_dphy_reg(hw, CSI2PHY_CLK1_LANE_ENABLE, BIT(6));
+ if (sensor->mbus.flags &
+ V4L2_MBUS_CSI2_CONTINUOUS_CLOCK)
+ write_csi2_dphy_reg(
+ hw, CSI2PHY_CLK1_CONTINUE_MODE, 0x30);
}
}
val |= pre_val;
@@ -859,10 +902,87 @@
write_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, 0x01);
csi2_dphy_hw_do_reset(hw);
- usleep_range(500, 1000);
mutex_unlock(&hw->mutex);
+ return 0;
+}
+
+static int csi2_dphy_hw_quick_stream_on(struct csi2_dphy *dphy,
+ struct v4l2_subdev *sd)
+{
+ struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
+ struct csi2_sensor *sensor;
+ struct csi2_dphy_hw *hw = dphy->dphy_hw;
+ u32 val = 0, pre_val = 0;
+
+ if (!sensor_sd)
+ return -ENODEV;
+ sensor = sd_to_sensor(dphy, sensor_sd);
+ if (!sensor)
+ return -ENODEV;
+
+ read_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, &pre_val);
+ if (hw->lane_mode == LANE_MODE_FULL) {
+ val |= (GENMASK(sensor->lanes - 1, 0) <<
+ CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT) |
+ (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
+ } else {
+ if (!(pre_val & (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT)))
+ val |= (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
+
+ if (dphy->phy_index % 3 == DPHY1)
+ val |= (GENMASK(sensor->lanes - 1, 0) <<
+ CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT);
+
+ if (dphy->phy_index % 3 == DPHY2) {
+ val |= (GENMASK(sensor->lanes - 1, 0) <<
+ CSI2_DPHY_CTRL_DATALANE_SPLIT_LANE2_3_OFFSET_BIT);
+ if (hw->drv_data->chip_id >= CHIP_ID_RK3588)
+ write_csi2_dphy_reg(hw, CSI2PHY_CLK1_LANE_ENABLE, BIT(6));
+ }
+ }
+ pre_val |= val;
+ write_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, pre_val);
+ return 0;
+}
+
+static int csi2_dphy_hw_quick_stream_off(struct csi2_dphy *dphy,
+ struct v4l2_subdev *sd)
+{
+ struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
+ struct csi2_sensor *sensor;
+ struct csi2_dphy_hw *hw = dphy->dphy_hw;
+ u32 val = 0, pre_val = 0;
+
+ if (!sensor_sd)
+ return -ENODEV;
+ sensor = sd_to_sensor(dphy, sensor_sd);
+ if (!sensor)
+ return -ENODEV;
+
+ read_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, &pre_val);
+ if (hw->lane_mode == LANE_MODE_FULL) {
+ val |= (GENMASK(sensor->lanes - 1, 0) <<
+ CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT) |
+ (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
+ } else {
+ if (!(pre_val & (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT)))
+ val |= (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
+
+ if (dphy->phy_index % 3 == DPHY1)
+ val |= (GENMASK(sensor->lanes - 1, 0) <<
+ CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT);
+
+ if (dphy->phy_index % 3 == DPHY2) {
+ val |= (GENMASK(sensor->lanes - 1, 0) <<
+ CSI2_DPHY_CTRL_DATALANE_SPLIT_LANE2_3_OFFSET_BIT);
+ if (hw->drv_data->chip_id >= CHIP_ID_RK3588)
+ write_csi2_dphy_reg(hw, CSI2PHY_CLK1_LANE_ENABLE, BIT(6));
+ }
+ }
+ pre_val &= ~val;
+ write_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, pre_val);
return 0;
}
@@ -913,7 +1033,9 @@
.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
.csi2dphy_regs = rk3568_csi2dphy_regs,
+ .num_csi2dphy_regs = ARRAY_SIZE(rk3568_csi2dphy_regs),
.grf_regs = rk3568_grf_dphy_regs,
+ .num_grf_regs = ARRAY_SIZE(rk3568_grf_dphy_regs),
.individual_init = rk3568_csi2_dphy_hw_individual_init,
.chip_id = CHIP_ID_RK3568,
.stream_on = csi2_dphy_hw_stream_on,
@@ -924,7 +1046,9 @@
.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
.csi2dphy_regs = rk3588_csi2dphy_regs,
+ .num_csi2dphy_regs = ARRAY_SIZE(rk3588_csi2dphy_regs),
.grf_regs = rk3588_grf_dphy_regs,
+ .num_grf_regs = ARRAY_SIZE(rk3588_grf_dphy_regs),
.individual_init = rk3588_csi2_dphy_hw_individual_init,
.chip_id = CHIP_ID_RK3588,
.stream_on = csi2_dphy_hw_stream_on,
@@ -935,7 +1059,9 @@
.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
.csi2dphy_regs = rv1106_csi2dphy_regs,
+ .num_csi2dphy_regs = ARRAY_SIZE(rv1106_csi2dphy_regs),
.grf_regs = rv1106_grf_dphy_regs,
+ .num_grf_regs = ARRAY_SIZE(rv1106_grf_dphy_regs),
.individual_init = rv1106_csi2_dphy_hw_individual_init,
.chip_id = CHIP_ID_RV1106,
.stream_on = csi2_dphy_hw_stream_on,
@@ -946,7 +1072,9 @@
.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
.csi2dphy_regs = rk3562_csi2dphy_regs,
+ .num_csi2dphy_regs = ARRAY_SIZE(rk3562_csi2dphy_regs),
.grf_regs = rk3562_grf_dphy_regs,
+ .num_grf_regs = ARRAY_SIZE(rk3562_grf_dphy_regs),
.individual_init = rk3562_csi2_dphy_hw_individual_init,
.chip_id = CHIP_ID_RK3562,
.stream_on = csi2_dphy_hw_stream_on,
@@ -1024,7 +1152,6 @@
dphy_hw->drv_data = drv_data;
dphy_hw->lane_mode = LANE_MODE_UNDEF;
dphy_hw->grf_regs = drv_data->grf_regs;
- dphy_hw->txrx_regs = drv_data->txrx_regs;
dphy_hw->csi2dphy_regs = drv_data->csi2dphy_regs;
res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1041,6 +1168,8 @@
}
dphy_hw->stream_on = drv_data->stream_on;
dphy_hw->stream_off = drv_data->stream_off;
+ dphy_hw->quick_stream_on = csi2_dphy_hw_quick_stream_on;
+ dphy_hw->quick_stream_off = csi2_dphy_hw_quick_stream_off;
if (drv_data->chip_id == CHIP_ID_RV1106) {
dphy_hw->ttl_mode_enable = csi2_dphy_hw_ttl_mode_enable;
--
Gitblit v1.6.2