diff --git a/drivers/phy/cadence/phy-cadence-torrent.c b/drivers/phy/cadence/phy-cadence-torrent.c
index 1596d2c..e3b3356 100644
--- a/drivers/phy/cadence/phy-cadence-torrent.c
+++ b/drivers/phy/cadence/phy-cadence-torrent.c
@@ -36,6 +36,9 @@
 #define PHY_AUX_CONFIG			0x00
 #define PHY_AUX_CTRL			0x04
 #define PHY_RESET			0x20
+#define PMA_TX_ELEC_IDLE_MASK		0xF0U
+#define PMA_TX_ELEC_IDLE_SHIFT		4
+#define PHY_L00_RESET_N_MASK		0x01U
 #define PHY_PMA_XCVR_PLLCLK_EN		0x24
 #define PHY_PMA_XCVR_PLLCLK_EN_ACK	0x28
 #define PHY_PMA_XCVR_POWER_STATE_REQ	0x2c
@@ -119,6 +122,10 @@
 #define CMN_PDIAG_PLL1_CP_IADJ_M0	0x00714
 #define CMN_PDIAG_PLL1_FILT_PADJ_M0	0x00718
 
+#define TX_TXCC_CTRL			0x10100
+#define TX_TXCC_CPOST_MULT_00		0x10130
+#define TX_TXCC_MGNFS_MULT_000		0x10140
+#define DRV_DIAG_TX_DRV			0x10318
 #define XCVR_DIAG_PLLDRC_CTRL		0x10394
 #define XCVR_DIAG_HSCLK_SEL		0x10398
 #define XCVR_DIAG_HSCLK_DIV		0x1039c
@@ -128,6 +135,8 @@
 #define TX_PSC_A2			0x10408
 #define TX_PSC_A3			0x1040c
 #define TX_RCVDET_ST_TMR		0x1048c
+#define TX_DIAG_ACYA			0x1079c
+#define TX_DIAG_ACYA_HBDC_MASK		0x0001U
 #define RX_PSC_A0			0x20000
 #define RX_PSC_A1			0x20004
 #define RX_PSC_A2			0x20008
@@ -139,6 +148,9 @@
 
 #define PHY_PLL_CFG			0x30038
 
+#define PHY_PMA_CMN_CTRL2		0x38004
+#define PHY_PMA_PLL_RAW_CTRL		0x3800c
+
 struct cdns_torrent_phy {
 	void __iomem *base;	/* DPTX registers base */
 	void __iomem *sd_base; /* SD0801 registers base */
@@ -158,7 +170,8 @@ enum phy_powerstate {
 
 static int cdns_torrent_dp_init(struct phy *phy);
 static int cdns_torrent_dp_exit(struct phy *phy);
-static int cdns_torrent_dp_run(struct cdns_torrent_phy *cdns_phy);
+static int cdns_torrent_dp_run(struct cdns_torrent_phy *cdns_phy,
+			       u32 num_lanes);
 static
 int cdns_torrent_dp_wait_pma_cmn_ready(struct cdns_torrent_phy *cdns_phy);
 static void cdns_torrent_dp_pma_cfg(struct cdns_torrent_phy *cdns_phy);
@@ -182,9 +195,16 @@ static void cdns_dp_phy_write_field(struct cdns_torrent_phy *cdns_phy,
 				    unsigned char num_bits,
 				    unsigned int val);
 
+static int cdns_torrent_dp_configure(struct phy *phy,
+				     union phy_configure_opts *opts);
+static int cdns_torrent_dp_set_power_state(struct cdns_torrent_phy *cdns_phy,
+					   u32 num_lanes,
+					   enum phy_powerstate powerstate);
+
 static const struct phy_ops cdns_torrent_phy_ops = {
 	.init		= cdns_torrent_dp_init,
 	.exit		= cdns_torrent_dp_exit,
+	.configure	= cdns_torrent_dp_configure,
 	.owner		= THIS_MODULE,
 };
 
@@ -196,6 +216,16 @@ static void cdns_torrent_phy_write(struct cdns_torrent_phy *cdns_phy,
 	writel(val, cdns_phy->sd_base + offset);
 }
 
+static u32 cdns_torrent_phy_read(struct cdns_torrent_phy *cdns_phy, u32 offset)
+{
+	return readl(cdns_phy->sd_base + offset);
+}
+
+#define cdns_torrent_phy_read_poll_timeout(cdns_phy, offset, val, cond, \
+					   delay_us, timeout_us) \
+	readl_poll_timeout((cdns_phy)->sd_base + (offset), \
+			   val, cond, delay_us, timeout_us)
+
 /* DPTX mmr access functions */
 
 static void cdns_torrent_dp_write(struct cdns_torrent_phy *cdns_phy,
@@ -214,6 +244,237 @@ static u32 cdns_torrent_dp_read(struct cdns_torrent_phy *cdns_phy, u32 offset)
 	readl_poll_timeout((cdns_phy)->base + (offset), \
 			   val, cond, delay_us, timeout_us)
 
+/*
+ * Structure used to store values of PHY registers for voltage-related
+ * coefficients, for particular voltage swing and pre-emphasis level. Values
+ * are shared across all physical lanes.
+ */
+struct coefficients {
+	/* Value of DRV_DIAG_TX_DRV register to use */
+	u16 diag_tx_drv;
+	/* Value of TX_TXCC_MGNFS_MULT_000 register to use */
+	u16 mgnfs_mult;
+	/* Value of TX_TXCC_CPOST_MULT_00 register to use */
+	u16 cpost_mult;
+};
+
+/*
+ * Array consists of values of voltage-related registers for sd0801 PHY. A value
+ * of 0xFFFF is a placeholder for invalid combination, and will never be used.
+ */
+static const struct coefficients vltg_coeff[4][4] = {
+	/* voltage swing 0, pre-emphasis 0->3 */
+	{	{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x002A,
+		 .cpost_mult = 0x0000},
+		{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x001F,
+		 .cpost_mult = 0x0014},
+		{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0012,
+		 .cpost_mult = 0x0020},
+		{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0000,
+		 .cpost_mult = 0x002A}
+	},
+
+	/* voltage swing 1, pre-emphasis 0->3 */
+	{	{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x001F,
+		 .cpost_mult = 0x0000},
+		{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0013,
+		 .cpost_mult = 0x0012},
+		{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0000,
+		 .cpost_mult = 0x001F},
+		{.diag_tx_drv = 0xFFFF, .mgnfs_mult = 0xFFFF,
+		 .cpost_mult = 0xFFFF}
+	},
+
+	/* voltage swing 2, pre-emphasis 0->3 */
+	{	{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0013,
+		 .cpost_mult = 0x0000},
+		{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0000,
+		 .cpost_mult = 0x0013},
+		{.diag_tx_drv = 0xFFFF, .mgnfs_mult = 0xFFFF,
+		 .cpost_mult = 0xFFFF},
+		{.diag_tx_drv = 0xFFFF, .mgnfs_mult = 0xFFFF,
+		 .cpost_mult = 0xFFFF}
+	},
+
+	/* voltage swing 3, pre-emphasis 0->3 */
+	{	{.diag_tx_drv = 0x0003, .mgnfs_mult = 0x0000,
+		 .cpost_mult = 0x0000},
+		{.diag_tx_drv = 0xFFFF, .mgnfs_mult = 0xFFFF,
+		 .cpost_mult = 0xFFFF},
+		{.diag_tx_drv = 0xFFFF, .mgnfs_mult = 0xFFFF,
+		 .cpost_mult = 0xFFFF},
+		{.diag_tx_drv = 0xFFFF, .mgnfs_mult = 0xFFFF,
+		 .cpost_mult = 0xFFFF}
+	}
+};
+
+/*
+ * Enable or disable PLL for selected lanes.
+ */
+static int cdns_torrent_dp_set_pll_en(struct cdns_torrent_phy *cdns_phy,
+				      struct phy_configure_opts_dp *dp,
+				      bool enable)
+{
+	u32 rd_val;
+	u32 ret;
+	/*
+	 * Used to determine, which bits to check for or enable in
+	 * PHY_PMA_XCVR_PLLCLK_EN register.
+	 */
+	u32 pll_bits;
+	/* Used to enable or disable lanes. */
+	u32 pll_val;
+
+	/* Select values of registers and mask, depending on enabled lane
+	 * count.
+	 */
+	switch (dp->lanes) {
+	/* lane 0 */
+	case (1):
+		pll_bits = 0x00000001;
+		break;
+	/* lanes 0-1 */
+	case (2):
+		pll_bits = 0x00000003;
+		break;
+	/* lanes 0-3, all */
+	default:
+		pll_bits = 0x0000000F;
+		break;
+	}
+
+	if (enable)
+		pll_val = pll_bits;
+	else
+		pll_val = 0x00000000;
+
+	cdns_torrent_dp_write(cdns_phy, PHY_PMA_XCVR_PLLCLK_EN, pll_val);
+
+	/* Wait for acknowledgment from PHY. */
+	ret = cdns_torrent_dp_read_poll_timeout(cdns_phy,
+						PHY_PMA_XCVR_PLLCLK_EN_ACK,
+						rd_val,
+						(rd_val & pll_bits) == pll_val,
+						0, POLL_TIMEOUT_US);
+	ndelay(100);
+	return ret;
+}
+
+/*
+ * Perform register operations related to setting link rate, once powerstate is
+ * set and PLL disable request was processed.
+ */
+static int cdns_torrent_dp_configure_rate(struct cdns_torrent_phy *cdns_phy,
+					  struct phy_configure_opts_dp *dp)
+{
+	u32 ret;
+	u32 read_val;
+
+	/* Disable the cmn_pll0_en before re-programming the new data rate. */
+	cdns_torrent_phy_write(cdns_phy, PHY_PMA_PLL_RAW_CTRL, 0);
+
+	/*
+	 * Wait for PLL ready de-assertion.
+	 * For PLL0 - PHY_PMA_CMN_CTRL2[2] == 1
+	 */
+	ret = cdns_torrent_phy_read_poll_timeout(cdns_phy, PHY_PMA_CMN_CTRL2,
+						 read_val,
+						 ((read_val >> 2) & 0x01) != 0,
+						 0, POLL_TIMEOUT_US);
+	if (ret)
+		return ret;
+	ndelay(200);
+
+	/* DP Rate Change - VCO Output settings. */
+	if (cdns_phy->ref_clk_rate == REF_CLK_19_2MHz) {
+		/* PMA common configuration 19.2MHz */
+		cdns_torrent_dp_pma_cmn_vco_cfg_19_2mhz(cdns_phy, dp->link_rate,
+							dp->ssc);
+		cdns_torrent_dp_pma_cmn_cfg_19_2mhz(cdns_phy);
+	} else if (cdns_phy->ref_clk_rate == REF_CLK_25MHz) {
+		/* PMA common configuration 25MHz */
+		cdns_torrent_dp_pma_cmn_vco_cfg_25mhz(cdns_phy, dp->link_rate,
+						      dp->ssc);
+		cdns_torrent_dp_pma_cmn_cfg_25mhz(cdns_phy);
+	}
+	cdns_torrent_dp_pma_cmn_rate(cdns_phy, dp->link_rate, dp->lanes);
+
+	/* Enable the cmn_pll0_en. */
+	cdns_torrent_phy_write(cdns_phy, PHY_PMA_PLL_RAW_CTRL, 0x3);
+
+	/*
+	 * Wait for PLL ready assertion.
+	 * For PLL0 - PHY_PMA_CMN_CTRL2[0] == 1
+	 */
+	ret = cdns_torrent_phy_read_poll_timeout(cdns_phy, PHY_PMA_CMN_CTRL2,
+						 read_val,
+						 (read_val & 0x01) != 0,
+						 0, POLL_TIMEOUT_US);
+	return ret;
+}
+
+/*
+ * Verify, that parameters to configure PHY with are correct.
+ */
+static int cdns_torrent_dp_verify_config(struct cdns_torrent_phy *cdns_phy,
+					 struct phy_configure_opts_dp *dp)
+{
+	u8 i;
+
+	/* If changing link rate was required, verify it's supported. */
+	if (dp->set_rate) {
+		switch (dp->link_rate) {
+		case 1620:
+		case 2160:
+		case 2430:
+		case 2700:
+		case 3240:
+		case 4320:
+		case 5400:
+		case 8100:
+			/* valid bit rate */
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	/* Verify lane count. */
+	switch (dp->lanes) {
+	case 1:
+	case 2:
+	case 4:
+		/* valid lane count. */
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Check against actual number of PHY's lanes. */
+	if (dp->lanes > cdns_phy->num_lanes)
+		return -EINVAL;
+
+	/*
+	 * If changing voltages is required, check swing and pre-emphasis
+	 * levels, per-lane.
+	 */
+	if (dp->set_voltages) {
+		/* Lane count verified previously. */
+		for (i = 0; i < dp->lanes; i++) {
+			if (dp->voltage[i] > 3 || dp->pre[i] > 3)
+				return -EINVAL;
+
+			/* Sum of voltage swing and pre-emphasis levels cannot
+			 * exceed 3.
+			 */
+			if (dp->voltage[i] + dp->pre[i] > 3)
+				return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 /* Set power state A0 and PLL clock enable to 0 on enabled lanes. */
 static void cdns_torrent_dp_set_a0_pll(struct cdns_torrent_phy *cdns_phy,
 				       u32 num_lanes)
@@ -250,6 +511,171 @@ static void cdns_torrent_dp_set_a0_pll(struct cdns_torrent_phy *cdns_phy,
 	cdns_torrent_dp_write(cdns_phy, PHY_PMA_XCVR_PLLCLK_EN, pll_clk_en);
 }
 
+/* Configure lane count as required. */
+static int cdns_torrent_dp_set_lanes(struct cdns_torrent_phy *cdns_phy,
+				     struct phy_configure_opts_dp *dp)
+{
+	u32 value;
+	u32 ret;
+	u8 lane_mask = (1 << dp->lanes) - 1;
+
+	value = cdns_torrent_dp_read(cdns_phy, PHY_RESET);
+	/* clear pma_tx_elec_idle_ln_* bits. */
+	value &= ~PMA_TX_ELEC_IDLE_MASK;
+	/* Assert pma_tx_elec_idle_ln_* for disabled lanes. */
+	value |= ((~lane_mask) << PMA_TX_ELEC_IDLE_SHIFT) &
+		 PMA_TX_ELEC_IDLE_MASK;
+	cdns_torrent_dp_write(cdns_phy, PHY_RESET, value);
+
+	/* reset the link by asserting phy_l00_reset_n low */
+	cdns_torrent_dp_write(cdns_phy, PHY_RESET,
+			      value & (~PHY_L00_RESET_N_MASK));
+
+	/*
+	 * Assert lane reset on unused lanes and lane 0 so they remain in reset
+	 * and powered down when re-enabling the link
+	 */
+	value = (value & 0x0000FFF0) | (0x0000000E & lane_mask);
+	cdns_torrent_dp_write(cdns_phy, PHY_RESET, value);
+
+	cdns_torrent_dp_set_a0_pll(cdns_phy, dp->lanes);
+
+	/* release phy_l0*_reset_n based on used laneCount */
+	value = (value & 0x0000FFF0) | (0x0000000F & lane_mask);
+	cdns_torrent_dp_write(cdns_phy, PHY_RESET, value);
+
+	/* Wait, until PHY gets ready after releasing PHY reset signal. */
+	ret = cdns_torrent_dp_wait_pma_cmn_ready(cdns_phy);
+	if (ret)
+		return ret;
+
+	ndelay(100);
+
+	/* release pma_xcvr_pllclk_en_ln_*, only for the master lane */
+	cdns_torrent_dp_write(cdns_phy, PHY_PMA_XCVR_PLLCLK_EN, 0x0001);
+
+	ret = cdns_torrent_dp_run(cdns_phy, dp->lanes);
+
+	return ret;
+}
+
+/* Configure link rate as required. */
+static int cdns_torrent_dp_set_rate(struct cdns_torrent_phy *cdns_phy,
+				    struct phy_configure_opts_dp *dp)
+{
+	u32 ret;
+
+	ret = cdns_torrent_dp_set_power_state(cdns_phy, dp->lanes,
+					      POWERSTATE_A3);
+	if (ret)
+		return ret;
+	ret = cdns_torrent_dp_set_pll_en(cdns_phy, dp, false);
+	if (ret)
+		return ret;
+	ndelay(200);
+
+	ret = cdns_torrent_dp_configure_rate(cdns_phy, dp);
+	if (ret)
+		return ret;
+	ndelay(200);
+
+	ret = cdns_torrent_dp_set_pll_en(cdns_phy, dp, true);
+	if (ret)
+		return ret;
+	ret = cdns_torrent_dp_set_power_state(cdns_phy, dp->lanes,
+					      POWERSTATE_A2);
+	if (ret)
+		return ret;
+	ret = cdns_torrent_dp_set_power_state(cdns_phy, dp->lanes,
+					      POWERSTATE_A0);
+	if (ret)
+		return ret;
+	ndelay(900);
+
+	return ret;
+}
+
+/* Configure voltage swing and pre-emphasis for all enabled lanes. */
+static void cdns_torrent_dp_set_voltages(struct cdns_torrent_phy *cdns_phy,
+					 struct phy_configure_opts_dp *dp)
+{
+	u8 lane;
+	u16 val;
+	unsigned int lane_bits;
+
+	for (lane = 0; lane < dp->lanes; lane++) {
+		lane_bits = (lane & LANE_MASK) << 11;
+
+		val = cdns_torrent_phy_read(cdns_phy,
+					    (TX_DIAG_ACYA | lane_bits));
+		/*
+		 * Write 1 to register bit TX_DIAG_ACYA[0] to freeze the
+		 * current state of the analog TX driver.
+		 */
+		val |= TX_DIAG_ACYA_HBDC_MASK;
+		cdns_torrent_phy_write(cdns_phy,
+				       (TX_DIAG_ACYA | lane_bits), val);
+
+		cdns_torrent_phy_write(cdns_phy,
+				       (TX_TXCC_CTRL | lane_bits), 0x08A4);
+		val = vltg_coeff[dp->voltage[lane]][dp->pre[lane]].diag_tx_drv;
+		cdns_torrent_phy_write(cdns_phy,
+				       (DRV_DIAG_TX_DRV | lane_bits), val);
+		val = vltg_coeff[dp->voltage[lane]][dp->pre[lane]].mgnfs_mult;
+		cdns_torrent_phy_write(cdns_phy,
+				       (TX_TXCC_MGNFS_MULT_000 | lane_bits),
+				       val);
+		val = vltg_coeff[dp->voltage[lane]][dp->pre[lane]].cpost_mult;
+		cdns_torrent_phy_write(cdns_phy,
+				       (TX_TXCC_CPOST_MULT_00 | lane_bits),
+				       val);
+
+		val = cdns_torrent_phy_read(cdns_phy,
+					    (TX_DIAG_ACYA | lane_bits));
+		/*
+		 * Write 0 to register bit TX_DIAG_ACYA[0] to allow the state of
+		 * analog TX driver to reflect the new programmed one.
+		 */
+		val &= ~TX_DIAG_ACYA_HBDC_MASK;
+		cdns_torrent_phy_write(cdns_phy,
+				       (TX_DIAG_ACYA | lane_bits), val);
+	}
+};
+
+static int cdns_torrent_dp_configure(struct phy *phy,
+				     union phy_configure_opts *opts)
+{
+	struct cdns_torrent_phy *cdns_phy = phy_get_drvdata(phy);
+	int ret;
+
+	ret = cdns_torrent_dp_verify_config(cdns_phy, &opts->dp);
+	if (ret) {
+		dev_err(&phy->dev, "invalid params for phy configure\n");
+		return ret;
+	}
+
+	if (opts->dp.set_lanes) {
+		ret = cdns_torrent_dp_set_lanes(cdns_phy, &opts->dp);
+		if (ret) {
+			dev_err(&phy->dev, "cdns_torrent_dp_set_lanes failed\n");
+			return ret;
+		}
+	}
+
+	if (opts->dp.set_rate) {
+		ret = cdns_torrent_dp_set_rate(cdns_phy, &opts->dp);
+		if (ret) {
+			dev_err(&phy->dev, "cdns_torrent_dp_set_rate failed\n");
+			return ret;
+		}
+	}
+
+	if (opts->dp.set_voltages)
+		cdns_torrent_dp_set_voltages(cdns_phy, &opts->dp);
+
+	return ret;
+}
+
 static int cdns_torrent_dp_init(struct phy *phy)
 {
 	unsigned char lane_bits;
@@ -321,7 +747,7 @@ static int cdns_torrent_dp_init(struct phy *phy)
 	if (ret)
 		return ret;
 
-	ret = cdns_torrent_dp_run(cdns_phy);
+	ret = cdns_torrent_dp_run(cdns_phy, cdns_phy->num_lanes);
 
 	return ret;
 }
@@ -898,7 +1324,7 @@ static int cdns_torrent_dp_set_power_state(struct cdns_torrent_phy *cdns_phy,
 	return ret;
 }
 
-static int cdns_torrent_dp_run(struct cdns_torrent_phy *cdns_phy)
+static int cdns_torrent_dp_run(struct cdns_torrent_phy *cdns_phy, u32 num_lanes)
 {
 	unsigned int read_val;
 	int ret;
@@ -919,12 +1345,12 @@ static int cdns_torrent_dp_run(struct cdns_torrent_phy *cdns_phy)
 
 	ndelay(100);
 
-	ret = cdns_torrent_dp_set_power_state(cdns_phy, cdns_phy->num_lanes,
+	ret = cdns_torrent_dp_set_power_state(cdns_phy, num_lanes,
 					      POWERSTATE_A2);
 	if (ret)
 		return ret;
 
-	ret = cdns_torrent_dp_set_power_state(cdns_phy, cdns_phy->num_lanes,
+	ret = cdns_torrent_dp_set_power_state(cdns_phy, num_lanes,
 					      POWERSTATE_A0);
 
 	return ret;
