Add support for enabling a specifically requested number of lanes in the .configure() callback and disable all lanes in power off and reset callbacks for proper hardware cleanup.
Signed-off-by: AngeloGioacchino Del Regno <[email protected]> --- drivers/phy/mediatek/phy-mtk-dp.c | 57 +++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/drivers/phy/mediatek/phy-mtk-dp.c b/drivers/phy/mediatek/phy-mtk-dp.c index 4247506ceb99..276391d991aa 100644 --- a/drivers/phy/mediatek/phy-mtk-dp.c +++ b/drivers/phy/mediatek/phy-mtk-dp.c @@ -10,6 +10,7 @@ * AngeloGioacchino Del Regno <[email protected]> */ +#include <linux/bitfield.h> #include <linux/delay.h> #include <linux/io.h> #include <linux/mfd/syscon.h> @@ -50,6 +51,9 @@ #define DRIVING_PARAM_1_DEFAULT 0x0 #define DRIVING_PARAM_2_DEFAULT 0x0 +/* DP_PHYD_TX_CTL_0 */ +#define PHYD_TX_LN_EN GENMASK(7, 4) + #define XTP_LN_TX_LCTXC0_SW0_PRE0_DEFAULT BIT(4) #define XTP_LN_TX_LCTXC0_SW0_PRE1_DEFAULT (BIT(10) | BIT(12)) #define XTP_LN_TX_LCTXC0_SW0_PRE2_DEFAULT GENMASK(20, 19) @@ -113,6 +117,7 @@ enum mtk_dp_phyd_dig_glb_regidx { DP_PHYD_SW_RST, DP_PHYD_BIT_RATE, DP_PHYD_AUX_RX_CTL, + DP_PHYD_TX_CTL_0, DP_PHYD_GLOBAL_MAX }; @@ -131,6 +136,7 @@ static const u8 mt8195_phy_dig_glb_regs[DP_PHYD_GLOBAL_MAX] = { [DP_PHYD_SW_RST] = 0x38, [DP_PHYD_BIT_RATE] = 0x3c, [DP_PHYD_AUX_RX_CTL] = 0x40, + [DP_PHYD_TX_CTL_0] = 0x44, }; /** @@ -200,6 +206,7 @@ static int mtk_dp_phy_configure(struct phy *phy, union phy_configure_opts *opts) struct mtk_dp_phy *dp_phy = phy_get_drvdata(phy); const struct mtk_dp_phy_pdata *pdata = dp_phy->pdata; u32 val; + int i; if (opts->dp.set_rate) { const u32 reg_bit_rate = pdata->regs_dig_glb[DP_PHYD_BIT_RATE]; @@ -226,6 +233,17 @@ static int mtk_dp_phy_configure(struct phy *phy, union phy_configure_opts *opts) regmap_write(dp_phy->regmap, pdata->off_dig_glb + reg_bit_rate, val); } + if (opts->dp.set_lanes) { + const u32 reg_dig_tx_ctl = pdata->regs_dig_glb[DP_PHYD_TX_CTL_0]; + + val = 0; + for (i = 0; i < opts->dp.lanes; i++) + val |= FIELD_PREP(PHYD_TX_LN_EN, BIT(i)); + + regmap_update_bits(dp_phy->regmap, pdata->off_dig_glb + reg_dig_tx_ctl, + PHYD_TX_LN_EN, val); + } + regmap_update_bits(dp_phy->regmap, pdata->off_dig_glb + pdata->regs_dig_glb[DP_PHYD_PLL_CTL_1], TPLL_SSC_EN, opts->dp.ssc ? TPLL_SSC_EN : 0); @@ -254,12 +272,39 @@ static int mtk_dp_phy_power_on(struct phy *phy) return 0; } +static int mtk_dp_phy_disable_all_lanes(struct mtk_dp_phy *dp_phy) +{ + const struct mtk_dp_phy_pdata *pdata = dp_phy->pdata; + const u8 *regs = pdata->regs_dig_glb; + int ret; + u32 val; + + /* Get mask of currently enabled lane */ + regmap_read(dp_phy->regmap, pdata->off_dig_glb + regs[DP_PHYD_TX_CTL_0], &val); + val = FIELD_GET(PHYD_TX_LN_EN, val); + if (val == 0) + return 0; + + /* Disable all lanes (needs to be done one by one, from last to first) */ + do { + u32 lane_num = fls(val) - 1; + val &= ~BIT(lane_num); + + regmap_clear_bits(dp_phy->regmap, + pdata->off_dig_glb + regs[DP_PHYD_TX_CTL_0], + FIELD_PREP(PHYD_TX_LN_EN, BIT(lane_num))); + } while (val); + + return 0; +} + static int mtk_dp_phy_power_off(struct phy *phy) { struct mtk_dp_phy *dp_phy = phy_get_drvdata(phy); const struct mtk_dp_phy_pdata *pdata = dp_phy->pdata; const u8 *regs_dig = pdata->regs_dig_glb; const u8 *regs_ana = pdata->regs_ana_glb; + int ret; regmap_set_bits(dp_phy->regmap, pdata->off_ana_glb + regs_ana[DP_PHYA_GLB_FORCE_CTRL_1], @@ -269,6 +314,12 @@ static int mtk_dp_phy_power_off(struct phy *phy) regmap_write(dp_phy->regmap, pdata->off_dig_glb + regs_dig[DP_PHYD_AUX_RX_CTL], 0); + ret = mtk_dp_phy_disable_all_lanes(dp_phy); + if (ret) { + dev_err(dp_phy->dev, "Could not disable lanes for poweroff!\n"); + return ret; + } + return 0; } @@ -277,6 +328,7 @@ static int mtk_dp_phy_reset(struct phy *phy) struct mtk_dp_phy *dp_phy = phy_get_drvdata(phy); const struct mtk_dp_phy_pdata *pdata = dp_phy->pdata; const u32 reg_rst = pdata->regs_dig_glb[DP_PHYD_SW_RST]; + int ret; /* Clearing bits sets reset state */ regmap_clear_bits(dp_phy->regmap, pdata->off_dig_glb + reg_rst, DP_GLB_SW_RST_PHYD); @@ -287,6 +339,11 @@ static int mtk_dp_phy_reset(struct phy *phy) /* Setting bits means go out of reset */ regmap_set_bits(dp_phy->regmap, pdata->off_dig_glb + reg_rst, DP_GLB_SW_RST_PHYD); + /* Disable all lanes and continue reset even if this fails, but notify */ + ret = mtk_dp_phy_disable_all_lanes(dp_phy); + if (ret) + dev_err(dp_phy->dev, "Could not disable lanes during reset!\n"); + return 0; } -- 2.55.0
