From e1ecd29fcc6bdbb3b7857ec18b430bbcd2e72178 Mon Sep 17 00:00:00 2001 From: Zefa Chen Date: Fri, 24 Nov 2023 14:26:23 +0800 Subject: [PATCH] phy: rockchip: mipi csi2 dphy support set quick stream Signed-off-by: Zefa Chen Change-Id: I1a36a5244cef005daddcd4d80fa8fe051061c7ca --- .../rockchip/phy-rockchip-csi2-dphy-common.h | 2 + .../phy/rockchip/phy-rockchip-csi2-dphy-hw.c | 81 ++++++++++++++++++- drivers/phy/rockchip/phy-rockchip-csi2-dphy.c | 20 +++++ 3 files changed, 102 insertions(+), 1 deletion(-) diff --git a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h index 8ade19e23d28..7af000b42785 100644 --- a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h +++ b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-common.h @@ -130,6 +130,8 @@ struct csi2_dphy_hw { int (*stream_off)(struct csi2_dphy *dphy, struct v4l2_subdev *sd); int (*ttl_mode_enable)(struct csi2_dphy_hw *hw); void (*ttl_mode_disable)(struct csi2_dphy_hw *hw); + int (*quick_stream_on)(struct csi2_dphy *dphy, struct v4l2_subdev *sd); + int (*quick_stream_off)(struct csi2_dphy *dphy, struct v4l2_subdev *sd); }; int rockchip_csi2_dphy_hw_init(void); diff --git a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c index d0e643ed7dc1..4aada197a7b3 100644 --- a/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c +++ b/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c @@ -902,13 +902,90 @@ static int csi2_dphy_hw_stream_off(struct csi2_dphy *dphy, 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; +} + static int csi2_dphy_hw_ttl_mode_enable(struct csi2_dphy_hw *hw) { int ret = 0; @@ -1091,6 +1168,8 @@ static int rockchip_csi2_dphy_hw_probe(struct platform_device *pdev) } 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; diff --git a/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c b/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c index c80e75288e9e..6ce85e8ede17 100644 --- a/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c +++ b/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c @@ -689,6 +689,8 @@ static long rkcif_csi2_dphy_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void { struct csi2_dphy *dphy = to_csi2_dphy(sd); long ret = 0; + int i = 0; + int on = 0; switch (cmd) { case RKCIF_CMD_SET_CSI_IDX: @@ -696,6 +698,24 @@ static long rkcif_csi2_dphy_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void dphy->drv_data->chip_id != CHIP_ID_RV1106) dphy->csi_info = *((struct rkcif_csi_info *)arg); break; + case RKMODULE_SET_QUICK_STREAM: + for (i = 0; i < dphy->csi_info.csi_num; i++) { + if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_INNO) { + dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i]; + if (!dphy->dphy_hw || + !dphy->dphy_hw->quick_stream_off || + !dphy->dphy_hw->quick_stream_on) { + ret = -EINVAL; + break; + } + on = *(int *)arg; + if (on) + dphy->dphy_hw->quick_stream_on(dphy, sd); + else + dphy->dphy_hw->quick_stream_off(dphy, sd); + } + } + break; default: ret = -ENOIOCTLCMD; break;