Merge tag 'char-misc-5.18-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/char-misc

Pull char/misc and other driver updates from Greg KH: "Here is the big set of char/misc and other small driver subsystem updates for 5.18-rc1. Included in here are merges from driver subsystems which contain: - iio driver updates and new drivers - fsi driver updates - fpga driver updates - habanalabs driver updates and support for new hardware - soundwire driver updates and new drivers - phy driver updates and new drivers - coresight driver updates - icc driver updates Individual changes include: - mei driver updates - interconnect driver updates - new PECI driver subsystem added - vmci driver updates - lots of tiny misc/char driver updates All of these have been in linux-next for a while with no reported problems" * tag 'char-misc-5.18-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/char-misc: (556 commits) firmware: google: Properly state IOMEM dependency kgdbts: fix return value of __setup handler firmware: sysfb: fix platform-device leak in error path firmware: stratix10-svc: add missing callback parameter on RSU arm64: dts: qcom: add non-secure domain property to fastrpc nodes misc: fastrpc: Add dma handle implementation misc: fastrpc: Add fdlist implementation misc: fastrpc: Add helper function to get list and page misc: fastrpc: Add support to secure memory map dt-bindings: misc: add fastrpc domain vmid property misc: fastrpc: check before loading process to the DSP misc: fastrpc: add secure domain support dt-bindings: misc: add property to support non-secure DSP misc: fastrpc: Add support to get DSP capabilities misc: fastrpc: add support for FASTRPC_IOCTL_MEM_MAP/UNMAP misc: fastrpc: separate fastrpc device from channel context dt-bindings: nvmem: brcm,nvram: add basic NVMEM cells dt-bindings: nvmem: make "reg" property optional nvmem: brcm_nvram: parse NVRAM content into NVMEM cells nvmem: dt-bindings: Fix the error of dt-bindings check ...
author: Linus Torvalds <torvalds@linux-foundation.org> 2022-03-28 12:27:35 -0700
committer: Linus Torvalds <torvalds@linux-foundation.org> 2022-03-28 12:27:35 -0700
commit: 02e2af20f4f9f2aa0c84e9a30a35c02f0fbb7daa (patch)
tree: d126449a7d2ea2270627183f7cebd726fbe56a9d /drivers/phy
parent: Merge tag 'pinctrl-v5.18-1' of git://git.kernel.org/pub/scm/linux/kernel/git/linusw/linux-pinctrl (diff)
parent: firmware: google: Properly state IOMEM dependency (diff)
download: linux-dev-02e2af20f4f9f2aa0c84e9a30a35c02f0fbb7daa.tar.xz
linux-dev-02e2af20f4f9f2aa0c84e9a30a35c02f0fbb7daa.zip
25 files changed, 2711 insertions, 234 deletions
diff --git a/drivers/phy/allwinner/phy-sun4i-usb.c b/drivers/phy/allwinner/phy-sun4i-usb.c
index 788dd5cdbb7d..d5f3b42eb8ce 100644
--- a/drivers/phy/allwinner/phy-sun4i-usb.c
+++ b/drivers/phy/allwinner/phy-sun4i-usb.c
@@ -43,7 +43,7 @@
 #define REG_PHYCTL_A33			0x10
 #define REG_PHY_OTGCTL			0x20
 
-#define REG_PMU_UNK1			0x10
+#define REG_HCI_PHY_CTL			0x10
 
 #define PHYCTL_DATA			BIT(7)
 
@@ -82,6 +82,7 @@
 /* A83T specific control bits for PHY0 */
 #define PHY_CTL_VBUSVLDEXT		BIT(5)
 #define PHY_CTL_SIDDQ			BIT(3)
+#define PHY_CTL_H3_SIDDQ		BIT(1)
 
 /* A83T specific control bits for PHY2 HSIC */
 #define SUNXI_EHCI_HS_FORCE		BIT(20)
@@ -115,9 +116,9 @@ struct sun4i_usb_phy_cfg {
 	int hsic_index;
 	enum sun4i_usb_phy_type type;
 	u32 disc_thresh;
+	u32 hci_phy_ctl_clear;
 	u8 phyctl_offset;
 	bool dedicated_clocks;
-	bool enable_pmu_unk1;
 	bool phy0_dual_route;
 	int missing_phys;
 };
@@ -288,6 +289,12 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 		return ret;
 	}
 
+	if (phy->pmu && data->cfg->hci_phy_ctl_clear) {
+		val = readl(phy->pmu + REG_HCI_PHY_CTL);
+		val &= ~data->cfg->hci_phy_ctl_clear;
+		writel(val, phy->pmu + REG_HCI_PHY_CTL);
+	}
+
 	if (data->cfg->type == sun8i_a83t_phy ||
 	    data->cfg->type == sun50i_h6_phy) {
 		if (phy->index == 0) {
@@ -297,11 +304,6 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 			writel(val, data->base + data->cfg->phyctl_offset);
 		}
 	} else {
-		if (phy->pmu && data->cfg->enable_pmu_unk1) {
-			val = readl(phy->pmu + REG_PMU_UNK1);
-			writel(val & ~2, phy->pmu + REG_PMU_UNK1);
-		}
-
 		/* Enable USB 45 Ohm resistor calibration */
 		if (phy->index == 0)
 			sun4i_usb_phy_write(phy, PHY_RES45_CAL_EN, 0x01, 1);
@@ -863,7 +865,6 @@ static const struct sun4i_usb_phy_cfg sun4i_a10_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A10,
 	.dedicated_clocks = false,
-	.enable_pmu_unk1 = false,
 };
 
 static const struct sun4i_usb_phy_cfg sun5i_a13_cfg = {
@@ -872,7 +873,6 @@ static const struct sun4i_usb_phy_cfg sun5i_a13_cfg = {
 	.disc_thresh = 2,
 	.phyctl_offset = REG_PHYCTL_A10,
 	.dedicated_clocks = false,
-	.enable_pmu_unk1 = false,
 };
 
 static const struct sun4i_usb_phy_cfg sun6i_a31_cfg = {
@@ -881,7 +881,6 @@ static const struct sun4i_usb_phy_cfg sun6i_a31_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A10,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = false,
 };
 
 static const struct sun4i_usb_phy_cfg sun7i_a20_cfg = {
@@ -890,7 +889,6 @@ static const struct sun4i_usb_phy_cfg sun7i_a20_cfg = {
 	.disc_thresh = 2,
 	.phyctl_offset = REG_PHYCTL_A10,
 	.dedicated_clocks = false,
-	.enable_pmu_unk1 = false,
 };
 
 static const struct sun4i_usb_phy_cfg sun8i_a23_cfg = {
@@ -899,7 +897,6 @@ static const struct sun4i_usb_phy_cfg sun8i_a23_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A10,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = false,
 };
 
 static const struct sun4i_usb_phy_cfg sun8i_a33_cfg = {
@@ -908,7 +905,6 @@ static const struct sun4i_usb_phy_cfg sun8i_a33_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A33,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = false,
 };
 
 static const struct sun4i_usb_phy_cfg sun8i_a83t_cfg = {
@@ -925,7 +921,7 @@ static const struct sun4i_usb_phy_cfg sun8i_h3_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A33,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = true,
+	.hci_phy_ctl_clear = PHY_CTL_H3_SIDDQ,
 	.phy0_dual_route = true,
 };
 
@@ -935,7 +931,7 @@ static const struct sun4i_usb_phy_cfg sun8i_r40_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A33,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = true,
+	.hci_phy_ctl_clear = PHY_CTL_H3_SIDDQ,
 	.phy0_dual_route = true,
 };
 
@@ -945,7 +941,16 @@ static const struct sun4i_usb_phy_cfg sun8i_v3s_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A33,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = true,
+	.hci_phy_ctl_clear = PHY_CTL_H3_SIDDQ,
+	.phy0_dual_route = true,
+};
+
+static const struct sun4i_usb_phy_cfg sun20i_d1_cfg = {
+	.num_phys = 2,
+	.type = sun50i_h6_phy,
+	.phyctl_offset = REG_PHYCTL_A33,
+	.dedicated_clocks = true,
+	.hci_phy_ctl_clear = PHY_CTL_SIDDQ,
 	.phy0_dual_route = true,
 };
 
@@ -955,14 +960,13 @@ static const struct sun4i_usb_phy_cfg sun50i_a64_cfg = {
 	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A33,
 	.dedicated_clocks = true,
-	.enable_pmu_unk1 = true,
+	.hci_phy_ctl_clear = PHY_CTL_H3_SIDDQ,
 	.phy0_dual_route = true,
 };
 
 static const struct sun4i_usb_phy_cfg sun50i_h6_cfg = {
 	.num_phys = 4,
 	.type = sun50i_h6_phy,
-	.disc_thresh = 3,
 	.phyctl_offset = REG_PHYCTL_A33,
 	.dedicated_clocks = true,
 	.phy0_dual_route = true,
@@ -980,6 +984,7 @@ static const struct of_device_id sun4i_usb_phy_of_match[] = {
 	{ .compatible = "allwinner,sun8i-h3-usb-phy", .data = &sun8i_h3_cfg },
 	{ .compatible = "allwinner,sun8i-r40-usb-phy", .data = &sun8i_r40_cfg },
 	{ .compatible = "allwinner,sun8i-v3s-usb-phy", .data = &sun8i_v3s_cfg },
+	{ .compatible = "allwinner,sun20i-d1-usb-phy", .data = &sun20i_d1_cfg },
 	{ .compatible = "allwinner,sun50i-a64-usb-phy",
 	  .data = &sun50i_a64_cfg},
 	{ .compatible = "allwinner,sun50i-h6-usb-phy", .data = &sun50i_h6_cfg },
diff --git a/drivers/phy/amlogic/phy-meson-gxl-usb2.c b/drivers/phy/amlogic/phy-meson-gxl-usb2.c
index 2b3c0d730f20..db17c3448bfe 100644
--- a/drivers/phy/amlogic/phy-meson-gxl-usb2.c
+++ b/drivers/phy/amlogic/phy-meson-gxl-usb2.c
@@ -114,8 +114,10 @@ static int phy_meson_gxl_usb2_init(struct phy *phy)
 		return ret;
 
 	ret = clk_prepare_enable(priv->clk);
-	if (ret)
+	if (ret) {
+		reset_control_rearm(priv->reset);
 		return ret;
+	}
 
 	return 0;
 }
@@ -125,6 +127,7 @@ static int phy_meson_gxl_usb2_exit(struct phy *phy)
 	struct phy_meson_gxl_usb2_priv *priv = phy_get_drvdata(phy);
 
 	clk_disable_unprepare(priv->clk);
+	reset_control_rearm(priv->reset);
 
 	return 0;
 }
diff --git a/drivers/phy/amlogic/phy-meson8b-usb2.c b/drivers/phy/amlogic/phy-meson8b-usb2.c
index cf10bed40528..dd96763911b8 100644
--- a/drivers/phy/amlogic/phy-meson8b-usb2.c
+++ b/drivers/phy/amlogic/phy-meson8b-usb2.c
@@ -154,6 +154,7 @@ static int phy_meson8b_usb2_power_on(struct phy *phy)
 	ret = clk_prepare_enable(priv->clk_usb_general);
 	if (ret) {
 		dev_err(&phy->dev, "Failed to enable USB general clock\n");
+		reset_control_rearm(priv->reset);
 		return ret;
 	}
 
@@ -161,6 +162,7 @@ static int phy_meson8b_usb2_power_on(struct phy *phy)
 	if (ret) {
 		dev_err(&phy->dev, "Failed to enable USB DDR clock\n");
 		clk_disable_unprepare(priv->clk_usb_general);
+		reset_control_rearm(priv->reset);
 		return ret;
 	}
 
@@ -199,6 +201,7 @@ static int phy_meson8b_usb2_power_on(struct phy *phy)
 				dev_warn(&phy->dev, "USB ID detect failed!\n");
 				clk_disable_unprepare(priv->clk_usb);
 				clk_disable_unprepare(priv->clk_usb_general);
+				reset_control_rearm(priv->reset);
 				return -EINVAL;
 			}
 		}
@@ -218,6 +221,7 @@ static int phy_meson8b_usb2_power_off(struct phy *phy)
 
 	clk_disable_unprepare(priv->clk_usb);
 	clk_disable_unprepare(priv->clk_usb_general);
+	reset_control_rearm(priv->reset);
 
 	/* power off the PHY by putting it into reset mode */
 	regmap_update_bits(priv->regmap, REG_CTRL, REG_CTRL_POWER_ON_RESET,
@@ -265,8 +269,9 @@ static int phy_meson8b_usb2_probe(struct platform_device *pdev)
 		return PTR_ERR(priv->clk_usb);
 
 	priv->reset = devm_reset_control_get_optional_shared(&pdev->dev, NULL);
-	if (PTR_ERR(priv->reset) == -EPROBE_DEFER)
-		return PTR_ERR(priv->reset);
+	if (IS_ERR(priv->reset))
+		return dev_err_probe(&pdev->dev, PTR_ERR(priv->reset),
+				     "Failed to get the reset line");
 
 	priv->dr_mode = of_usb_get_dr_mode_by_phy(pdev->dev.of_node, -1);
 	if (priv->dr_mode == USB_DR_MODE_UNKNOWN) {
diff --git a/drivers/phy/broadcom/phy-brcm-usb-init-synopsys.c b/drivers/phy/broadcom/phy-brcm-usb-init-synopsys.c
index e63457e145c7..d2524b70ea16 100644
--- a/drivers/phy/broadcom/phy-brcm-usb-init-synopsys.c
+++ b/drivers/phy/broadcom/phy-brcm-usb-init-synopsys.c
@@ -47,6 +47,8 @@
 #define   USB_CTRL_USB_PM_SOFT_RESET_MASK		0x40000000
 #define   USB_CTRL_USB_PM_BDC_SOFT_RESETB_MASK		0x00800000
 #define   USB_CTRL_USB_PM_XHC_SOFT_RESETB_MASK		0x00400000
+#define   USB_CTRL_USB_PM_XHC_PME_EN_MASK		0x00000010
+#define   USB_CTRL_USB_PM_XHC_S2_CLK_SWITCH_EN_MASK	0x00000008
 #define USB_CTRL_USB_PM_STATUS		0x08
 #define USB_CTRL_USB_DEVICE_CTL1	0x10
 #define   USB_CTRL_USB_DEVICE_CTL1_PORT_MODE_MASK	0x00000003
@@ -190,10 +192,6 @@ static void usb_init_common(struct brcm_usb_init_params *params)
 
 	pr_debug("%s\n", __func__);
 
-	USB_CTRL_UNSET(ctrl, USB_PM, USB_PWRDN);
-	/* 1 millisecond - for USB clocks to settle down */
-	usleep_range(1000, 2000);
-
 	if (USB_CTRL_MASK(USB_DEVICE_CTL1, PORT_MODE)) {
 		reg = brcm_usb_readl(USB_CTRL_REG(ctrl, USB_DEVICE_CTL1));
 		reg &= ~USB_CTRL_MASK(USB_DEVICE_CTL1, PORT_MODE);
@@ -222,6 +220,17 @@ static void usb_wake_enable_7211b0(struct brcm_usb_init_params *params,
 		USB_CTRL_UNSET(ctrl, CTLR_CSHCR, ctl_pme_en);
 }
 
+static void usb_wake_enable_7216(struct brcm_usb_init_params *params,
+				 bool enable)
+{
+	void __iomem *ctrl = params->regs[BRCM_REGS_CTRL];
+
+	if (enable)
+		USB_CTRL_SET(ctrl, USB_PM, XHC_PME_EN);
+	else
+		USB_CTRL_UNSET(ctrl, USB_PM, XHC_PME_EN);
+}
+
 static void usb_init_common_7211b0(struct brcm_usb_init_params *params)
 {
 	void __iomem *ctrl = params->regs[BRCM_REGS_CTRL];
@@ -295,6 +304,20 @@ static void usb_init_common_7211b0(struct brcm_usb_init_params *params)
 	usb2_eye_fix_7211b0(params);
 }
 
+static void usb_init_common_7216(struct brcm_usb_init_params *params)
+{
+	void __iomem *ctrl = params->regs[BRCM_REGS_CTRL];
+
+	USB_CTRL_UNSET(ctrl, USB_PM, XHC_S2_CLK_SWITCH_EN);
+	USB_CTRL_UNSET(ctrl, USB_PM, USB_PWRDN);
+
+	/* 1 millisecond - for USB clocks to settle down */
+	usleep_range(1000, 2000);
+
+	usb_wake_enable_7216(params, false);
+	usb_init_common(params);
+}
+
 static void usb_init_xhci(struct brcm_usb_init_params *params)
 {
 	pr_debug("%s\n", __func__);
@@ -302,14 +325,20 @@ static void usb_init_xhci(struct brcm_usb_init_params *params)
 	xhci_soft_reset(params, 0);
 }
 
-static void usb_uninit_common(struct brcm_usb_init_params *params)
+static void usb_uninit_common_7216(struct brcm_usb_init_params *params)
 {
 	void __iomem *ctrl = params->regs[BRCM_REGS_CTRL];
 
 	pr_debug("%s\n", __func__);
 
-	USB_CTRL_SET(ctrl, USB_PM, USB_PWRDN);
+	if (!params->wake_enabled) {
+		USB_CTRL_SET(ctrl, USB_PM, USB_PWRDN);
 
+		/* Switch to using slower clock during suspend to save power */
+		USB_CTRL_SET(ctrl, USB_PM, XHC_S2_CLK_SWITCH_EN);
+	} else {
+		usb_wake_enable_7216(params, true);
+	}
 }
 
 static void usb_uninit_common_7211b0(struct brcm_usb_init_params *params)
@@ -371,9 +400,9 @@ static void usb_set_dual_select(struct brcm_usb_init_params *params, int mode)
 
 static const struct brcm_usb_init_ops bcm7216_ops = {
 	.init_ipp = usb_init_ipp,
-	.init_common = usb_init_common,
+	.init_common = usb_init_common_7216,
 	.init_xhci = usb_init_xhci,
-	.uninit_common = usb_uninit_common,
+	.uninit_common = usb_uninit_common_7216,
 	.uninit_xhci = usb_uninit_xhci,
 	.get_dual_select = usb_get_dual_select,
 	.set_dual_select = usb_set_dual_select,
@@ -396,6 +425,7 @@ void brcm_usb_dvr_init_7216(struct brcm_usb_init_params *params)
 
 	params->family_name = "7216";
 	params->ops = &bcm7216_ops;
+	params->suspend_with_clocks = true;
 }
 
 void brcm_usb_dvr_init_7211b0(struct brcm_usb_init_params *params)
diff --git a/drivers/phy/broadcom/phy-brcm-usb-init.c b/drivers/phy/broadcom/phy-brcm-usb-init.c
index 9391ab42a12b..dd0f66288fbd 100644
--- a/drivers/phy/broadcom/phy-brcm-usb-init.c
+++ b/drivers/phy/broadcom/phy-brcm-usb-init.c
@@ -79,6 +79,7 @@
 
 enum brcm_family_type {
 	BRCM_FAMILY_3390A0,
+	BRCM_FAMILY_4908,
 	BRCM_FAMILY_7250B0,
 	BRCM_FAMILY_7271A0,
 	BRCM_FAMILY_7364A0,
@@ -96,6 +97,7 @@ enum brcm_family_type {
 
 static const char *family_names[BRCM_FAMILY_COUNT] = {
 	USB_BRCM_FAMILY(3390A0),
+	USB_BRCM_FAMILY(4908),
 	USB_BRCM_FAMILY(7250B0),
 	USB_BRCM_FAMILY(7271A0),
 	USB_BRCM_FAMILY(7364A0),
@@ -203,6 +205,27 @@ usb_reg_bits_map_table[BRCM_FAMILY_COUNT][USB_CTRL_SELECTOR_COUNT] = {
 		USB_CTRL_USB_PM_USB20_HC_RESETB_VAR_MASK,
 		ENDIAN_SETTINGS, /* USB_CTRL_SETUP ENDIAN bits */
 	},
+	/* 4908 */
+	[BRCM_FAMILY_4908] = {
+		0, /* USB_CTRL_SETUP_SCB1_EN_MASK */
+		0, /* USB_CTRL_SETUP_SCB2_EN_MASK */
+		0, /* USB_CTRL_SETUP_SS_EHCI64BIT_EN_MASK */
+		0, /* USB_CTRL_SETUP_STRAP_IPP_SEL_MASK */
+		0, /* USB_CTRL_SETUP_OC3_DISABLE_MASK */
+		0, /* USB_CTRL_PLL_CTL_PLL_IDDQ_PWRDN_MASK */
+		0, /* USB_CTRL_USB_PM_BDC_SOFT_RESETB_MASK */
+		USB_CTRL_USB_PM_XHC_SOFT_RESETB_MASK,
+		USB_CTRL_USB_PM_USB_PWRDN_MASK,
+		0, /* USB_CTRL_USB30_CTL1_XHC_SOFT_RESETB_MASK */
+		0, /* USB_CTRL_USB30_CTL1_USB3_IOC_MASK */
+		0, /* USB_CTRL_USB30_CTL1_USB3_IPP_MASK */
+		0, /* USB_CTRL_USB_DEVICE_CTL1_PORT_MODE_MASK */
+		0, /* USB_CTRL_USB_PM_SOFT_RESET_MASK */
+		0, /* USB_CTRL_SETUP_CC_DRD_MODE_ENABLE_MASK */
+		0, /* USB_CTRL_SETUP_STRAP_CC_DRD_MODE_ENABLE_SEL_MASK */
+		0, /* USB_CTRL_USB_PM_USB20_HC_RESETB_VAR_MASK */
+		0, /* USB_CTRL_SETUP ENDIAN bits */
+	},
 	/* 7250b0 */
 	[BRCM_FAMILY_7250B0] = {
 		USB_CTRL_SETUP_SCB1_EN_MASK,
@@ -559,6 +582,7 @@ static void brcmusb_usb3_pll_54mhz(struct brcm_usb_init_params *params)
 	 */
 	switch (params->selected_family) {
 	case BRCM_FAMILY_3390A0:
+	case BRCM_FAMILY_4908:
 	case BRCM_FAMILY_7250B0:
 	case BRCM_FAMILY_7366C0:
 	case BRCM_FAMILY_74371A0:
@@ -1004,6 +1028,18 @@ static const struct brcm_usb_init_ops bcm7445_ops = {
 	.set_dual_select = usb_set_dual_select,
 };
 
+void brcm_usb_dvr_init_4908(struct brcm_usb_init_params *params)
+{
+	int fam;
+
+	fam = BRCM_FAMILY_4908;
+	params->selected_family = fam;
+	params->usb_reg_bits_map =
+		&usb_reg_bits_map_table[fam][0];
+	params->family_name = family_names[fam];
+	params->ops = &bcm7445_ops;
+}
+
 void brcm_usb_dvr_init_7445(struct brcm_usb_init_params *params)
 {
 	int fam;
diff --git a/drivers/phy/broadcom/phy-brcm-usb-init.h b/drivers/phy/broadcom/phy-brcm-usb-init.h
index a39f30fa2e99..1ccb5ddab865 100644
--- a/drivers/phy/broadcom/phy-brcm-usb-init.h
+++ b/drivers/phy/broadcom/phy-brcm-usb-init.h
@@ -64,6 +64,7 @@ struct  brcm_usb_init_params {
 	bool suspend_with_clocks;
 };
 
+void brcm_usb_dvr_init_4908(struct brcm_usb_init_params *params);
 void brcm_usb_dvr_init_7445(struct brcm_usb_init_params *params);
 void brcm_usb_dvr_init_7216(struct brcm_usb_init_params *params);
 void brcm_usb_dvr_init_7211b0(struct brcm_usb_init_params *params);
diff --git a/drivers/phy/broadcom/phy-brcm-usb.c b/drivers/phy/broadcom/phy-brcm-usb.c
index 0f1deb6e0eab..2cb3779fcdf8 100644
--- a/drivers/phy/broadcom/phy-brcm-usb.c
+++ b/drivers/phy/broadcom/phy-brcm-usb.c
@@ -283,6 +283,15 @@ static const struct attribute_group brcm_usb_phy_group = {
 	.attrs = brcm_usb_phy_attrs,
 };
 
+static const struct match_chip_info chip_info_4908 = {
+	.init_func = &brcm_usb_dvr_init_4908,
+	.required_regs = {
+		BRCM_REGS_CTRL,
+		BRCM_REGS_XHCI_EC,
+		-1,
+	},
+};
+
 static const struct match_chip_info chip_info_7216 = {
 	.init_func = &brcm_usb_dvr_init_7216,
 	.required_regs = {
@@ -318,7 +327,7 @@ static const struct match_chip_info chip_info_7445 = {
 static const struct of_device_id brcm_usb_dt_ids[] = {
 	{
 		.compatible = "brcm,bcm4908-usb-phy",
-		.data = &chip_info_7445,
+		.data = &chip_info_4908,
 	},
 	{
 		.compatible = "brcm,bcm7216-usb-phy",
diff --git a/drivers/phy/cadence/Kconfig b/drivers/phy/cadence/Kconfig
index a62910ff5591..1adde2d99ae7 100644
--- a/drivers/phy/cadence/Kconfig
+++ b/drivers/phy/cadence/Kconfig
@@ -22,6 +22,14 @@ config PHY_CADENCE_DPHY
 	  system. If M is selected, the module will be called
 	  cdns-dphy.
 
+config PHY_CADENCE_DPHY_RX
+	tristate "Cadence D-PHY Rx Support"
+	depends on HAS_IOMEM && OF
+	select GENERIC_PHY
+	select GENERIC_PHY_MIPI_DPHY
+	help
+	  Support for Cadence D-PHY in Rx configuration.
+
 config PHY_CADENCE_SIERRA
 	tristate "Cadence Sierra PHY Driver"
 	depends on OF && HAS_IOMEM && RESET_CONTROLLER
diff --git a/drivers/phy/cadence/Makefile b/drivers/phy/cadence/Makefile
index 26e16bd34efe..e17f035ddece 100644
--- a/drivers/phy/cadence/Makefile
+++ b/drivers/phy/cadence/Makefile
@@ -1,5 +1,6 @@
 # SPDX-License-Identifier: GPL-2.0-only
 obj-$(CONFIG_PHY_CADENCE_TORRENT)	+= phy-cadence-torrent.o
 obj-$(CONFIG_PHY_CADENCE_DPHY)	+= cdns-dphy.o
+obj-$(CONFIG_PHY_CADENCE_DPHY_RX)	+= cdns-dphy-rx.o
 obj-$(CONFIG_PHY_CADENCE_SIERRA)	+= phy-cadence-sierra.o
 obj-$(CONFIG_PHY_CADENCE_SALVO)	+= phy-cadence-salvo.o
diff --git a/drivers/phy/cadence/cdns-dphy-rx.c b/drivers/phy/cadence/cdns-dphy-rx.c
new file mode 100644
index 000000000000..572c70089a94
--- /dev/null
+++ b/drivers/phy/cadence/cdns-dphy-rx.c
@@ -0,0 +1,255 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2022 Texas Instruments Incorporated - https://www.ti.com/
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bitops.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/phy/phy.h>
+#include <linux/phy/phy-mipi-dphy.h>
+#include <linux/platform_device.h>
+
+#define DPHY_PMA_CMN(reg)		(reg)
+#define DPHY_PCS(reg)			(0xb00 + (reg))
+#define DPHY_ISO(reg)			(0xc00 + (reg))
+
+#define DPHY_CMN_SSM			DPHY_PMA_CMN(0x20)
+#define DPHY_CMN_RX_MODE_EN		BIT(10)
+#define DPHY_CMN_RX_BANDGAP_TIMER_MASK	GENMASK(8, 1)
+#define DPHY_CMN_SSM_EN			BIT(0)
+
+#define DPHY_CMN_RX_BANDGAP_TIMER	0x14
+
+#define DPHY_BAND_CFG			DPHY_PCS(0x0)
+#define DPHY_BAND_CFG_RIGHT_BAND	GENMASK(9, 5)
+#define DPHY_BAND_CFG_LEFT_BAND		GENMASK(4, 0)
+
+#define DPHY_POWER_ISLAND_EN_DATA	DPHY_PCS(0x8)
+#define DPHY_POWER_ISLAND_EN_DATA_VAL	0xaaaaaaaa
+
+#define DPHY_POWER_ISLAND_EN_CLK	DPHY_PCS(0xc)
+#define DPHY_POWER_ISLAND_EN_CLK_VAL	0xaa
+
+#define DPHY_ISO_CL_CTRL_L		DPHY_ISO(0x10)
+#define DPHY_ISO_DL_CTRL_L0		DPHY_ISO(0x14)
+#define DPHY_ISO_DL_CTRL_L1		DPHY_ISO(0x20)
+#define DPHY_ISO_DL_CTRL_L2		DPHY_ISO(0x30)
+#define DPHY_ISO_DL_CTRL_L3		DPHY_ISO(0x3c)
+
+#define DPHY_ISO_LANE_READY_BIT		0
+#define DPHY_ISO_LANE_READY_TIMEOUT_MS	100UL
+
+#define DPHY_LANES_MIN			1
+#define DPHY_LANES_MAX			4
+
+struct cdns_dphy_rx {
+	void __iomem *regs;
+	struct device *dev;
+	struct phy *phy;
+};
+
+struct cdns_dphy_rx_band {
+	/* Rates are in Mbps. */
+	unsigned int min_rate;
+	unsigned int max_rate;
+};
+
+/* Order of bands is important since the index is the band number. */
+static const struct cdns_dphy_rx_band bands[] = {
+	{ 80, 100 }, { 100, 120 }, { 120, 160 }, { 160, 200 }, { 200, 240 },
+	{ 240, 280 }, { 280, 320 }, { 320, 360 }, { 360, 400 }, { 400, 480 },
+	{ 480, 560 }, { 560, 640 }, { 640, 720 }, { 720, 800 }, { 800, 880 },
+	{ 880, 1040 }, { 1040, 1200 }, { 1200, 1350 }, { 1350, 1500 },
+	{ 1500, 1750 }, { 1750, 2000 }, { 2000, 2250 }, { 2250, 2500 }
+};
+
+static int cdns_dphy_rx_power_on(struct phy *phy)
+{
+	struct cdns_dphy_rx *dphy = phy_get_drvdata(phy);
+
+	/* Start RX state machine. */
+	writel(DPHY_CMN_SSM_EN | DPHY_CMN_RX_MODE_EN |
+	       FIELD_PREP(DPHY_CMN_RX_BANDGAP_TIMER_MASK,
+			  DPHY_CMN_RX_BANDGAP_TIMER),
+	       dphy->regs + DPHY_CMN_SSM);
+
+	return 0;
+}
+
+static int cdns_dphy_rx_power_off(struct phy *phy)
+{
+	struct cdns_dphy_rx *dphy = phy_get_drvdata(phy);
+
+	writel(0, dphy->regs + DPHY_CMN_SSM);
+
+	return 0;
+}
+
+static int cdns_dphy_rx_get_band_ctrl(unsigned long hs_clk_rate)
+{
+	unsigned int rate, i;
+
+	rate = hs_clk_rate / 1000000UL;
+	/* Since CSI-2 clock is DDR, the bit rate is twice the clock rate. */
+	rate *= 2;
+
+	if (rate < bands[0].min_rate)
+		return -EOPNOTSUPP;
+
+	for (i = 0; i < ARRAY_SIZE(bands); i++)
+		if (rate < bands[i].max_rate)
+			return i;
+
+	return -EOPNOTSUPP;
+}
+
+static inline int cdns_dphy_rx_wait_for_bit(void __iomem *addr,
+					    unsigned int bit)
+{
+	u32 val;
+
+	return readl_relaxed_poll_timeout(addr, val, val & BIT(bit), 10,
+					  DPHY_ISO_LANE_READY_TIMEOUT_MS * 1000);
+}
+
+static int cdns_dphy_rx_wait_lane_ready(struct cdns_dphy_rx *dphy,
+					unsigned int lanes)
+{
+	static const u32 data_lane_ctrl[] = {DPHY_ISO_DL_CTRL_L0,
+					     DPHY_ISO_DL_CTRL_L1,
+					     DPHY_ISO_DL_CTRL_L2,
+					     DPHY_ISO_DL_CTRL_L3};
+	void __iomem *reg = dphy->regs;
+	unsigned int i;
+	int ret;
+
+	/* Clock lane */
+	ret = cdns_dphy_rx_wait_for_bit(reg + DPHY_ISO_CL_CTRL_L,
+					DPHY_ISO_LANE_READY_BIT);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < lanes; i++) {
+		ret = cdns_dphy_rx_wait_for_bit(reg + data_lane_ctrl[i],
+						DPHY_ISO_LANE_READY_BIT);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int cdns_dphy_rx_configure(struct phy *phy,
+				  union phy_configure_opts *opts)
+{
+	struct cdns_dphy_rx *dphy = phy_get_drvdata(phy);
+	unsigned int reg, lanes = opts->mipi_dphy.lanes;
+	int band_ctrl, ret;
+
+	/* Data lanes. Minimum one lane is mandatory. */
+	if (lanes < DPHY_LANES_MIN || lanes > DPHY_LANES_MAX)
+		return -EINVAL;
+
+	band_ctrl = cdns_dphy_rx_get_band_ctrl(opts->mipi_dphy.hs_clk_rate);
+	if (band_ctrl < 0)
+		return band_ctrl;
+
+	reg = FIELD_PREP(DPHY_BAND_CFG_LEFT_BAND, band_ctrl) |
+	      FIELD_PREP(DPHY_BAND_CFG_RIGHT_BAND, band_ctrl);
+	writel(reg, dphy->regs + DPHY_BAND_CFG);
+
+	/*
+	 * Set the required power island phase 2 time. This is mandated by DPHY
+	 * specs.
+	 */
+	reg = DPHY_POWER_ISLAND_EN_DATA_VAL;
+	writel(reg, dphy->regs + DPHY_POWER_ISLAND_EN_DATA);
+	reg = DPHY_POWER_ISLAND_EN_CLK_VAL;
+	writel(reg, dphy->regs + DPHY_POWER_ISLAND_EN_CLK);
+
+	ret = cdns_dphy_rx_wait_lane_ready(dphy, lanes);
+	if (ret) {
+		dev_err(dphy->dev, "DPHY wait for lane ready timeout\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int cdns_dphy_rx_validate(struct phy *phy, enum phy_mode mode,
+				 int submode, union phy_configure_opts *opts)
+{
+	int ret;
+
+	if (mode != PHY_MODE_MIPI_DPHY)
+		return -EINVAL;
+
+	ret = cdns_dphy_rx_get_band_ctrl(opts->mipi_dphy.hs_clk_rate);
+	if (ret < 0)
+		return ret;
+
+	return phy_mipi_dphy_config_validate(&opts->mipi_dphy);
+}
+
+static const struct phy_ops cdns_dphy_rx_ops = {
+	.power_on = cdns_dphy_rx_power_on,
+	.power_off = cdns_dphy_rx_power_off,
+	.configure = cdns_dphy_rx_configure,
+	.validate = cdns_dphy_rx_validate,
+};
+
+static int cdns_dphy_rx_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct phy_provider *provider;
+	struct cdns_dphy_rx *dphy;
+
+	dphy = devm_kzalloc(dev, sizeof(*dphy), GFP_KERNEL);
+	if (!dphy)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, dphy);
+	dphy->dev = dev;
+
+	dphy->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(dphy->regs))
+		return PTR_ERR(dphy->regs);
+
+	dphy->phy = devm_phy_create(dev, NULL, &cdns_dphy_rx_ops);
+	if (IS_ERR(dphy->phy)) {
+		dev_err(dev, "Failed to create PHY: %ld\n", PTR_ERR(dphy->phy));
+		return PTR_ERR(dphy->phy);
+	}
+
+	phy_set_drvdata(dphy->phy, dphy);
+	provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(provider)) {
+		dev_err(dev, "Failed to register PHY provider: %ld\n",
+			PTR_ERR(provider));
+		return PTR_ERR(provider);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id cdns_dphy_rx_of_match[] = {
+	{ .compatible = "cdns,dphy-rx" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, cdns_dphy_rx_of_match);
+
+static struct platform_driver cdns_dphy_rx_platform_driver = {
+	.probe		= cdns_dphy_rx_probe,
+	.driver		= {
+		.name		= "cdns-mipi-dphy-rx",
+		.of_match_table	= cdns_dphy_rx_of_match,
+	},
+};
+module_platform_driver(cdns_dphy_rx_platform_driver);
+
+MODULE_AUTHOR("Pratyush Yadav <p.yadav@ti.com>");
+MODULE_DESCRIPTION("Cadence D-PHY Rx Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/phy/cadence/phy-cadence-salvo.c b/drivers/phy/cadence/phy-cadence-salvo.c
index 51c0b98f5fd7..e569f5f67578 100644
--- a/drivers/phy/cadence/phy-cadence-salvo.c
+++ b/drivers/phy/cadence/phy-cadence-salvo.c
@@ -263,14 +263,9 @@ static int cdns_salvo_phy_probe(struct platform_device *pdev)
 	struct phy_provider *phy_provider;
 	struct device *dev = &pdev->dev;
 	struct cdns_salvo_phy *salvo_phy;
-	const struct of_device_id *match;
 	struct cdns_salvo_data *data;
 
-	match = of_match_device(cdns_salvo_phy_of_match, dev);
-	if (!match)
-		return -EINVAL;
-
-	data = (struct cdns_salvo_data *)match->data;
+	data = (struct cdns_salvo_data *)of_device_get_match_data(dev);
 	salvo_phy = devm_kzalloc(dev, sizeof(*salvo_phy), GFP_KERNEL);
 	if (!salvo_phy)
 		return -ENOMEM;
diff --git a/drivers/phy/cadence/phy-cadence-sierra.c b/drivers/phy/cadence/phy-cadence-sierra.c
index e265647e29a2..6b917f7bddbe 100644
--- a/drivers/phy/cadence/phy-cadence-sierra.c
+++ b/drivers/phy/cadence/phy-cadence-sierra.c
@@ -370,6 +370,7 @@ struct cdns_sierra_phy {
 	int nsubnodes;
 	u32 num_lanes;
 	bool autoconf;
+	int already_configured;
 	struct clk_onecell_data clk_data;
 	struct clk *output_clks[CDNS_SIERRA_OUTPUT_CLOCKS];
 };
@@ -517,7 +518,7 @@ static int cdns_sierra_phy_init(struct phy *gphy)
 	int i, j;
 
 	/* Initialise the PHY registers, unless auto configured */
-	if (phy->autoconf || phy->nsubnodes > 1)
+	if (phy->autoconf || phy->already_configured || phy->nsubnodes > 1)
 		return 0;
 
 	clk_set_rate(phy->input_clks[CMN_REFCLK_DIG_DIV], 25000000);
@@ -646,6 +647,18 @@ static const struct phy_ops ops = {
 	.owner		= THIS_MODULE,
 };
 
+static int cdns_sierra_noop_phy_on(struct phy *gphy)
+{
+	usleep_range(5000, 10000);
+
+	return 0;
+}
+
+static const struct phy_ops noop_ops = {
+	.power_on	= cdns_sierra_noop_phy_on,
+	.owner		= THIS_MODULE,
+};
+
 static u8 cdns_sierra_pll_mux_get_parent(struct clk_hw *hw)
 {
 	struct cdns_sierra_pll_mux *mux = to_cdns_sierra_pll_mux(hw);
@@ -1118,13 +1131,6 @@ static int cdns_sierra_phy_get_clocks(struct cdns_sierra_phy *sp,
 	struct clk *clk;
 	int ret;
 
-	clk = devm_clk_get_optional(dev, "phy_clk");
-	if (IS_ERR(clk)) {
-		dev_err(dev, "failed to get clock phy_clk\n");
-		return PTR_ERR(clk);
-	}
-	sp->input_clks[PHY_CLK] = clk;
-
 	clk = devm_clk_get_optional(dev, "cmn_refclk_dig_div");
 	if (IS_ERR(clk)) {
 		dev_err(dev, "cmn_refclk_dig_div clock not found\n");
@@ -1160,17 +1166,33 @@ static int cdns_sierra_phy_get_clocks(struct cdns_sierra_phy *sp,
 	return 0;
 }
 
-static int cdns_sierra_phy_enable_clocks(struct cdns_sierra_phy *sp)
+static int cdns_sierra_phy_clk(struct cdns_sierra_phy *sp)
 {
+	struct device *dev = sp->dev;
+	struct clk *clk;
 	int ret;
 
+	clk = devm_clk_get_optional(dev, "phy_clk");
+	if (IS_ERR(clk)) {
+		dev_err(dev, "failed to get clock phy_clk\n");
+		return PTR_ERR(clk);
+	}
+	sp->input_clks[PHY_CLK] = clk;
+
 	ret = clk_prepare_enable(sp->input_clks[PHY_CLK]);
 	if (ret)
 		return ret;
 
+	return 0;
+}
+
+static int cdns_sierra_phy_enable_clocks(struct cdns_sierra_phy *sp)
+{
+	int ret;
+
 	ret = clk_prepare_enable(sp->output_clks[CDNS_SIERRA_PLL_CMNLC]);
 	if (ret)
-		goto err_pll_cmnlc;
+		return ret;
 
 	ret = clk_prepare_enable(sp->output_clks[CDNS_SIERRA_PLL_CMNLC1]);
 	if (ret)
@@ -1181,9 +1203,6 @@ static int cdns_sierra_phy_enable_clocks(struct cdns_sierra_phy *sp)
 err_pll_cmnlc1:
 	clk_disable_unprepare(sp->output_clks[CDNS_SIERRA_PLL_CMNLC]);
 
-err_pll_cmnlc:
-	clk_disable_unprepare(sp->input_clks[PHY_CLK]);
-
 	return ret;
 }
 
@@ -1191,7 +1210,8 @@ static void cdns_sierra_phy_disable_clocks(struct cdns_sierra_phy *sp)
 {
 	clk_disable_unprepare(sp->output_clks[CDNS_SIERRA_PLL_CMNLC1]);
 	clk_disable_unprepare(sp->output_clks[CDNS_SIERRA_PLL_CMNLC]);
-	clk_disable_unprepare(sp->input_clks[PHY_CLK]);
+	if (!sp->already_configured)
+		clk_disable_unprepare(sp->input_clks[PHY_CLK]);
 }
 
 static int cdns_sierra_phy_get_resets(struct cdns_sierra_phy *sp,
@@ -1382,22 +1402,30 @@ static int cdns_sierra_phy_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	ret = cdns_sierra_phy_get_resets(sp, dev);
-	if (ret)
-		goto unregister_clk;
-
 	ret = cdns_sierra_phy_enable_clocks(sp);
 	if (ret)
 		goto unregister_clk;
 
-	/* Enable APB */
-	reset_control_deassert(sp->apb_rst);
+	regmap_field_read(sp->pma_cmn_ready, &sp->already_configured);
+
+	if (!sp->already_configured) {
+		ret = cdns_sierra_phy_clk(sp);
+		if (ret)
+			goto clk_disable;
+
+		ret = cdns_sierra_phy_get_resets(sp, dev);
+		if (ret)
+			goto clk_disable;
+
+		/* Enable APB */
+		reset_control_deassert(sp->apb_rst);
+	}
 
 	/* Check that PHY is present */
 	regmap_field_read(sp->macro_id_type, &id_value);
 	if  (sp->init_data->id_value != id_value) {
 		ret = -EINVAL;
-		goto clk_disable;
+		goto ctrl_assert;
 	}
 
 	sp->autoconf = of_property_read_bool(dn, "cdns,autoconf");
@@ -1433,8 +1461,10 @@ static int cdns_sierra_phy_probe(struct platform_device *pdev)
 
 		sp->num_lanes += sp->phys[node].num_lanes;
 
-		gphy = devm_phy_create(dev, child, &ops);
-
+		if (!sp->already_configured)
+			gphy = devm_phy_create(dev, child, &ops);
+		else
+			gphy = devm_phy_create(dev, child, &noop_ops);
 		if (IS_ERR(gphy)) {
 			ret = PTR_ERR(gphy);
 			of_node_put(child);
@@ -1455,7 +1485,7 @@ static int cdns_sierra_phy_probe(struct platform_device *pdev)
 	}
 
 	/* If more than one subnode, configure the PHY as multilink */
-	if (!sp->autoconf && sp->nsubnodes > 1) {
+	if (!sp->already_configured && !sp->autoconf && sp->nsubnodes > 1) {
 		ret = cdns_sierra_phy_configure_multilink(sp);
 		if (ret)
 			goto put_control;
@@ -1473,9 +1503,11 @@ static int cdns_sierra_phy_probe(struct platform_device *pdev)
 put_control:
 	while (--node >= 0)
 		reset_control_put(sp->phys[node].lnk_rst);
+ctrl_assert:
+	if (!sp->already_configured)
+		reset_control_assert(sp->apb_rst);
 clk_disable:
 	cdns_sierra_phy_disable_clocks(sp);
-	reset_control_assert(sp->apb_rst);
 unregister_clk:
 	cdns_sierra_clk_unregister(sp);
 	return ret;
diff --git a/drivers/phy/freescale/Kconfig b/drivers/phy/freescale/Kconfig
index 0e91cd99c36b..8d945211c7b4 100644
--- a/drivers/phy/freescale/Kconfig
+++ b/drivers/phy/freescale/Kconfig
@@ -1,4 +1,7 @@
 # SPDX-License-Identifier: GPL-2.0-only
+
+if (ARCH_MXC && ARM64) || COMPILE_TEST
+
 config PHY_FSL_IMX8MQ_USB
 	tristate "Freescale i.MX8M USB3 PHY"
 	depends on OF && HAS_IOMEM
@@ -32,3 +35,5 @@ config PHY_FSL_LYNX_28G
 	  found on NXP's Layerscape platforms such as LX2160A.
 	  Used to change the protocol running on SerDes lanes at runtime.
 	  Only useful for a restricted set of Ethernet protocols.
+
+endif
diff --git a/drivers/phy/freescale/phy-fsl-imx8m-pcie.c b/drivers/phy/freescale/phy-fsl-imx8m-pcie.c
index 04b1aafb29f4..f1eb03ba25d6 100644
--- a/drivers/phy/freescale/phy-fsl-imx8m-pcie.c
+++ b/drivers/phy/freescale/phy-fsl-imx8m-pcie.c
@@ -5,9 +5,9 @@
 
 #include <linux/bitfield.h>
 #include <linux/clk.h>
+#include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/iopoll.h>
-#include <linux/delay.h>
 #include <linux/mfd/syscon.h>
 #include <linux/mfd/syscon/imx7-iomuxc-gpr.h>
 #include <linux/module.h>
@@ -15,6 +15,7 @@
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/reset.h>
+
 #include <dt-bindings/phy/phy-imx8-pcie.h>
 
 #define IMX8MM_PCIE_PHY_CMN_REG061	0x184
diff --git a/drivers/phy/marvell/phy-mvebu-a3700-comphy.c b/drivers/phy/marvell/phy-mvebu-a3700-comphy.c
index 6781488cfc58..a4d7d9bd100d 100644
--- a/drivers/phy/marvell/phy-mvebu-a3700-comphy.c
+++ b/drivers/phy/marvell/phy-mvebu-a3700-comphy.c
@@ -5,12 +5,16 @@
  * Authors:
  *   Evan Wang <xswang@marvell.com>
  *   Miquèl Raynal <miquel.raynal@bootlin.com>
+ *   Pali Rohár <pali@kernel.org>
+ *   Marek Behún <kabel@kernel.org>
  *
  * Structure inspired from phy-mvebu-cp110-comphy.c written by Antoine Tenart.
- * SMC call initial support done by Grzegorz Jaszczyk.
+ * Comphy code from ARM Trusted Firmware ported by Pali Rohár <pali@kernel.org>
+ * and Marek Behún <kabel@kernel.org>.
  */
 
-#include <linux/arm-smccc.h>
+#include <linux/bitfield.h>
+#include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/iopoll.h>
 #include <linux/mfd/syscon.h>
@@ -18,109 +22,1118 @@
 #include <linux/phy.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
+#include <linux/spinlock.h>
 
-#define MVEBU_A3700_COMPHY_LANES		3
-#define MVEBU_A3700_COMPHY_PORTS		2
-
-/* COMPHY Fast SMC function identifiers */
-#define COMPHY_SIP_POWER_ON			0x82000001
-#define COMPHY_SIP_POWER_OFF			0x82000002
-#define COMPHY_SIP_PLL_LOCK			0x82000003
-
-#define COMPHY_FW_MODE_SATA			0x1
-#define COMPHY_FW_MODE_SGMII			0x2
-#define COMPHY_FW_MODE_2500BASEX		0x3
-#define COMPHY_FW_MODE_USB3H			0x4
-#define COMPHY_FW_MODE_USB3D			0x5
-#define COMPHY_FW_MODE_PCIE			0x6
-#define COMPHY_FW_MODE_USB3			0xa
-
-#define COMPHY_FW_SPEED_1_25G			0 /* SGMII 1G */
-#define COMPHY_FW_SPEED_2_5G			1
-#define COMPHY_FW_SPEED_3_125G			2 /* 2500BASE-X */
-#define COMPHY_FW_SPEED_5G			3
-#define COMPHY_FW_SPEED_MAX			0x3F
-
-#define COMPHY_FW_MODE(mode)			((mode) << 12)
-#define COMPHY_FW_NET(mode, idx, speed)		(COMPHY_FW_MODE(mode) | \
-						 ((idx) << 8) |	\
-						 ((speed) << 2))
-#define COMPHY_FW_PCIE(mode, idx, speed, width)	(COMPHY_FW_NET(mode, idx, speed) | \
-						 ((width) << 18))
+#define PLL_SET_DELAY_US		600
+#define COMPHY_PLL_SLEEP		1000
+#define COMPHY_PLL_TIMEOUT		150000
+
+/* Comphy lane2 indirect access register offset */
+#define COMPHY_LANE2_INDIR_ADDR		0x0
+#define COMPHY_LANE2_INDIR_DATA		0x4
+
+/* SATA and USB3 PHY offset compared to SATA PHY */
+#define COMPHY_LANE2_REGS_BASE		0x200
+
+/*
+ * When accessing common PHY lane registers directly, we need to shift by 1,
+ * since the registers are 16-bit.
+ */
+#define COMPHY_LANE_REG_DIRECT(reg)	(((reg) & 0x7FF) << 1)
+
+/* COMPHY registers */
+#define COMPHY_POWER_PLL_CTRL		0x01
+#define PU_IVREF_BIT			BIT(15)
+#define PU_PLL_BIT			BIT(14)
+#define PU_RX_BIT			BIT(13)
+#define PU_TX_BIT			BIT(12)
+#define PU_TX_INTP_BIT			BIT(11)
+#define PU_DFE_BIT			BIT(10)
+#define RESET_DTL_RX_BIT		BIT(9)
+#define PLL_LOCK_BIT			BIT(8)
+#define REF_FREF_SEL_MASK		GENMASK(4, 0)
+#define REF_FREF_SEL_SERDES_25MHZ	FIELD_PREP(REF_FREF_SEL_MASK, 0x1)
+#define REF_FREF_SEL_SERDES_40MHZ	FIELD_PREP(REF_FREF_SEL_MASK, 0x3)
+#define REF_FREF_SEL_SERDES_50MHZ	FIELD_PREP(REF_FREF_SEL_MASK, 0x4)
+#define REF_FREF_SEL_PCIE_USB3_25MHZ	FIELD_PREP(REF_FREF_SEL_MASK, 0x2)
+#define REF_FREF_SEL_PCIE_USB3_40MHZ	FIELD_PREP(REF_FREF_SEL_MASK, 0x3)
+#define COMPHY_MODE_MASK		GENMASK(7, 5)
+#define COMPHY_MODE_SATA		FIELD_PREP(COMPHY_MODE_MASK, 0x0)
+#define COMPHY_MODE_PCIE		FIELD_PREP(COMPHY_MODE_MASK, 0x3)
+#define COMPHY_MODE_SERDES		FIELD_PREP(COMPHY_MODE_MASK, 0x4)
+#define COMPHY_MODE_USB3		FIELD_PREP(COMPHY_MODE_MASK, 0x5)
+
+#define COMPHY_KVCO_CAL_CTRL		0x02
+#define USE_MAX_PLL_RATE_BIT		BIT(12)
+#define SPEED_PLL_MASK			GENMASK(7, 2)
+#define SPEED_PLL_VALUE_16		FIELD_PREP(SPEED_PLL_MASK, 0x10)
+
+#define COMPHY_DIG_LOOPBACK_EN		0x23
+#define SEL_DATA_WIDTH_MASK		GENMASK(11, 10)
+#define DATA_WIDTH_10BIT		FIELD_PREP(SEL_DATA_WIDTH_MASK, 0x0)
+#define DATA_WIDTH_20BIT		FIELD_PREP(SEL_DATA_WIDTH_MASK, 0x1)
+#define DATA_WIDTH_40BIT		FIELD_PREP(SEL_DATA_WIDTH_MASK, 0x2)
+#define PLL_READY_TX_BIT		BIT(4)
+
+#define COMPHY_SYNC_PATTERN		0x24
+#define TXD_INVERT_BIT			BIT(10)
+#define RXD_INVERT_BIT			BIT(11)
+
+#define COMPHY_SYNC_MASK_GEN		0x25
+#define PHY_GEN_MAX_MASK		GENMASK(11, 10)
+#define PHY_GEN_MAX_USB3_5G		FIELD_PREP(PHY_GEN_MAX_MASK, 0x1)
+
+#define COMPHY_ISOLATION_CTRL		0x26
+#define PHY_ISOLATE_MODE		BIT(15)
+
+#define COMPHY_GEN2_SET2		0x3e
+#define GS2_TX_SSC_AMP_MASK		GENMASK(15, 9)
+#define GS2_TX_SSC_AMP_4128		FIELD_PREP(GS2_TX_SSC_AMP_MASK, 0x20)
+#define GS2_VREG_RXTX_MAS_ISET_MASK	GENMASK(8, 7)
+#define GS2_VREG_RXTX_MAS_ISET_60U	FIELD_PREP(GS2_VREG_RXTX_MAS_ISET_MASK,\
+						   0x0)
+#define GS2_VREG_RXTX_MAS_ISET_80U	FIELD_PREP(GS2_VREG_RXTX_MAS_ISET_MASK,\
+						   0x1)
+#define GS2_VREG_RXTX_MAS_ISET_100U	FIELD_PREP(GS2_VREG_RXTX_MAS_ISET_MASK,\
+						   0x2)
+#define GS2_VREG_RXTX_MAS_ISET_120U	FIELD_PREP(GS2_VREG_RXTX_MAS_ISET_MASK,\
+						   0x3)
+#define GS2_RSVD_6_0_MASK		GENMASK(6, 0)
+
+#define COMPHY_GEN3_SET2		0x3f
+
+#define COMPHY_IDLE_SYNC_EN		0x48
+#define IDLE_SYNC_EN			BIT(12)
+
+#define COMPHY_MISC_CTRL0		0x4F
+#define CLK100M_125M_EN			BIT(4)
+#define TXDCLK_2X_SEL			BIT(6)
+#define CLK500M_EN			BIT(7)
+#define PHY_REF_CLK_SEL			BIT(10)
+
+#define COMPHY_SFT_RESET		0x52
+#define SFT_RST				BIT(9)
+#define SFT_RST_NO_REG			BIT(10)
+
+#define COMPHY_MISC_CTRL1		0x73
+#define SEL_BITS_PCIE_FORCE		BIT(15)
+
+#define COMPHY_GEN2_SET3		0x112
+#define GS3_FFE_CAP_SEL_MASK		GENMASK(3, 0)
+#define GS3_FFE_CAP_SEL_VALUE		FIELD_PREP(GS3_FFE_CAP_SEL_MASK, 0xF)
+
+/* PIPE registers */
+#define COMPHY_PIPE_LANE_CFG0		0x180
+#define PRD_TXDEEMPH0_MASK		BIT(0)
+#define PRD_TXMARGIN_MASK		GENMASK(3, 1)
+#define PRD_TXSWING_MASK		BIT(4)
+#define CFG_TX_ALIGN_POS_MASK		GENMASK(8, 5)
+
+#define COMPHY_PIPE_LANE_CFG1		0x181
+#define PRD_TXDEEMPH1_MASK		BIT(15)
+#define USE_MAX_PLL_RATE_EN		BIT(9)
+#define TX_DET_RX_MODE			BIT(6)
+#define GEN2_TX_DATA_DLY_MASK		GENMASK(4, 3)
+#define GEN2_TX_DATA_DLY_DEFT		FIELD_PREP(GEN2_TX_DATA_DLY_MASK, 2)
+#define TX_ELEC_IDLE_MODE_EN		BIT(0)
+
+#define COMPHY_PIPE_LANE_STAT1		0x183
+#define TXDCLK_PCLK_EN			BIT(0)
+
+#define COMPHY_PIPE_LANE_CFG4		0x188
+#define SPREAD_SPECTRUM_CLK_EN		BIT(7)
+
+#define COMPHY_PIPE_RST_CLK_CTRL	0x1C1
+#define PIPE_SOFT_RESET			BIT(0)
+#define PIPE_REG_RESET			BIT(1)
+#define MODE_CORE_CLK_FREQ_SEL		BIT(9)
+#define MODE_PIPE_WIDTH_32		BIT(3)
+#define MODE_REFDIV_MASK		GENMASK(5, 4)
+#define MODE_REFDIV_BY_4		FIELD_PREP(MODE_REFDIV_MASK, 0x2)
+
+#define COMPHY_PIPE_TEST_MODE_CTRL	0x1C2
+#define MODE_MARGIN_OVERRIDE		BIT(2)
+
+#define COMPHY_PIPE_CLK_SRC_LO		0x1C3
+#define MODE_CLK_SRC			BIT(0)
+#define BUNDLE_PERIOD_SEL		BIT(1)
+#define BUNDLE_PERIOD_SCALE_MASK	GENMASK(3, 2)
+#define BUNDLE_SAMPLE_CTRL		BIT(4)
+#define PLL_READY_DLY_MASK		GENMASK(7, 5)
+#define CFG_SEL_20B			BIT(15)
+
+#define COMPHY_PIPE_PWR_MGM_TIM1	0x1D0
+#define CFG_PM_OSCCLK_WAIT_MASK		GENMASK(15, 12)
+#define CFG_PM_RXDEN_WAIT_MASK		GENMASK(11, 8)
+#define CFG_PM_RXDEN_WAIT_1_UNIT	FIELD_PREP(CFG_PM_RXDEN_WAIT_MASK, 0x1)
+#define CFG_PM_RXDLOZ_WAIT_MASK		GENMASK(7, 0)
+#define CFG_PM_RXDLOZ_WAIT_7_UNIT	FIELD_PREP(CFG_PM_RXDLOZ_WAIT_MASK, 0x7)
+#define CFG_PM_RXDLOZ_WAIT_12_UNIT	FIELD_PREP(CFG_PM_RXDLOZ_WAIT_MASK, 0xC)
+
+/*
+ * This register is not from PHY lane register space. It only exists in the
+ * indirect register space, before the actual PHY lane 2 registers. So the
+ * offset is absolute, not relative to COMPHY_LANE2_REGS_BASE.
+ * It is used only for SATA PHY initialization.
+ */
+#define COMPHY_RESERVED_REG		0x0E
+#define PHYCTRL_FRM_PIN_BIT		BIT(13)
+
+/* South Bridge PHY Configuration Registers */
+#define COMPHY_PHY_REG(lane, reg)	(((1 - (lane)) * 0x28) + ((reg) & 0x3f))
+
+/*
+ * lane0: USB3/GbE1 PHY Configuration 1
+ * lane1: PCIe/GbE0 PHY Configuration 1
+ * (used only by SGMII code)
+ */
+#define COMPHY_PHY_CFG1			0x0
+#define PIN_PU_IVREF_BIT		BIT(1)
+#define PIN_RESET_CORE_BIT		BIT(11)
+#define PIN_RESET_COMPHY_BIT		BIT(12)
+#define PIN_PU_PLL_BIT			BIT(16)
+#define PIN_PU_RX_BIT			BIT(17)
+#define PIN_PU_TX_BIT			BIT(18)
+#define PIN_TX_IDLE_BIT			BIT(19)
+#define GEN_RX_SEL_MASK			GENMASK(25, 22)
+#define GEN_RX_SEL_VALUE(val)		FIELD_PREP(GEN_RX_SEL_MASK, (val))
+#define GEN_TX_SEL_MASK			GENMASK(29, 26)
+#define GEN_TX_SEL_VALUE(val)		FIELD_PREP(GEN_TX_SEL_MASK, (val))
+#define SERDES_SPEED_1_25_G		0x6
+#define SERDES_SPEED_3_125_G		0x8
+#define PHY_RX_INIT_BIT			BIT(30)
+
+/*
+ * lane0: USB3/GbE1 PHY Status 1
+ * lane1: PCIe/GbE0 PHY Status 1
+ * (used only by SGMII code)
+ */
+#define COMPHY_PHY_STAT1		0x18
+#define PHY_RX_INIT_DONE_BIT		BIT(0)
+#define PHY_PLL_READY_RX_BIT		BIT(2)
+#define PHY_PLL_READY_TX_BIT		BIT(3)
+
+/* PHY Selector */
+#define COMPHY_SELECTOR_PHY_REG			0xFC
+/* bit0: 0: Lane1 is GbE0; 1: Lane1 is PCIe */
+#define COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT	BIT(0)
+/* bit4: 0: Lane0 is GbE1; 1: Lane0 is USB3 */
+#define COMPHY_SELECTOR_USB3_GBE1_SEL_BIT	BIT(4)
+/* bit8: 0: Lane0 is USB3 instead of GbE1, Lane2 is SATA; 1: Lane2 is USB3 */
+#define COMPHY_SELECTOR_USB3_PHY_SEL_BIT	BIT(8)
 
 struct mvebu_a3700_comphy_conf {
 	unsigned int lane;
 	enum phy_mode mode;
 	int submode;
-	unsigned int port;
-	u32 fw_mode;
 };
 
-#define MVEBU_A3700_COMPHY_CONF(_lane, _mode, _smode, _port, _fw)	\
+#define MVEBU_A3700_COMPHY_CONF(_lane, _mode, _smode)			\
 	{								\
 		.lane = _lane,						\
 		.mode = _mode,						\
 		.submode = _smode,					\
-		.port = _port,						\
-		.fw_mode = _fw,						\
 	}
 
-#define MVEBU_A3700_COMPHY_CONF_GEN(_lane, _mode, _port, _fw) \
-	MVEBU_A3700_COMPHY_CONF(_lane, _mode, PHY_INTERFACE_MODE_NA, _port, _fw)
+#define MVEBU_A3700_COMPHY_CONF_GEN(_lane, _mode) \
+	MVEBU_A3700_COMPHY_CONF(_lane, _mode, PHY_INTERFACE_MODE_NA)
 
-#define MVEBU_A3700_COMPHY_CONF_ETH(_lane, _smode, _port, _fw) \
-	MVEBU_A3700_COMPHY_CONF(_lane, PHY_MODE_ETHERNET, _smode, _port, _fw)
+#define MVEBU_A3700_COMPHY_CONF_ETH(_lane, _smode) \
+	MVEBU_A3700_COMPHY_CONF(_lane, PHY_MODE_ETHERNET, _smode)
 
 static const struct mvebu_a3700_comphy_conf mvebu_a3700_comphy_modes[] = {
 	/* lane 0 */
-	MVEBU_A3700_COMPHY_CONF_GEN(0, PHY_MODE_USB_HOST_SS, 0,
-				    COMPHY_FW_MODE_USB3H),
-	MVEBU_A3700_COMPHY_CONF_ETH(0, PHY_INTERFACE_MODE_SGMII, 1,
-				    COMPHY_FW_MODE_SGMII),
-	MVEBU_A3700_COMPHY_CONF_ETH(0, PHY_INTERFACE_MODE_2500BASEX, 1,
-				    COMPHY_FW_MODE_2500BASEX),
+	MVEBU_A3700_COMPHY_CONF_GEN(0, PHY_MODE_USB_HOST_SS),
+	MVEBU_A3700_COMPHY_CONF_ETH(0, PHY_INTERFACE_MODE_SGMII),
+	MVEBU_A3700_COMPHY_CONF_ETH(0, PHY_INTERFACE_MODE_1000BASEX),
+	MVEBU_A3700_COMPHY_CONF_ETH(0, PHY_INTERFACE_MODE_2500BASEX),
 	/* lane 1 */
-	MVEBU_A3700_COMPHY_CONF_GEN(1, PHY_MODE_PCIE, 0,
-				    COMPHY_FW_MODE_PCIE),
-	MVEBU_A3700_COMPHY_CONF_ETH(1, PHY_INTERFACE_MODE_SGMII, 0,
-				    COMPHY_FW_MODE_SGMII),
-	MVEBU_A3700_COMPHY_CONF_ETH(1, PHY_INTERFACE_MODE_2500BASEX, 0,
-				    COMPHY_FW_MODE_2500BASEX),
+	MVEBU_A3700_COMPHY_CONF_GEN(1, PHY_MODE_PCIE),
+	MVEBU_A3700_COMPHY_CONF_ETH(1, PHY_INTERFACE_MODE_SGMII),
+	MVEBU_A3700_COMPHY_CONF_ETH(1, PHY_INTERFACE_MODE_1000BASEX),
+	MVEBU_A3700_COMPHY_CONF_ETH(1, PHY_INTERFACE_MODE_2500BASEX),
 	/* lane 2 */
-	MVEBU_A3700_COMPHY_CONF_GEN(2, PHY_MODE_SATA, 0,
-				    COMPHY_FW_MODE_SATA),
-	MVEBU_A3700_COMPHY_CONF_GEN(2, PHY_MODE_USB_HOST_SS, 0,
-				    COMPHY_FW_MODE_USB3H),
+	MVEBU_A3700_COMPHY_CONF_GEN(2, PHY_MODE_SATA),
+	MVEBU_A3700_COMPHY_CONF_GEN(2, PHY_MODE_USB_HOST_SS),
+};
+
+struct mvebu_a3700_comphy_priv {
+	void __iomem *comphy_regs;
+	void __iomem *lane0_phy_regs; /* USB3 and GbE1 */
+	void __iomem *lane1_phy_regs; /* PCIe and GbE0 */
+	void __iomem *lane2_phy_indirect; /* SATA and USB3 */
+	spinlock_t lock; /* for PHY selector access */
+	bool xtal_is_40m;
 };
 
 struct mvebu_a3700_comphy_lane {
+	struct mvebu_a3700_comphy_priv *priv;
 	struct device *dev;
 	unsigned int id;
 	enum phy_mode mode;
 	int submode;
-	int port;
+	bool invert_tx;
+	bool invert_rx;
+	bool needs_reset;
+};
+
+struct gbe_phy_init_data_fix {
+	u16 addr;
+	u16 value;
 };
 
-static int mvebu_a3700_comphy_smc(unsigned long function, unsigned long lane,
-				  unsigned long mode)
+/* Changes to 40M1G25 mode data required for running 40M3G125 init mode */
+static struct gbe_phy_init_data_fix gbe_phy_init_fix[] = {
+	{ 0x005, 0x07CC }, { 0x015, 0x0000 }, { 0x01B, 0x0000 },
+	{ 0x01D, 0x0000 }, { 0x01E, 0x0000 }, { 0x01F, 0x0000 },
+	{ 0x020, 0x0000 }, { 0x021, 0x0030 }, { 0x026, 0x0888 },
+	{ 0x04D, 0x0152 }, { 0x04F, 0xA020 }, { 0x050, 0x07CC },
+	{ 0x053, 0xE9CA }, { 0x055, 0xBD97 }, { 0x071, 0x3015 },
+	{ 0x076, 0x03AA }, { 0x07C, 0x0FDF }, { 0x0C2, 0x3030 },
+	{ 0x0C3, 0x8000 }, { 0x0E2, 0x5550 }, { 0x0E3, 0x12A4 },
+	{ 0x0E4, 0x7D00 }, { 0x0E6, 0x0C83 }, { 0x101, 0xFCC0 },
+	{ 0x104, 0x0C10 }
+};
+
+/* 40M1G25 mode init data */
+static u16 gbe_phy_init[512] = {
+	/* 0       1       2       3       4       5       6       7 */
+	/*-----------------------------------------------------------*/
+	/* 8       9       A       B       C       D       E       F */
+	0x3110, 0xFD83, 0x6430, 0x412F, 0x82C0, 0x06FA, 0x4500, 0x6D26,	/* 00 */
+	0xAFC0, 0x8000, 0xC000, 0x0000, 0x2000, 0x49CC, 0x0BC9, 0x2A52,	/* 08 */
+	0x0BD2, 0x0CDE, 0x13D2, 0x0CE8, 0x1149, 0x10E0, 0x0000, 0x0000,	/* 10 */
+	0x0000, 0x0000, 0x0000, 0x0001, 0x0000, 0x4134, 0x0D2D, 0xFFFF,	/* 18 */
+	0xFFE0, 0x4030, 0x1016, 0x0030, 0x0000, 0x0800, 0x0866, 0x0000,	/* 20 */
+	0x0000, 0x0000, 0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF,	/* 28 */
+	0xFFFF, 0xFFFF, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/* 30 */
+	0x0000, 0x0000, 0x000F, 0x6A62, 0x1988, 0x3100, 0x3100, 0x3100,	/* 38 */
+	0x3100, 0xA708, 0x2430, 0x0830, 0x1030, 0x4610, 0xFF00, 0xFF00,	/* 40 */
+	0x0060, 0x1000, 0x0400, 0x0040, 0x00F0, 0x0155, 0x1100, 0xA02A,	/* 48 */
+	0x06FA, 0x0080, 0xB008, 0xE3ED, 0x5002, 0xB592, 0x7A80, 0x0001,	/* 50 */
+	0x020A, 0x8820, 0x6014, 0x8054, 0xACAA, 0xFC88, 0x2A02, 0x45CF,	/* 58 */
+	0x000F, 0x1817, 0x2860, 0x064F, 0x0000, 0x0204, 0x1800, 0x6000,	/* 60 */
+	0x810F, 0x4F23, 0x4000, 0x4498, 0x0850, 0x0000, 0x000E, 0x1002,	/* 68 */
+	0x9D3A, 0x3009, 0xD066, 0x0491, 0x0001, 0x6AB0, 0x0399, 0x3780,	/* 70 */
+	0x0040, 0x5AC0, 0x4A80, 0x0000, 0x01DF, 0x0000, 0x0007, 0x0000,	/* 78 */
+	0x2D54, 0x00A1, 0x4000, 0x0100, 0xA20A, 0x0000, 0x0000, 0x0000,	/* 80 */
+	0x0000, 0x0000, 0x0000, 0x7400, 0x0E81, 0x1000, 0x1242, 0x0210,	/* 88 */
+	0x80DF, 0x0F1F, 0x2F3F, 0x4F5F, 0x6F7F, 0x0F1F, 0x2F3F, 0x4F5F,	/* 90 */
+	0x6F7F, 0x4BAD, 0x0000, 0x0000, 0x0800, 0x0000, 0x2400, 0xB651,	/* 98 */
+	0xC9E0, 0x4247, 0x0A24, 0x0000, 0xAF19, 0x1004, 0x0000, 0x0000,	/* A0 */
+	0x0000, 0x0013, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/* A8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/* B0 */
+	0x0000, 0x0000, 0x0000, 0x0060, 0x0000, 0x0000, 0x0000, 0x0000,	/* B8 */
+	0x0000, 0x0000, 0x3010, 0xFA00, 0x0000, 0x0000, 0x0000, 0x0003,	/* C0 */
+	0x1618, 0x8200, 0x8000, 0x0400, 0x050F, 0x0000, 0x0000, 0x0000,	/* C8 */
+	0x4C93, 0x0000, 0x1000, 0x1120, 0x0010, 0x1242, 0x1242, 0x1E00,	/* D0 */
+	0x0000, 0x0000, 0x0000, 0x00F8, 0x0000, 0x0041, 0x0800, 0x0000,	/* D8 */
+	0x82A0, 0x572E, 0x2490, 0x14A9, 0x4E00, 0x0000, 0x0803, 0x0541,	/* E0 */
+	0x0C15, 0x0000, 0x0000, 0x0400, 0x2626, 0x0000, 0x0000, 0x4200,	/* E8 */
+	0x0000, 0xAA55, 0x1020, 0x0000, 0x0000, 0x5010, 0x0000, 0x0000,	/* F0 */
+	0x0000, 0x0000, 0x5000, 0x0000, 0x0000, 0x0000, 0x02F2, 0x0000,	/* F8 */
+	0x101F, 0xFDC0, 0x4000, 0x8010, 0x0110, 0x0006, 0x0000, 0x0000,	/*100 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*108 */
+	0x04CF, 0x0000, 0x04CF, 0x0000, 0x04CF, 0x0000, 0x04C6, 0x0000,	/*110 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*118 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*120 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*128 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*130 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*138 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*140 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*148 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*150 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*158 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*160 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*168 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*170 */
+	0x0000, 0x0000, 0x0000, 0x00F0, 0x08A2, 0x3112, 0x0A14, 0x0000,	/*178 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*180 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*188 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*190 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*198 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1A0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1A8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1B0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1B8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1C0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1C8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1D0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1D8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1E0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1E8 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,	/*1F0 */
+	0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000	/*1F8 */
+};
+
+static inline void comphy_reg_set(void __iomem *addr, u32 data, u32 mask)
 {
-	struct arm_smccc_res res;
-	s32 ret;
+	u32 val;
 
-	arm_smccc_smc(function, lane, mode, 0, 0, 0, 0, 0, &res);
-	ret = res.a0;
+	val = readl(addr);
+	val = (val & ~mask) | (data & mask);
+	writel(val, addr);
+}
+
+static inline void comphy_reg_set16(void __iomem *addr, u16 data, u16 mask)
+{
+	u16 val;
+
+	val = readw(addr);
+	val = (val & ~mask) | (data & mask);
+	writew(val, addr);
+}
+
+/* Used for accessing lane 2 registers (SATA/USB3 PHY) */
+static void comphy_set_indirect(struct mvebu_a3700_comphy_priv *priv,
+				u32 offset, u16 data, u16 mask)
+{
+	writel(offset,
+	       priv->lane2_phy_indirect + COMPHY_LANE2_INDIR_ADDR);
+	comphy_reg_set(priv->lane2_phy_indirect + COMPHY_LANE2_INDIR_DATA,
+		       data, mask);
+}
+
+static void comphy_lane_reg_set(struct mvebu_a3700_comphy_lane *lane,
+				u16 reg, u16 data, u16 mask)
+{
+	if (lane->id == 2) {
+		/* lane 2 PHY registers are accessed indirectly */
+		comphy_set_indirect(lane->priv,
+				    reg + COMPHY_LANE2_REGS_BASE,
+				    data, mask);
+	} else {
+		void __iomem *base = lane->id == 1 ?
+				     lane->priv->lane1_phy_regs :
+				     lane->priv->lane0_phy_regs;
+
+		comphy_reg_set16(base + COMPHY_LANE_REG_DIRECT(reg),
+				 data, mask);
+	}
+}
+
+static int comphy_lane_reg_poll(struct mvebu_a3700_comphy_lane *lane,
+				u16 reg, u16 bits,
+				ulong sleep_us, ulong timeout_us)
+{
+	int ret;
+
+	if (lane->id == 2) {
+		u32 data;
+
+		/* lane 2 PHY registers are accessed indirectly */
+		writel(reg + COMPHY_LANE2_REGS_BASE,
+		       lane->priv->lane2_phy_indirect +
+		       COMPHY_LANE2_INDIR_ADDR);
+
+		ret = readl_poll_timeout(lane->priv->lane2_phy_indirect +
+					 COMPHY_LANE2_INDIR_DATA,
+					 data, (data & bits) == bits,
+					 sleep_us, timeout_us);
+	} else {
+		void __iomem *base = lane->id == 1 ?
+				     lane->priv->lane1_phy_regs :
+				     lane->priv->lane0_phy_regs;
+		u16 data;
+
+		ret = readw_poll_timeout(base + COMPHY_LANE_REG_DIRECT(reg),
+					 data, (data & bits) == bits,
+					 sleep_us, timeout_us);
+	}
+
+	return ret;
+}
+
+static void comphy_periph_reg_set(struct mvebu_a3700_comphy_lane *lane,
+				  u8 reg, u32 data, u32 mask)
+{
+	comphy_reg_set(lane->priv->comphy_regs + COMPHY_PHY_REG(lane->id, reg),
+		       data, mask);
+}
+
+static int comphy_periph_reg_poll(struct mvebu_a3700_comphy_lane *lane,
+				  u8 reg, u32 bits,
+				  ulong sleep_us, ulong timeout_us)
+{
+	u32 data;
+
+	return readl_poll_timeout(lane->priv->comphy_regs +
+				  COMPHY_PHY_REG(lane->id, reg),
+				  data, (data & bits) == bits,
+				  sleep_us, timeout_us);
+}
+
+/* PHY selector configures with corresponding modes */
+static int
+mvebu_a3700_comphy_set_phy_selector(struct mvebu_a3700_comphy_lane *lane)
+{
+	u32 old, new, clr = 0, set = 0;
+	unsigned long flags;
+
+	switch (lane->mode) {
+	case PHY_MODE_SATA:
+		/* SATA must be in Lane2 */
+		if (lane->id == 2)
+			clr = COMPHY_SELECTOR_USB3_PHY_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	case PHY_MODE_ETHERNET:
+		if (lane->id == 0)
+			clr = COMPHY_SELECTOR_USB3_GBE1_SEL_BIT;
+		else if (lane->id == 1)
+			clr = COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	case PHY_MODE_USB_HOST_SS:
+		if (lane->id == 2)
+			set = COMPHY_SELECTOR_USB3_PHY_SEL_BIT;
+		else if (lane->id == 0)
+			set = COMPHY_SELECTOR_USB3_GBE1_SEL_BIT;
+		else
+			goto error;
+		break;
+
+	case PHY_MODE_PCIE:
+		/* PCIE must be in Lane1 */
+		if (lane->id == 1)
+			set = COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT;
+		else
+			goto error;
+		break;
 
-	switch (ret) {
-	case SMCCC_RET_SUCCESS:
-		return 0;
-	case SMCCC_RET_NOT_SUPPORTED:
-		return -EOPNOTSUPP;
 	default:
+		goto error;
+	}
+
+	spin_lock_irqsave(&lane->priv->lock, flags);
+
+	old = readl(lane->priv->comphy_regs + COMPHY_SELECTOR_PHY_REG);
+	new = (old & ~clr) | set;
+	writel(new, lane->priv->comphy_regs + COMPHY_SELECTOR_PHY_REG);
+
+	spin_unlock_irqrestore(&lane->priv->lock, flags);
+
+	dev_dbg(lane->dev,
+		"COMPHY[%d] mode[%d] changed PHY selector 0x%08x -> 0x%08x\n",
+		lane->id, lane->mode, old, new);
+
+	return 0;
+error:
+	dev_err(lane->dev, "COMPHY[%d] mode[%d] is invalid\n", lane->id,
+		lane->mode);
+	return -EINVAL;
+}
+
+static int
+mvebu_a3700_comphy_sata_power_on(struct mvebu_a3700_comphy_lane *lane)
+{
+	u32 mask, data, ref_clk;
+	int ret;
+
+	/* Configure phy selector for SATA */
+	ret = mvebu_a3700_comphy_set_phy_selector(lane);
+	if (ret)
+		return ret;
+
+	/* Clear phy isolation mode to make it work in normal mode */
+	comphy_lane_reg_set(lane, COMPHY_ISOLATION_CTRL,
+			    0x0, PHY_ISOLATE_MODE);
+
+	/* 0. Check the Polarity invert bits */
+	data = 0x0;
+	if (lane->invert_tx)
+		data |= TXD_INVERT_BIT;
+	if (lane->invert_rx)
+		data |= RXD_INVERT_BIT;
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	comphy_lane_reg_set(lane, COMPHY_SYNC_PATTERN, data, mask);
+
+	/* 1. Select 40-bit data width */
+	comphy_lane_reg_set(lane, COMPHY_DIG_LOOPBACK_EN,
+			    DATA_WIDTH_40BIT, SEL_DATA_WIDTH_MASK);
+
+	/* 2. Select reference clock(25M) and PHY mode (SATA) */
+	if (lane->priv->xtal_is_40m)
+		ref_clk = REF_FREF_SEL_SERDES_40MHZ;
+	else
+		ref_clk = REF_FREF_SEL_SERDES_25MHZ;
+
+	data = ref_clk | COMPHY_MODE_SATA;
+	mask = REF_FREF_SEL_MASK | COMPHY_MODE_MASK;
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL, data, mask);
+
+	/* 3. Use maximum PLL rate (no power save) */
+	comphy_lane_reg_set(lane, COMPHY_KVCO_CAL_CTRL,
+			    USE_MAX_PLL_RATE_BIT, USE_MAX_PLL_RATE_BIT);
+
+	/* 4. Reset reserved bit */
+	comphy_set_indirect(lane->priv, COMPHY_RESERVED_REG,
+			    0x0, PHYCTRL_FRM_PIN_BIT);
+
+	/* 5. Set vendor-specific configuration (It is done in sata driver) */
+	/* XXX: in U-Boot below sequence was executed in this place, in Linux
+	 * not.  Now it is done only in U-Boot before this comphy
+	 * initialization - tests shows that it works ok, but in case of any
+	 * future problem it is left for reference.
+	 *   reg_set(MVEBU_REGS_BASE + 0xe00a0, 0, 0xffffffff);
+	 *   reg_set(MVEBU_REGS_BASE + 0xe00a4, BIT(6), BIT(6));
+	 */
+
+	/* Wait for > 55 us to allow PLL be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	/* Polling status */
+	ret = comphy_lane_reg_poll(lane, COMPHY_DIG_LOOPBACK_EN,
+				   PLL_READY_TX_BIT, COMPHY_PLL_SLEEP,
+				   COMPHY_PLL_TIMEOUT);
+	if (ret)
+		dev_err(lane->dev, "Failed to lock SATA PLL\n");
+
+	return ret;
+}
+
+static void comphy_gbe_phy_init(struct mvebu_a3700_comphy_lane *lane,
+				bool is_1gbps)
+{
+	int addr, fix_idx;
+	u16 val;
+
+	fix_idx = 0;
+	for (addr = 0; addr < 512; addr++) {
+		/*
+		 * All PHY register values are defined in full for 3.125Gbps
+		 * SERDES speed. The values required for 1.25 Gbps are almost
+		 * the same and only few registers should be "fixed" in
+		 * comparison to 3.125 Gbps values. These register values are
+		 * stored in "gbe_phy_init_fix" array.
+		 */
+		if (!is_1gbps && gbe_phy_init_fix[fix_idx].addr == addr) {
+			/* Use new value */
+			val = gbe_phy_init_fix[fix_idx].value;
+			if (fix_idx < ARRAY_SIZE(gbe_phy_init_fix))
+				fix_idx++;
+		} else {
+			val = gbe_phy_init[addr];
+		}
+
+		comphy_lane_reg_set(lane, addr, val, 0xFFFF);
+	}
+}
+
+static int
+mvebu_a3700_comphy_ethernet_power_on(struct mvebu_a3700_comphy_lane *lane)
+{
+	u32 mask, data, speed_sel;
+	int ret;
+
+	/* Set selector */
+	ret = mvebu_a3700_comphy_set_phy_selector(lane);
+	if (ret)
+		return ret;
+
+	/*
+	 * 1. Reset PHY by setting PHY input port PIN_RESET=1.
+	 * 2. Set PHY input port PIN_TX_IDLE=1, PIN_PU_IVREF=1 to keep
+	 *    PHY TXP/TXN output to idle state during PHY initialization
+	 * 3. Set PHY input port PIN_PU_PLL=0, PIN_PU_RX=0, PIN_PU_TX=0.
+	 */
+	data = PIN_PU_IVREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
+	mask = data | PIN_RESET_CORE_BIT | PIN_PU_PLL_BIT | PIN_PU_RX_BIT |
+	       PIN_PU_TX_BIT | PHY_RX_INIT_BIT;
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, data, mask);
+
+	/* 4. Release reset to the PHY by setting PIN_RESET=0. */
+	data = 0x0;
+	mask = PIN_RESET_COMPHY_BIT;
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, data, mask);
+
+	/*
+	 * 5. Set PIN_PHY_GEN_TX[3:0] and PIN_PHY_GEN_RX[3:0] to decide COMPHY
+	 * bit rate
+	 */
+	switch (lane->submode) {
+	case PHY_INTERFACE_MODE_SGMII:
+	case PHY_INTERFACE_MODE_1000BASEX:
+		/* SGMII 1G, SerDes speed 1.25G */
+		speed_sel = SERDES_SPEED_1_25_G;
+		break;
+	case PHY_INTERFACE_MODE_2500BASEX:
+		/* 2500Base-X, SerDes speed 3.125G */
+		speed_sel = SERDES_SPEED_3_125_G;
+		break;
+	default:
+		/* Other rates are not supported */
+		dev_err(lane->dev,
+			"unsupported phy speed %d on comphy lane%d\n",
+			lane->submode, lane->id);
 		return -EINVAL;
 	}
+	data = GEN_RX_SEL_VALUE(speed_sel) | GEN_TX_SEL_VALUE(speed_sel);
+	mask = GEN_RX_SEL_MASK | GEN_TX_SEL_MASK;
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, data, mask);
+
+	/*
+	 * 6. Wait 10mS for bandgap and reference clocks to stabilize; then
+	 * start SW programming.
+	 */
+	mdelay(10);
+
+	/* 7. Program COMPHY register PHY_MODE */
+	data = COMPHY_MODE_SERDES;
+	mask = COMPHY_MODE_MASK;
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL, data, mask);
+
+	/*
+	 * 8. Set COMPHY register REFCLK_SEL to select the correct REFCLK
+	 * source
+	 */
+	data = 0x0;
+	mask = PHY_REF_CLK_SEL;
+	comphy_lane_reg_set(lane, COMPHY_MISC_CTRL0, data, mask);
+
+	/*
+	 * 9. Set correct reference clock frequency in COMPHY register
+	 * REF_FREF_SEL.
+	 */
+	if (lane->priv->xtal_is_40m)
+		data = REF_FREF_SEL_SERDES_50MHZ;
+	else
+		data = REF_FREF_SEL_SERDES_25MHZ;
+
+	mask = REF_FREF_SEL_MASK;
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL, data, mask);
+
+	/*
+	 * 10. Program COMPHY register PHY_GEN_MAX[1:0]
+	 * This step is mentioned in the flow received from verification team.
+	 * However the PHY_GEN_MAX value is only meaningful for other interfaces
+	 * (not SERDES). For instance, it selects SATA speed 1.5/3/6 Gbps or
+	 * PCIe speed 2.5/5 Gbps
+	 */
+
+	/*
+	 * 11. Program COMPHY register SEL_BITS to set correct parallel data
+	 * bus width
+	 */
+	data = DATA_WIDTH_10BIT;
+	mask = SEL_DATA_WIDTH_MASK;
+	comphy_lane_reg_set(lane, COMPHY_DIG_LOOPBACK_EN, data, mask);
+
+	/*
+	 * 12. As long as DFE function needs to be enabled in any mode,
+	 * COMPHY register DFE_UPDATE_EN[5:0] shall be programmed to 0x3F
+	 * for real chip during COMPHY power on.
+	 * The value of the DFE_UPDATE_EN already is 0x3F, because it is the
+	 * default value after reset of the PHY.
+	 */
+
+	/*
+	 * 13. Program COMPHY GEN registers.
+	 * These registers should be programmed based on the lab testing result
+	 * to achieve optimal performance. Please contact the CEA group to get
+	 * the related GEN table during real chip bring-up. We only required to
+	 * run though the entire registers programming flow defined by
+	 * "comphy_gbe_phy_init" when the REF clock is 40 MHz. For REF clock
+	 * 25 MHz the default values stored in PHY registers are OK.
+	 */
+	dev_dbg(lane->dev, "Running C-DPI phy init %s mode\n",
+		lane->submode == PHY_INTERFACE_MODE_2500BASEX ? "2G5" : "1G");
+	if (lane->priv->xtal_is_40m)
+		comphy_gbe_phy_init(lane,
+				    lane->submode != PHY_INTERFACE_MODE_2500BASEX);
+
+	/*
+	 * 14. Check the PHY Polarity invert bit
+	 */
+	data = 0x0;
+	if (lane->invert_tx)
+		data |= TXD_INVERT_BIT;
+	if (lane->invert_rx)
+		data |= RXD_INVERT_BIT;
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	comphy_lane_reg_set(lane, COMPHY_SYNC_PATTERN, data, mask);
+
+	/*
+	 * 15. Set PHY input ports PIN_PU_PLL, PIN_PU_TX and PIN_PU_RX to 1 to
+	 * start PHY power up sequence. All the PHY register programming should
+	 * be done before PIN_PU_PLL=1. There should be no register programming
+	 * for normal PHY operation from this point.
+	 */
+	data = PIN_PU_PLL_BIT | PIN_PU_RX_BIT | PIN_PU_TX_BIT;
+	mask = data;
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, data, mask);
+
+	/*
+	 * 16. Wait for PHY power up sequence to finish by checking output ports
+	 * PIN_PLL_READY_TX=1 and PIN_PLL_READY_RX=1.
+	 */
+	ret = comphy_periph_reg_poll(lane, COMPHY_PHY_STAT1,
+				     PHY_PLL_READY_TX_BIT |
+				     PHY_PLL_READY_RX_BIT,
+				     COMPHY_PLL_SLEEP, COMPHY_PLL_TIMEOUT);
+	if (ret) {
+		dev_err(lane->dev, "Failed to lock PLL for SERDES PHY %d\n",
+			lane->id);
+		return ret;
+	}
+
+	/*
+	 * 17. Set COMPHY input port PIN_TX_IDLE=0
+	 */
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, 0x0, PIN_TX_IDLE_BIT);
+
+	/*
+	 * 18. After valid data appear on PIN_RXDATA bus, set PIN_RX_INIT=1. To
+	 * start RX initialization. PIN_RX_INIT_DONE will be cleared to 0 by the
+	 * PHY After RX initialization is done, PIN_RX_INIT_DONE will be set to
+	 * 1 by COMPHY Set PIN_RX_INIT=0 after PIN_RX_INIT_DONE= 1. Please
+	 * refer to RX initialization part for details.
+	 */
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1,
+			      PHY_RX_INIT_BIT, PHY_RX_INIT_BIT);
+
+	ret = comphy_periph_reg_poll(lane, COMPHY_PHY_STAT1,
+				     PHY_PLL_READY_TX_BIT |
+				     PHY_PLL_READY_RX_BIT,
+				     COMPHY_PLL_SLEEP, COMPHY_PLL_TIMEOUT);
+	if (ret) {
+		dev_err(lane->dev, "Failed to lock PLL for SERDES PHY %d\n",
+			lane->id);
+		return ret;
+	}
+
+	ret = comphy_periph_reg_poll(lane, COMPHY_PHY_STAT1,
+				     PHY_RX_INIT_DONE_BIT,
+				     COMPHY_PLL_SLEEP, COMPHY_PLL_TIMEOUT);
+	if (ret)
+		dev_err(lane->dev, "Failed to init RX of SERDES PHY %d\n",
+			lane->id);
+
+	return ret;
+}
+
+static int
+mvebu_a3700_comphy_usb3_power_on(struct mvebu_a3700_comphy_lane *lane)
+{
+	u32 mask, data, cfg, ref_clk;
+	int ret;
+
+	/* Set phy seclector */
+	ret = mvebu_a3700_comphy_set_phy_selector(lane);
+	if (ret)
+		return ret;
+
+	/*
+	 * 0. Set PHY OTG Control(0x5d034), bit 4, Power up OTG module The
+	 * register belong to UTMI module, so it is set in UTMI phy driver.
+	 */
+
+	/*
+	 * 1. Set PRD_TXDEEMPH (3.5db de-emph)
+	 */
+	data = PRD_TXDEEMPH0_MASK;
+	mask = PRD_TXDEEMPH0_MASK | PRD_TXMARGIN_MASK | PRD_TXSWING_MASK |
+	       CFG_TX_ALIGN_POS_MASK;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_LANE_CFG0, data, mask);
+
+	/*
+	 * 2. Set BIT0: enable transmitter in high impedance mode
+	 *    Set BIT[3:4]: delay 2 clock cycles for HiZ off latency
+	 *    Set BIT6: Tx detect Rx at HiZ mode
+	 *    Unset BIT15: set to 0 to set USB3 De-emphasize level to -3.5db
+	 *            together with bit 0 of COMPHY_PIPE_LANE_CFG0 register
+	 */
+	data = TX_DET_RX_MODE | GEN2_TX_DATA_DLY_DEFT | TX_ELEC_IDLE_MODE_EN;
+	mask = PRD_TXDEEMPH1_MASK | TX_DET_RX_MODE | GEN2_TX_DATA_DLY_MASK |
+	       TX_ELEC_IDLE_MODE_EN;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_LANE_CFG1, data, mask);
+
+	/*
+	 * 3. Set Spread Spectrum Clock Enabled
+	 */
+	comphy_lane_reg_set(lane, COMPHY_PIPE_LANE_CFG4,
+			    SPREAD_SPECTRUM_CLK_EN, SPREAD_SPECTRUM_CLK_EN);
+
+	/*
+	 * 4. Set Override Margining Controls From the MAC:
+	 *    Use margining signals from lane configuration
+	 */
+	comphy_lane_reg_set(lane, COMPHY_PIPE_TEST_MODE_CTRL,
+			    MODE_MARGIN_OVERRIDE, 0xFFFF);
+
+	/*
+	 * 5. Set Lane-to-Lane Bundle Clock Sampling Period = per PCLK cycles
+	 *    set Mode Clock Source = PCLK is generated from REFCLK
+	 */
+	data = 0x0;
+	mask = MODE_CLK_SRC | BUNDLE_PERIOD_SEL | BUNDLE_PERIOD_SCALE_MASK |
+	       BUNDLE_SAMPLE_CTRL | PLL_READY_DLY_MASK;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_CLK_SRC_LO, data, mask);
+
+	/*
+	 * 6. Set G2 Spread Spectrum Clock Amplitude at 4K
+	 */
+	comphy_lane_reg_set(lane, COMPHY_GEN2_SET2,
+			    GS2_TX_SSC_AMP_4128, GS2_TX_SSC_AMP_MASK);
+
+	/*
+	 * 7. Unset G3 Spread Spectrum Clock Amplitude
+	 *    set G3 TX and RX Register Master Current Select
+	 */
+	data = GS2_VREG_RXTX_MAS_ISET_60U;
+	mask = GS2_TX_SSC_AMP_MASK | GS2_VREG_RXTX_MAS_ISET_MASK |
+	       GS2_RSVD_6_0_MASK;
+	comphy_lane_reg_set(lane, COMPHY_GEN3_SET2, data, mask);
+
+	/*
+	 * 8. Check crystal jumper setting and program the Power and PLL Control
+	 * accordingly Change RX wait
+	 */
+	if (lane->priv->xtal_is_40m) {
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
+		cfg = CFG_PM_RXDLOZ_WAIT_12_UNIT;
+	} else {
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
+		cfg = CFG_PM_RXDLOZ_WAIT_7_UNIT;
+	}
+
+	data = PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
+	       PU_TX_INTP_BIT | PU_DFE_BIT | COMPHY_MODE_USB3 | ref_clk;
+	mask = PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
+	       PU_TX_INTP_BIT | PU_DFE_BIT | PLL_LOCK_BIT | COMPHY_MODE_MASK |
+	       REF_FREF_SEL_MASK;
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL, data, mask);
+
+	data = CFG_PM_RXDEN_WAIT_1_UNIT | cfg;
+	mask = CFG_PM_OSCCLK_WAIT_MASK | CFG_PM_RXDEN_WAIT_MASK |
+	       CFG_PM_RXDLOZ_WAIT_MASK;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_PWR_MGM_TIM1, data, mask);
+
+	/*
+	 * 9. Enable idle sync
+	 */
+	comphy_lane_reg_set(lane, COMPHY_IDLE_SYNC_EN,
+			    IDLE_SYNC_EN, IDLE_SYNC_EN);
+
+	/*
+	 * 10. Enable the output of 500M clock
+	 */
+	comphy_lane_reg_set(lane, COMPHY_MISC_CTRL0, CLK500M_EN, CLK500M_EN);
+
+	/*
+	 * 11. Set 20-bit data width
+	 */
+	comphy_lane_reg_set(lane, COMPHY_DIG_LOOPBACK_EN,
+			    DATA_WIDTH_20BIT, 0xFFFF);
+
+	/*
+	 * 12. Override Speed_PLL value and use MAC PLL
+	 */
+	data = SPEED_PLL_VALUE_16 | USE_MAX_PLL_RATE_BIT;
+	mask = 0xFFFF;
+	comphy_lane_reg_set(lane, COMPHY_KVCO_CAL_CTRL, data, mask);
+
+	/*
+	 * 13. Check the Polarity invert bit
+	 */
+	data = 0x0;
+	if (lane->invert_tx)
+		data |= TXD_INVERT_BIT;
+	if (lane->invert_rx)
+		data |= RXD_INVERT_BIT;
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	comphy_lane_reg_set(lane, COMPHY_SYNC_PATTERN, data, mask);
+
+	/*
+	 * 14. Set max speed generation to USB3.0 5Gbps
+	 */
+	comphy_lane_reg_set(lane, COMPHY_SYNC_MASK_GEN,
+			    PHY_GEN_MAX_USB3_5G, PHY_GEN_MAX_MASK);
+
+	/*
+	 * 15. Set capacitor value for FFE gain peaking to 0xF
+	 */
+	comphy_lane_reg_set(lane, COMPHY_GEN2_SET3,
+			    GS3_FFE_CAP_SEL_VALUE, GS3_FFE_CAP_SEL_MASK);
+
+	/*
+	 * 16. Release SW reset
+	 */
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32 | MODE_REFDIV_BY_4;
+	mask = 0xFFFF;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_RST_CLK_CTRL, data, mask);
+
+	/* Wait for > 55 us to allow PCLK be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	ret = comphy_lane_reg_poll(lane, COMPHY_PIPE_LANE_STAT1, TXDCLK_PCLK_EN,
+				   COMPHY_PLL_SLEEP, COMPHY_PLL_TIMEOUT);
+	if (ret)
+		dev_err(lane->dev, "Failed to lock USB3 PLL\n");
+
+	return ret;
+}
+
+static int
+mvebu_a3700_comphy_pcie_power_on(struct mvebu_a3700_comphy_lane *lane)
+{
+	u32 mask, data, ref_clk;
+	int ret;
+
+	/* Configure phy selector for PCIe */
+	ret = mvebu_a3700_comphy_set_phy_selector(lane);
+	if (ret)
+		return ret;
+
+	/* 1. Enable max PLL. */
+	comphy_lane_reg_set(lane, COMPHY_PIPE_LANE_CFG1,
+			    USE_MAX_PLL_RATE_EN, USE_MAX_PLL_RATE_EN);
+
+	/* 2. Select 20 bit SERDES interface. */
+	comphy_lane_reg_set(lane, COMPHY_PIPE_CLK_SRC_LO,
+			    CFG_SEL_20B, CFG_SEL_20B);
+
+	/* 3. Force to use reg setting for PCIe mode */
+	comphy_lane_reg_set(lane, COMPHY_MISC_CTRL1,
+			    SEL_BITS_PCIE_FORCE, SEL_BITS_PCIE_FORCE);
+
+	/* 4. Change RX wait */
+	data = CFG_PM_RXDEN_WAIT_1_UNIT | CFG_PM_RXDLOZ_WAIT_12_UNIT;
+	mask = CFG_PM_OSCCLK_WAIT_MASK | CFG_PM_RXDEN_WAIT_MASK |
+	       CFG_PM_RXDLOZ_WAIT_MASK;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_PWR_MGM_TIM1, data, mask);
+
+	/* 5. Enable idle sync */
+	comphy_lane_reg_set(lane, COMPHY_IDLE_SYNC_EN,
+			    IDLE_SYNC_EN, IDLE_SYNC_EN);
+
+	/* 6. Enable the output of 100M/125M/500M clock */
+	data = CLK500M_EN | TXDCLK_2X_SEL | CLK100M_125M_EN;
+	mask = data;
+	comphy_lane_reg_set(lane, COMPHY_MISC_CTRL0, data, mask);
+
+	/*
+	 * 7. Enable TX, PCIE global register, 0xd0074814, it is done in
+	 * PCI-E driver
+	 */
+
+	/*
+	 * 8. Check crystal jumper setting and program the Power and PLL
+	 * Control accordingly
+	 */
+
+	if (lane->priv->xtal_is_40m)
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
+	else
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
+
+	data = PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
+	       PU_TX_INTP_BIT | PU_DFE_BIT | COMPHY_MODE_PCIE | ref_clk;
+	mask = 0xFFFF;
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL, data, mask);
+
+	/* 9. Override Speed_PLL value and use MAC PLL */
+	comphy_lane_reg_set(lane, COMPHY_KVCO_CAL_CTRL,
+			    SPEED_PLL_VALUE_16 | USE_MAX_PLL_RATE_BIT,
+			    0xFFFF);
+
+	/* 10. Check the Polarity invert bit */
+	data = 0x0;
+	if (lane->invert_tx)
+		data |= TXD_INVERT_BIT;
+	if (lane->invert_rx)
+		data |= RXD_INVERT_BIT;
+	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
+	comphy_lane_reg_set(lane, COMPHY_SYNC_PATTERN, data, mask);
+
+	/* 11. Release SW reset */
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32;
+	mask = data | PIPE_SOFT_RESET | MODE_REFDIV_MASK;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_RST_CLK_CTRL, data, mask);
+
+	/* Wait for > 55 us to allow PCLK be enabled */
+	udelay(PLL_SET_DELAY_US);
+
+	ret = comphy_lane_reg_poll(lane, COMPHY_PIPE_LANE_STAT1, TXDCLK_PCLK_EN,
+				   COMPHY_PLL_SLEEP, COMPHY_PLL_TIMEOUT);
+	if (ret)
+		dev_err(lane->dev, "Failed to lock PCIE PLL\n");
+
+	return ret;
+}
+
+static void
+mvebu_a3700_comphy_sata_power_off(struct mvebu_a3700_comphy_lane *lane)
+{
+	/* Set phy isolation mode */
+	comphy_lane_reg_set(lane, COMPHY_ISOLATION_CTRL,
+			    PHY_ISOLATE_MODE, PHY_ISOLATE_MODE);
+
+	/* Power off PLL, Tx, Rx */
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL,
+			    0x0, PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT);
+}
+
+static void
+mvebu_a3700_comphy_ethernet_power_off(struct mvebu_a3700_comphy_lane *lane)
+{
+	u32 mask, data;
+
+	data = PIN_RESET_CORE_BIT | PIN_RESET_COMPHY_BIT | PIN_PU_IVREF_BIT |
+	       PHY_RX_INIT_BIT;
+	mask = data;
+	comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, data, mask);
+}
+
+static void
+mvebu_a3700_comphy_pcie_power_off(struct mvebu_a3700_comphy_lane *lane)
+{
+	/* Power off PLL, Tx, Rx */
+	comphy_lane_reg_set(lane, COMPHY_POWER_PLL_CTRL,
+			    0x0, PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT);
+}
+
+static int mvebu_a3700_comphy_reset(struct phy *phy)
+{
+	struct mvebu_a3700_comphy_lane *lane = phy_get_drvdata(phy);
+	u16 mask, data;
+
+	dev_dbg(lane->dev, "resetting lane %d\n", lane->id);
+
+	/* COMPHY reset for internal logic */
+	comphy_lane_reg_set(lane, COMPHY_SFT_RESET,
+			    SFT_RST_NO_REG, SFT_RST_NO_REG);
+
+	/* COMPHY register reset (cleared automatically) */
+	comphy_lane_reg_set(lane, COMPHY_SFT_RESET, SFT_RST, SFT_RST);
+
+	/* PIPE soft and register reset */
+	data = PIPE_SOFT_RESET | PIPE_REG_RESET;
+	mask = data;
+	comphy_lane_reg_set(lane, COMPHY_PIPE_RST_CLK_CTRL, data, mask);
+
+	/* Release PIPE register reset */
+	comphy_lane_reg_set(lane, COMPHY_PIPE_RST_CLK_CTRL,
+			    0x0, PIPE_REG_RESET);
+
+	/* Reset SB configuration register (only for lanes 0 and 1) */
+	if (lane->id == 0 || lane->id == 1) {
+		u32 mask, data;
+
+		data = PIN_RESET_CORE_BIT | PIN_RESET_COMPHY_BIT |
+		       PIN_PU_PLL_BIT | PIN_PU_RX_BIT | PIN_PU_TX_BIT;
+		mask = data | PIN_PU_IVREF_BIT | PIN_TX_IDLE_BIT;
+		comphy_periph_reg_set(lane, COMPHY_PHY_CFG1, data, mask);
+	}
+
+	return 0;
 }
 
-static int mvebu_a3700_comphy_get_fw_mode(int lane, int port,
+static bool mvebu_a3700_comphy_check_mode(int lane,
 					  enum phy_mode mode,
 					  int submode)
 {
@@ -128,38 +1141,40 @@ static int mvebu_a3700_comphy_get_fw_mode(int lane, int port,
 
 	/* Unused PHY mux value is 0x0 */
 	if (mode == PHY_MODE_INVALID)
-		return -EINVAL;
+		return false;
 
 	for (i = 0; i < n; i++) {
 		if (mvebu_a3700_comphy_modes[i].lane == lane &&
-		    mvebu_a3700_comphy_modes[i].port == port &&
 		    mvebu_a3700_comphy_modes[i].mode == mode &&
 		    mvebu_a3700_comphy_modes[i].submode == submode)
 			break;
 	}
 
 	if (i == n)
-		return -EINVAL;
+		return false;
 
-	return mvebu_a3700_comphy_modes[i].fw_mode;
+	return true;
 }
 
 static int mvebu_a3700_comphy_set_mode(struct phy *phy, enum phy_mode mode,
 				       int submode)
 {
 	struct mvebu_a3700_comphy_lane *lane = phy_get_drvdata(phy);
-	int fw_mode;
-
-	if (submode == PHY_INTERFACE_MODE_1000BASEX)
-		submode = PHY_INTERFACE_MODE_SGMII;
 
-	fw_mode = mvebu_a3700_comphy_get_fw_mode(lane->id, lane->port, mode,
-						 submode);
-	if (fw_mode < 0) {
+	if (!mvebu_a3700_comphy_check_mode(lane->id, mode, submode)) {
 		dev_err(lane->dev, "invalid COMPHY mode\n");
-		return fw_mode;
+		return -EINVAL;
 	}
 
+	/* Mode cannot be changed while the PHY is powered on */
+	if (phy->power_count &&
+	    (lane->mode != mode || lane->submode != submode))
+		return -EBUSY;
+
+	/* If changing mode, ensure reset is called */
+	if (lane->mode != PHY_MODE_INVALID && lane->mode != mode)
+		lane->needs_reset = true;
+
 	/* Just remember the mode, ->power_on() will do the real setup */
 	lane->mode = mode;
 	lane->submode = submode;
@@ -170,75 +1185,77 @@ static int mvebu_a3700_comphy_set_mode(struct phy *phy, enum phy_mode mode,
 static int mvebu_a3700_comphy_power_on(struct phy *phy)
 {
 	struct mvebu_a3700_comphy_lane *lane = phy_get_drvdata(phy);
-	u32 fw_param;
-	int fw_mode;
 	int ret;
 
-	fw_mode = mvebu_a3700_comphy_get_fw_mode(lane->id, lane->port,
-						 lane->mode, lane->submode);
-	if (fw_mode < 0) {
+	if (!mvebu_a3700_comphy_check_mode(lane->id, lane->mode,
+					   lane->submode)) {
 		dev_err(lane->dev, "invalid COMPHY mode\n");
-		return fw_mode;
+		return -EINVAL;
+	}
+
+	if (lane->needs_reset) {
+		ret = mvebu_a3700_comphy_reset(phy);
+		if (ret)
+			return ret;
+
+		lane->needs_reset = false;
 	}
 
 	switch (lane->mode) {
 	case PHY_MODE_USB_HOST_SS:
 		dev_dbg(lane->dev, "set lane %d to USB3 host mode\n", lane->id);
-		fw_param = COMPHY_FW_MODE(fw_mode);
-		break;
+		return mvebu_a3700_comphy_usb3_power_on(lane);
 	case PHY_MODE_SATA:
 		dev_dbg(lane->dev, "set lane %d to SATA mode\n", lane->id);
-		fw_param = COMPHY_FW_MODE(fw_mode);
-		break;
+		return mvebu_a3700_comphy_sata_power_on(lane);
 	case PHY_MODE_ETHERNET:
-		switch (lane->submode) {
-		case PHY_INTERFACE_MODE_SGMII:
-			dev_dbg(lane->dev, "set lane %d to SGMII mode\n",
-				lane->id);
-			fw_param = COMPHY_FW_NET(fw_mode, lane->port,
-						 COMPHY_FW_SPEED_1_25G);
-			break;
-		case PHY_INTERFACE_MODE_2500BASEX:
-			dev_dbg(lane->dev, "set lane %d to 2500BASEX mode\n",
-				lane->id);
-			fw_param = COMPHY_FW_NET(fw_mode, lane->port,
-						 COMPHY_FW_SPEED_3_125G);
-			break;
-		default:
-			dev_err(lane->dev, "unsupported PHY submode (%d)\n",
-				lane->submode);
-			return -ENOTSUPP;
-		}
-		break;
+		dev_dbg(lane->dev, "set lane %d to Ethernet mode\n", lane->id);
+		return mvebu_a3700_comphy_ethernet_power_on(lane);
 	case PHY_MODE_PCIE:
 		dev_dbg(lane->dev, "set lane %d to PCIe mode\n", lane->id);
-		fw_param = COMPHY_FW_PCIE(fw_mode, lane->port,
-					  COMPHY_FW_SPEED_5G,
-					  phy->attrs.bus_width);
-		break;
+		return mvebu_a3700_comphy_pcie_power_on(lane);
 	default:
 		dev_err(lane->dev, "unsupported PHY mode (%d)\n", lane->mode);
-		return -ENOTSUPP;
+		return -EOPNOTSUPP;
 	}
-
-	ret = mvebu_a3700_comphy_smc(COMPHY_SIP_POWER_ON, lane->id, fw_param);
-	if (ret == -EOPNOTSUPP)
-		dev_err(lane->dev,
-			"unsupported SMC call, try updating your firmware\n");
-
-	return ret;
 }
 
 static int mvebu_a3700_comphy_power_off(struct phy *phy)
 {
 	struct mvebu_a3700_comphy_lane *lane = phy_get_drvdata(phy);
 
-	return mvebu_a3700_comphy_smc(COMPHY_SIP_POWER_OFF, lane->id, 0);
+	switch (lane->mode) {
+	case PHY_MODE_USB_HOST_SS:
+		/*
+		 * The USB3 MAC sets the USB3 PHY to low state, so we do not
+		 * need to power off USB3 PHY again.
+		 */
+		break;
+
+	case PHY_MODE_SATA:
+		mvebu_a3700_comphy_sata_power_off(lane);
+		break;
+
+	case PHY_MODE_ETHERNET:
+		mvebu_a3700_comphy_ethernet_power_off(lane);
+		break;
+
+	case PHY_MODE_PCIE:
+		mvebu_a3700_comphy_pcie_power_off(lane);
+		break;
+
+	default:
+		dev_err(lane->dev, "invalid COMPHY mode\n");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 static const struct phy_ops mvebu_a3700_comphy_ops = {
 	.power_on	= mvebu_a3700_comphy_power_on,
 	.power_off	= mvebu_a3700_comphy_power_off,
+	.reset		= mvebu_a3700_comphy_reset,
 	.set_mode	= mvebu_a3700_comphy_set_mode,
 	.owner		= THIS_MODULE,
 };
@@ -247,25 +1264,90 @@ static struct phy *mvebu_a3700_comphy_xlate(struct device *dev,
 					    struct of_phandle_args *args)
 {
 	struct mvebu_a3700_comphy_lane *lane;
+	unsigned int port;
 	struct phy *phy;
 
-	if (WARN_ON(args->args[0] >= MVEBU_A3700_COMPHY_PORTS))
-		return ERR_PTR(-EINVAL);
-
 	phy = of_phy_simple_xlate(dev, args);
 	if (IS_ERR(phy))
 		return phy;
 
 	lane = phy_get_drvdata(phy);
-	lane->port = args->args[0];
+
+	port = args->args[0];
+	if (port != 0 && (port != 1 || lane->id != 0)) {
+		dev_err(lane->dev, "invalid port number %u\n", port);
+		return ERR_PTR(-EINVAL);
+	}
+
+	lane->invert_tx = args->args[1] & BIT(0);
+	lane->invert_rx = args->args[1] & BIT(1);
 
 	return phy;
 }
 
 static int mvebu_a3700_comphy_probe(struct platform_device *pdev)
 {
+	struct mvebu_a3700_comphy_priv *priv;
 	struct phy_provider *provider;
 	struct device_node *child;
+	struct resource *res;
+	struct clk *clk;
+	int ret;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	spin_lock_init(&priv->lock);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "comphy");
+	priv->comphy_regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->comphy_regs))
+		return PTR_ERR(priv->comphy_regs);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "lane1_pcie_gbe");
+	priv->lane1_phy_regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->lane1_phy_regs))
+		return PTR_ERR(priv->lane1_phy_regs);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "lane0_usb3_gbe");
+	priv->lane0_phy_regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->lane0_phy_regs))
+		return PTR_ERR(priv->lane0_phy_regs);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "lane2_sata_usb3");
+	priv->lane2_phy_indirect = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->lane2_phy_indirect))
+		return PTR_ERR(priv->lane2_phy_indirect);
+
+	/*
+	 * Driver needs to know if reference xtal clock is 40MHz or 25MHz.
+	 * Old DT bindings do not have xtal clk present. So do not fail here
+	 * and expects that default 25MHz reference clock is used.
+	 */
+	clk = clk_get(&pdev->dev, "xtal");
+	if (IS_ERR(clk)) {
+		if (PTR_ERR(clk) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		dev_warn(&pdev->dev, "missing 'xtal' clk (%ld)\n",
+			 PTR_ERR(clk));
+	} else {
+		ret = clk_prepare_enable(clk);
+		if (ret) {
+			dev_warn(&pdev->dev, "enabling xtal clk failed (%d)\n",
+				 ret);
+		} else {
+			if (clk_get_rate(clk) == 40000000)
+				priv->xtal_is_40m = true;
+			clk_disable_unprepare(clk);
+		}
+		clk_put(clk);
+	}
+
+	dev_set_drvdata(&pdev->dev, priv);
 
 	for_each_available_child_of_node(pdev->dev.of_node, child) {
 		struct mvebu_a3700_comphy_lane *lane;
@@ -280,7 +1362,7 @@ static int mvebu_a3700_comphy_probe(struct platform_device *pdev)
 			continue;
 		}
 
-		if (lane_id >= MVEBU_A3700_COMPHY_LANES) {
+		if (lane_id >= 3) {
 			dev_err(&pdev->dev, "invalid 'reg' property\n");
 			continue;
 		}
@@ -298,16 +1380,26 @@ static int mvebu_a3700_comphy_probe(struct platform_device *pdev)
 			return PTR_ERR(phy);
 		}
 
+		lane->priv = priv;
 		lane->dev = &pdev->dev;
 		lane->mode = PHY_MODE_INVALID;
 		lane->submode = PHY_INTERFACE_MODE_NA;
 		lane->id = lane_id;
-		lane->port = -1;
+		lane->invert_tx = false;
+		lane->invert_rx = false;
 		phy_set_drvdata(phy, lane);
+
+		/*
+		 * To avoid relying on the bootloader/firmware configuration,
+		 * power off all comphys.
+		 */
+		mvebu_a3700_comphy_reset(phy);
+		lane->needs_reset = false;
 	}
 
 	provider = devm_of_phy_provider_register(&pdev->dev,
 						 mvebu_a3700_comphy_xlate);
+
 	return PTR_ERR_OR_ZERO(provider);
 }
 
@@ -327,5 +1419,7 @@ static struct platform_driver mvebu_a3700_comphy_driver = {
 module_platform_driver(mvebu_a3700_comphy_driver);
 
 MODULE_AUTHOR("Miquèl Raynal <miquel.raynal@bootlin.com>");
+MODULE_AUTHOR("Pali Rohár <pali@kernel.org>");
+MODULE_AUTHOR("Marek Behún <kabel@kernel.org>");
 MODULE_DESCRIPTION("Common PHY driver for A3700");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-core-mipi-dphy.c b/drivers/phy/phy-core-mipi-dphy.c
index ccb4045685cd..929e86d6558e 100644
--- a/drivers/phy/phy-core-mipi-dphy.c
+++ b/drivers/phy/phy-core-mipi-dphy.c
@@ -64,10 +64,10 @@ int phy_mipi_dphy_get_default_config(unsigned long pixel_clock,
 	cfg->hs_trail = max(4 * 8 * ui, 60000 + 4 * 4 * ui);
 
 	cfg->init = 100;
-	cfg->lpx = 60000;
+	cfg->lpx = 50000;
 	cfg->ta_get = 5 * cfg->lpx;
 	cfg->ta_go = 4 * cfg->lpx;
-	cfg->ta_sure = 2 * cfg->lpx;
+	cfg->ta_sure = cfg->lpx;
 	cfg->wakeup = 1000;
 
 	cfg->hs_clk_rate = hs_clk_rate;
diff --git a/drivers/phy/qualcomm/phy-qcom-edp.c b/drivers/phy/qualcomm/phy-qcom-edp.c
index a8ecd2e8442d..cacd32f6e0cc 100644
--- a/drivers/phy/qualcomm/phy-qcom-edp.c
+++ b/drivers/phy/qualcomm/phy-qcom-edp.c
@@ -335,9 +335,11 @@ static int qcom_edp_phy_power_on(struct phy *phy)
 	writel(0x00, edp->tx0 + TXn_LANE_MODE_1);
 	writel(0x00, edp->tx1 + TXn_LANE_MODE_1);
 
-	ret = qcom_edp_configure_ssc(edp);
-	if (ret)
-		return ret;
+	if (edp->dp_opts.ssc) {
+		ret = qcom_edp_configure_ssc(edp);
+		if (ret)
+			return ret;
+	}
 
 	ret = qcom_edp_configure_pll(edp);
 	if (ret)
@@ -654,6 +656,7 @@ static int qcom_edp_phy_probe(struct platform_device *pdev)
 }
 
 static const struct of_device_id qcom_edp_phy_match_table[] = {
+	{ .compatible = "qcom,sc7280-edp-phy" },
 	{ .compatible = "qcom,sc8180x-edp-phy" },
 	{ }
 };
diff --git a/drivers/phy/qualcomm/phy-qcom-ipq806x-usb.c b/drivers/phy/qualcomm/phy-qcom-ipq806x-usb.c
index fec1da470d26..7bacc527fbad 100644
--- a/drivers/phy/qualcomm/phy-qcom-ipq806x-usb.c
+++ b/drivers/phy/qualcomm/phy-qcom-ipq806x-usb.c
@@ -10,6 +10,7 @@
 #include <linux/delay.h>
 #include <linux/regmap.h>
 #include <linux/mfd/syscon.h>
+#include <linux/bitfield.h>
 
 /* USB QSCRATCH Hardware registers */
 #define QSCRATCH_GENERAL_CFG		(0x08)
@@ -74,20 +75,20 @@
 		 PHY_PARAM_CTRL1_LOS_BIAS_MASK)
 
 #define PHY_PARAM_CTRL1_TX_FULL_SWING(x)	\
-		(((x) << 20) & PHY_PARAM_CTRL1_TX_FULL_SWING_MASK)
+		FIELD_PREP(PHY_PARAM_CTRL1_TX_FULL_SWING_MASK, (x))
 #define PHY_PARAM_CTRL1_TX_DEEMPH_6DB(x)	\
-		(((x) << 14) & PHY_PARAM_CTRL1_TX_DEEMPH_6DB_MASK)
+		FIELD_PREP(PHY_PARAM_CTRL1_TX_DEEMPH_6DB_MASK, (x))
 #define PHY_PARAM_CTRL1_TX_DEEMPH_3_5DB(x)	\
-		(((x) <<  8) & PHY_PARAM_CTRL1_TX_DEEMPH_3_5DB_MASK)
+		FIELD_PREP(PHY_PARAM_CTRL1_TX_DEEMPH_3_5DB_MASK, x)
 #define PHY_PARAM_CTRL1_LOS_BIAS(x)	\
-		(((x) <<  3) & PHY_PARAM_CTRL1_LOS_BIAS_MASK)
+		FIELD_PREP(PHY_PARAM_CTRL1_LOS_BIAS_MASK, (x))
 
 /* RX OVRD IN HI bits */
 #define RX_OVRD_IN_HI_RX_RESET_OVRD		BIT(13)
 #define RX_OVRD_IN_HI_RX_RX_RESET		BIT(12)
 #define RX_OVRD_IN_HI_RX_EQ_OVRD		BIT(11)
 #define RX_OVRD_IN_HI_RX_EQ_MASK		GENMASK(10, 7)
-#define RX_OVRD_IN_HI_RX_EQ(x)			((x) << 8)
+#define RX_OVRD_IN_HI_RX_EQ(x)			FIELD_PREP(RX_OVRD_IN_HI_RX_EQ_MASK, (x))
 #define RX_OVRD_IN_HI_RX_EQ_EN_OVRD		BIT(7)
 #define RX_OVRD_IN_HI_RX_EQ_EN			BIT(6)
 #define RX_OVRD_IN_HI_RX_LOS_FILTER_OVRD	BIT(5)
@@ -111,6 +112,9 @@
 #define SS_CR_READ_REG				BIT(0)
 #define SS_CR_WRITE_REG				BIT(0)
 
+#define LATCH_SLEEP				40
+#define LATCH_TIMEOUT				100
+
 struct usb_phy {
 	void __iomem		*base;
 	struct device		*dev;
@@ -156,19 +160,9 @@ static inline void usb_phy_write_readback(struct usb_phy *phy_dwc3,
 
 static int wait_for_latch(void __iomem *addr)
 {
-	u32 retry = 10;
-
-	while (true) {
-		if (!readl(addr))
-			break;
-
-		if (--retry == 0)
-			return -ETIMEDOUT;
-
-		usleep_range(10, 20);
-	}
+	u32 val;
 
-	return 0;
+	return readl_poll_timeout(addr, val, !val, LATCH_SLEEP, LATCH_TIMEOUT);
 }
 
 /**
diff --git a/drivers/phy/qualcomm/phy-qcom-qmp.c b/drivers/phy/qualcomm/phy-qcom-qmp.c
index 8ea87c69f463..b144ae1f729a 100644
--- a/drivers/phy/qualcomm/phy-qcom-qmp.c
+++ b/drivers/phy/qualcomm/phy-qcom-qmp.c
@@ -5978,6 +5978,9 @@ static const struct of_device_id qcom_qmp_phy_of_match_table[] = {
 		.compatible = "qcom,sc8180x-qmp-ufs-phy",
 		.data = &sm8150_ufsphy_cfg,
 	}, {
+		.compatible = "qcom,sc8280xp-qmp-ufs-phy",
+		.data = &sm8350_ufsphy_cfg,
+	}, {
 		.compatible = "qcom,sc8180x-qmp-usb3-phy",
 		.data = &sm8150_usb3phy_cfg,
 	}, {
diff --git a/drivers/phy/qualcomm/phy-qcom-qusb2.c b/drivers/phy/qualcomm/phy-qcom-qusb2.c
index 032d02bf50c5..7529a7e6e5df 100644
--- a/drivers/phy/qualcomm/phy-qcom-qusb2.c
+++ b/drivers/phy/qualcomm/phy-qcom-qusb2.c
@@ -912,6 +912,9 @@ static const struct of_device_id qusb2_phy_of_match_table[] = {
 		.compatible	= "qcom,ipq8074-qusb2-phy",
 		.data		= &msm8996_phy_cfg,
 	}, {
+		.compatible	= "qcom,msm8953-qusb2-phy",
+		.data		= &msm8996_phy_cfg,
+	}, {
 		.compatible	= "qcom,msm8996-qusb2-phy",
 		.data		= &msm8996_phy_cfg,
 	}, {
diff --git a/drivers/phy/qualcomm/phy-qcom-snps-femto-v2.c b/drivers/phy/qualcomm/phy-qcom-snps-femto-v2.c
index 7e61202aa234..5d203784f75d 100644
--- a/drivers/phy/qualcomm/phy-qcom-snps-femto-v2.c
+++ b/drivers/phy/qualcomm/phy-qcom-snps-femto-v2.c
@@ -32,6 +32,7 @@
 #define POR					BIT(1)
 
 #define USB2_PHY_USB_PHY_HS_PHY_CTRL_COMMON0	(0x54)
+#define SIDDQ					BIT(2)
 #define RETENABLEN				BIT(3)
 #define FSEL_MASK				GENMASK(6, 4)
 #define FSEL_DEFAULT				(0x3 << 4)
@@ -233,6 +234,9 @@ static int qcom_snps_hsphy_init(struct phy *phy)
 	qcom_snps_hsphy_write_mask(hsphy->base, USB2_PHY_USB_PHY_UTMI_CTRL0,
 					SLEEPM, SLEEPM);
 
+	qcom_snps_hsphy_write_mask(hsphy->base, USB2_PHY_USB_PHY_HS_PHY_CTRL_COMMON0,
+				   SIDDQ, 0);
+
 	qcom_snps_hsphy_write_mask(hsphy->base, USB2_PHY_USB_PHY_UTMI_CTRL5,
 					POR, 0);
 
@@ -275,6 +279,7 @@ static const struct phy_ops qcom_snps_hsphy_gen_ops = {
 
 static const struct of_device_id qcom_snps_hsphy_of_match_table[] = {
 	{ .compatible	= "qcom,sm8150-usb-hs-phy", },
+	{ .compatible	= "qcom,usb-snps-hs-5nm-phy", },
 	{ .compatible	= "qcom,usb-snps-hs-7nm-phy", },
 	{ .compatible	= "qcom,usb-snps-femto-v2-phy",	},
 	{ }
diff --git a/drivers/phy/rockchip/Kconfig b/drivers/phy/rockchip/Kconfig
index e812adad7242..9022e395c056 100644
--- a/drivers/phy/rockchip/Kconfig
+++ b/drivers/phy/rockchip/Kconfig
@@ -66,6 +66,14 @@ config PHY_ROCKCHIP_INNO_DSIDPHY
 	  Enable this to support the Rockchip MIPI/LVDS/TTL PHY with
 	  Innosilicon IP block.
 
+config PHY_ROCKCHIP_NANENG_COMBO_PHY
+	tristate "Rockchip NANENG COMBO PHY Driver"
+	depends on ARCH_ROCKCHIP && OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the Rockchip PCIe/USB3.0/SATA/QSGMII
+	  combo PHY with NaNeng IP block.
+
 config PHY_ROCKCHIP_PCIE
 	tristate "Rockchip PCIe PHY Driver"
 	depends on (ARCH_ROCKCHIP && OF) || COMPILE_TEST
diff --git a/drivers/phy/rockchip/Makefile b/drivers/phy/rockchip/Makefile
index f0eec212b2aa..a5041efb5b8f 100644
--- a/drivers/phy/rockchip/Makefile
+++ b/drivers/phy/rockchip/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_PHY_ROCKCHIP_INNO_CSIDPHY)	+= phy-rockchip-inno-csidphy.o
 obj-$(CONFIG_PHY_ROCKCHIP_INNO_DSIDPHY)	+= phy-rockchip-inno-dsidphy.o
 obj-$(CONFIG_PHY_ROCKCHIP_INNO_HDMI)	+= phy-rockchip-inno-hdmi.o
 obj-$(CONFIG_PHY_ROCKCHIP_INNO_USB2)	+= phy-rockchip-inno-usb2.o
+obj-$(CONFIG_PHY_ROCKCHIP_NANENG_COMBO_PHY)	+= phy-rockchip-naneng-combphy.o
 obj-$(CONFIG_PHY_ROCKCHIP_PCIE)		+= phy-rockchip-pcie.o
 obj-$(CONFIG_PHY_ROCKCHIP_TYPEC)	+= phy-rockchip-typec.o
 obj-$(CONFIG_PHY_ROCKCHIP_USB)		+= phy-rockchip-usb.o
diff --git a/drivers/phy/rockchip/phy-rockchip-naneng-combphy.c b/drivers/phy/rockchip/phy-rockchip-naneng-combphy.c
new file mode 100644
index 000000000000..7b213825fb5d
--- /dev/null
+++ b/drivers/phy/rockchip/phy-rockchip-naneng-combphy.c
@@ -0,0 +1,581 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Rockchip PIPE USB3.0 PCIE SATA Combo Phy driver
+ *
+ * Copyright (C) 2021 Rockchip Electronics Co., Ltd.
+ */
+
+#include <dt-bindings/phy/phy.h>
+#include <linux/clk.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_device.h>
+#include <linux/phy/phy.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/units.h>
+
+#define BIT_WRITEABLE_SHIFT		16
+#define REF_CLOCK_24MHz			(24 * HZ_PER_MHZ)
+#define REF_CLOCK_25MHz			(25 * HZ_PER_MHZ)
+#define REF_CLOCK_100MHz		(100 * HZ_PER_MHZ)
+
+/* COMBO PHY REG */
+#define PHYREG6				0x14
+#define PHYREG6_PLL_DIV_MASK		GENMASK(7, 6)
+#define PHYREG6_PLL_DIV_SHIFT		6
+#define PHYREG6_PLL_DIV_2		1
+
+#define PHYREG7				0x18
+#define PHYREG7_TX_RTERM_MASK		GENMASK(7, 4)
+#define PHYREG7_TX_RTERM_SHIFT		4
+#define PHYREG7_TX_RTERM_50OHM		8
+#define PHYREG7_RX_RTERM_MASK		GENMASK(3, 0)
+#define PHYREG7_RX_RTERM_SHIFT		0
+#define PHYREG7_RX_RTERM_44OHM		15
+
+#define PHYREG8				0x1C
+#define PHYREG8_SSC_EN			BIT(4)
+
+#define PHYREG11			0x28
+#define PHYREG11_SU_TRIM_0_7		0xF0
+
+#define PHYREG12			0x2C
+#define PHYREG12_PLL_LPF_ADJ_VALUE	4
+
+#define PHYREG13			0x30
+#define PHYREG13_RESISTER_MASK		GENMASK(5, 4)
+#define PHYREG13_RESISTER_SHIFT		0x4
+#define PHYREG13_RESISTER_HIGH_Z	3
+#define PHYREG13_CKRCV_AMP0		BIT(7)
+
+#define PHYREG14			0x34
+#define PHYREG14_CKRCV_AMP1		BIT(0)
+
+#define PHYREG15			0x38
+#define PHYREG15_CTLE_EN		BIT(0)
+#define PHYREG15_SSC_CNT_MASK		GENMASK(7, 6)
+#define PHYREG15_SSC_CNT_SHIFT		6
+#define PHYREG15_SSC_CNT_VALUE		1
+
+#define PHYREG16			0x3C
+#define PHYREG16_SSC_CNT_VALUE		0x5f
+
+#define PHYREG18			0x44
+#define PHYREG18_PLL_LOOP		0x32
+
+#define PHYREG32			0x7C
+#define PHYREG32_SSC_MASK		GENMASK(7, 4)
+#define PHYREG32_SSC_DIR_SHIFT		4
+#define PHYREG32_SSC_UPWARD		0
+#define PHYREG32_SSC_DOWNWARD		1
+#define PHYREG32_SSC_OFFSET_SHIFT	6
+#define PHYREG32_SSC_OFFSET_500PPM	1
+
+#define PHYREG33			0x80
+#define PHYREG33_PLL_KVCO_MASK		GENMASK(4, 2)
+#define PHYREG33_PLL_KVCO_SHIFT		2
+#define PHYREG33_PLL_KVCO_VALUE		2
+
+struct rockchip_combphy_priv;
+
+struct combphy_reg {
+	u16 offset;
+	u16 bitend;
+	u16 bitstart;
+	u16 disable;
+	u16 enable;
+};
+
+struct rockchip_combphy_grfcfg {
+	struct combphy_reg pcie_mode_set;
+	struct combphy_reg usb_mode_set;
+	struct combphy_reg sgmii_mode_set;
+	struct combphy_reg qsgmii_mode_set;
+	struct combphy_reg pipe_rxterm_set;
+	struct combphy_reg pipe_txelec_set;
+	struct combphy_reg pipe_txcomp_set;
+	struct combphy_reg pipe_clk_25m;
+	struct combphy_reg pipe_clk_100m;
+	struct combphy_reg pipe_phymode_sel;
+	struct combphy_reg pipe_rate_sel;
+	struct combphy_reg pipe_rxterm_sel;
+	struct combphy_reg pipe_txelec_sel;
+	struct combphy_reg pipe_txcomp_sel;
+	struct combphy_reg pipe_clk_ext;
+	struct combphy_reg pipe_sel_usb;
+	struct combphy_reg pipe_sel_qsgmii;
+	struct combphy_reg pipe_phy_status;
+	struct combphy_reg con0_for_pcie;
+	struct combphy_reg con1_for_pcie;
+	struct combphy_reg con2_for_pcie;
+	struct combphy_reg con3_for_pcie;
+	struct combphy_reg con0_for_sata;
+	struct combphy_reg con1_for_sata;
+	struct combphy_reg con2_for_sata;
+	struct combphy_reg con3_for_sata;
+	struct combphy_reg pipe_con0_for_sata;
+	struct combphy_reg pipe_xpcs_phy_ready;
+};
+
+struct rockchip_combphy_cfg {
+	const struct rockchip_combphy_grfcfg *grfcfg;
+	int (*combphy_cfg)(struct rockchip_combphy_priv *priv);
+};
+
+struct rockchip_combphy_priv {
+	u8 type;
+	void __iomem *mmio;
+	int num_clks;
+	struct clk_bulk_data *clks;
+	struct device *dev;
+	struct regmap *pipe_grf;
+	struct regmap *phy_grf;
+	struct phy *phy;
+	struct reset_control *phy_rst;
+	const struct rockchip_combphy_cfg *cfg;
+	bool enable_ssc;
+	bool ext_refclk;
+	struct clk *refclk;
+};
+
+static void rockchip_combphy_updatel(struct rockchip_combphy_priv *priv,
+				     int mask, int val, int reg)
+{
+	unsigned int temp;
+
+	temp = readl(priv->mmio + reg);
+	temp = (temp & ~(mask)) | val;
+	writel(temp, priv->mmio + reg);
+}
+
+static int rockchip_combphy_param_write(struct regmap *base,
+					const struct combphy_reg *reg, bool en)
+{
+	u32 val, mask, tmp;
+
+	tmp = en ? reg->enable : reg->disable;
+	mask = GENMASK(reg->bitend, reg->bitstart);
+	val = (tmp << reg->bitstart) | (mask << BIT_WRITEABLE_SHIFT);
+
+	return regmap_write(base, reg->offset, val);
+}
+
+static u32 rockchip_combphy_is_ready(struct rockchip_combphy_priv *priv)
+{
+	const struct rockchip_combphy_grfcfg *cfg = priv->cfg->grfcfg;
+	u32 mask, val;
+
+	mask = GENMASK(cfg->pipe_phy_status.bitend,
+		       cfg->pipe_phy_status.bitstart);
+
+	regmap_read(priv->phy_grf, cfg->pipe_phy_status.offset, &val);
+	val = (val & mask) >> cfg->pipe_phy_status.bitstart;
+
+	return val;
+}
+
+static int rockchip_combphy_init(struct phy *phy)
+{
+	struct rockchip_combphy_priv *priv = phy_get_drvdata(phy);
+	const struct rockchip_combphy_grfcfg *cfg = priv->cfg->grfcfg;
+	u32 val;
+	int ret;
+
+	ret = clk_bulk_prepare_enable(priv->num_clks, priv->clks);
+	if (ret) {
+		dev_err(priv->dev, "failed to enable clks\n");
+		return ret;
+	}
+
+	switch (priv->type) {
+	case PHY_TYPE_PCIE:
+	case PHY_TYPE_USB3:
+	case PHY_TYPE_SATA:
+	case PHY_TYPE_SGMII:
+	case PHY_TYPE_QSGMII:
+		if (priv->cfg->combphy_cfg)
+			ret = priv->cfg->combphy_cfg(priv);
+		break;
+	default:
+		dev_err(priv->dev, "incompatible PHY type\n");
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret) {
+		dev_err(priv->dev, "failed to init phy for phy type %x\n", priv->type);
+		goto err_clk;
+	}
+
+	ret = reset_control_deassert(priv->phy_rst);
+	if (ret)
+		goto err_clk;
+
+	if (priv->type == PHY_TYPE_USB3) {
+		ret = readx_poll_timeout_atomic(rockchip_combphy_is_ready,
+						priv, val,
+						val == cfg->pipe_phy_status.enable,
+						10, 1000);
+		if (ret)
+			dev_warn(priv->dev, "wait phy status ready timeout\n");
+	}
+
+	return 0;
+
+err_clk:
+	clk_bulk_disable_unprepare(priv->num_clks, priv->clks);
+
+	return ret;
+}
+
+static int rockchip_combphy_exit(struct phy *phy)
+{
+	struct rockchip_combphy_priv *priv = phy_get_drvdata(phy);
+
+	clk_bulk_disable_unprepare(priv->num_clks, priv->clks);
+	reset_control_assert(priv->phy_rst);
+
+	return 0;
+}
+
+static const struct phy_ops rochchip_combphy_ops = {
+	.init = rockchip_combphy_init,
+	.exit = rockchip_combphy_exit,
+	.owner = THIS_MODULE,
+};
+
+static struct phy *rockchip_combphy_xlate(struct device *dev, struct of_phandle_args *args)
+{
+	struct rockchip_combphy_priv *priv = dev_get_drvdata(dev);
+
+	if (args->args_count != 1) {
+		dev_err(dev, "invalid number of arguments\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (priv->type != PHY_NONE && priv->type != args->args[0])
+		dev_warn(dev, "phy type select %d overwriting type %d\n",
+			 args->args[0], priv->type);
+
+	priv->type = args->args[0];
+
+	return priv->phy;
+}
+
+static int rockchip_combphy_parse_dt(struct device *dev, struct rockchip_combphy_priv *priv)
+{
+	int i;
+
+	priv->num_clks = devm_clk_bulk_get_all(dev, &priv->clks);
+	if (priv->num_clks < 1)
+		return -EINVAL;
+
+	priv->refclk = NULL;
+	for (i = 0; i < priv->num_clks; i++) {
+		if (!strncmp(priv->clks[i].id, "ref", 3)) {
+			priv->refclk = priv->clks[i].clk;
+			break;
+		}
+	}
+
+	if (!priv->refclk) {
+		dev_err(dev, "no refclk found\n");
+		return -EINVAL;
+	}
+
+	priv->pipe_grf = syscon_regmap_lookup_by_phandle(dev->of_node, "rockchip,pipe-grf");
+	if (IS_ERR(priv->pipe_grf)) {
+		dev_err(dev, "failed to find peri_ctrl pipe-grf regmap\n");
+		return PTR_ERR(priv->pipe_grf);
+	}
+
+	priv->phy_grf = syscon_regmap_lookup_by_phandle(dev->of_node, "rockchip,pipe-phy-grf");
+	if (IS_ERR(priv->phy_grf)) {
+		dev_err(dev, "failed to find peri_ctrl pipe-phy-grf regmap\n");
+		return PTR_ERR(priv->phy_grf);
+	}
+
+	priv->enable_ssc = device_property_present(dev, "rockchip,enable-ssc");
+
+	priv->ext_refclk = device_property_present(dev, "rockchip,ext-refclk");
+
+	priv->phy_rst = devm_reset_control_array_get_exclusive(dev);
+	if (IS_ERR(priv->phy_rst))
+		return dev_err_probe(dev, PTR_ERR(priv->phy_rst), "failed to get phy reset\n");
+
+	return 0;
+}
+
+static int rockchip_combphy_probe(struct platform_device *pdev)
+{
+	struct phy_provider *phy_provider;
+	struct device *dev = &pdev->dev;
+	struct rockchip_combphy_priv *priv;
+	const struct rockchip_combphy_cfg *phy_cfg;
+	struct resource *res;
+	int ret;
+
+	phy_cfg = of_device_get_match_data(dev);
+	if (!phy_cfg) {
+		dev_err(dev, "no OF match data provided\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->mmio = devm_platform_get_and_ioremap_resource(pdev, 0, &res);
+	if (IS_ERR(priv->mmio)) {
+		ret = PTR_ERR(priv->mmio);
+		return ret;
+	}
+
+	priv->dev = dev;
+	priv->type = PHY_NONE;
+	priv->cfg = phy_cfg;
+
+	ret = rockchip_combphy_parse_dt(dev, priv);
+	if (ret)
+		return ret;
+
+	ret = reset_control_assert(priv->phy_rst);
+	if (ret) {
+		dev_err(dev, "failed to reset phy\n");
+		return ret;
+	}
+
+	priv->phy = devm_phy_create(dev, NULL, &rochchip_combphy_ops);
+	if (IS_ERR(priv->phy)) {
+		dev_err(dev, "failed to create combphy\n");
+		return PTR_ERR(priv->phy);
+	}
+
+	dev_set_drvdata(dev, priv);
+	phy_set_drvdata(priv->phy, priv);
+
+	phy_provider = devm_of_phy_provider_register(dev, rockchip_combphy_xlate);
+
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static int rk3568_combphy_cfg(struct rockchip_combphy_priv *priv)
+{
+	const struct rockchip_combphy_grfcfg *cfg = priv->cfg->grfcfg;
+	unsigned long rate;
+	u32 val;
+
+	switch (priv->type) {
+	case PHY_TYPE_PCIE:
+		/* Set SSC downward spread spectrum. */
+		rockchip_combphy_updatel(priv, PHYREG32_SSC_MASK,
+					 PHYREG32_SSC_DOWNWARD << PHYREG32_SSC_DIR_SHIFT,
+					 PHYREG32);
+
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con0_for_pcie, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con1_for_pcie, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con2_for_pcie, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con3_for_pcie, true);
+		break;
+
+	case PHY_TYPE_USB3:
+		/* Set SSC downward spread spectrum. */
+		rockchip_combphy_updatel(priv, PHYREG32_SSC_MASK,
+					 PHYREG32_SSC_DOWNWARD << PHYREG32_SSC_DIR_SHIFT,
+					 PHYREG32);
+
+		/* Enable adaptive CTLE for USB3.0 Rx. */
+		val = readl(priv->mmio + PHYREG15);
+		val |= PHYREG15_CTLE_EN;
+		writel(val, priv->mmio + PHYREG15);
+
+		/* Set PLL KVCO fine tuning signals. */
+		rockchip_combphy_updatel(priv, PHYREG33_PLL_KVCO_MASK,
+					 PHYREG33_PLL_KVCO_VALUE << PHYREG33_PLL_KVCO_SHIFT,
+					 PHYREG33);
+
+		/* Enable controlling random jitter. */
+		writel(PHYREG12_PLL_LPF_ADJ_VALUE, priv->mmio + PHYREG12);
+
+		/* Set PLL input clock divider 1/2. */
+		rockchip_combphy_updatel(priv, PHYREG6_PLL_DIV_MASK,
+					 PHYREG6_PLL_DIV_2 << PHYREG6_PLL_DIV_SHIFT,
+					 PHYREG6);
+
+		writel(PHYREG18_PLL_LOOP, priv->mmio + PHYREG18);
+		writel(PHYREG11_SU_TRIM_0_7, priv->mmio + PHYREG11);
+
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_sel_usb, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_txcomp_sel, false);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_txelec_sel, false);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->usb_mode_set, true);
+		break;
+
+	case PHY_TYPE_SATA:
+		/* Enable adaptive CTLE for SATA Rx. */
+		val = readl(priv->mmio + PHYREG15);
+		val |= PHYREG15_CTLE_EN;
+		writel(val, priv->mmio + PHYREG15);
+		/*
+		 * Set tx_rterm=50ohm and rx_rterm=44ohm for SATA.
+		 * 0: 60ohm, 8: 50ohm 15: 44ohm (by step abort 1ohm)
+		 */
+		val = PHYREG7_TX_RTERM_50OHM << PHYREG7_TX_RTERM_SHIFT;
+		val |= PHYREG7_RX_RTERM_44OHM << PHYREG7_RX_RTERM_SHIFT;
+		writel(val, priv->mmio + PHYREG7);
+
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con0_for_sata, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con1_for_sata, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con2_for_sata, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->con3_for_sata, true);
+		rockchip_combphy_param_write(priv->pipe_grf, &cfg->pipe_con0_for_sata, true);
+		break;
+
+	case PHY_TYPE_SGMII:
+		rockchip_combphy_param_write(priv->pipe_grf, &cfg->pipe_xpcs_phy_ready, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_phymode_sel, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_sel_qsgmii, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->sgmii_mode_set, true);
+		break;
+
+	case PHY_TYPE_QSGMII:
+		rockchip_combphy_param_write(priv->pipe_grf, &cfg->pipe_xpcs_phy_ready, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_phymode_sel, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_rate_sel, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_sel_qsgmii, true);
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->qsgmii_mode_set, true);
+		break;
+
+	default:
+		dev_err(priv->dev, "incompatible PHY type\n");
+		return -EINVAL;
+	}
+
+	rate = clk_get_rate(priv->refclk);
+
+	switch (rate) {
+	case REF_CLOCK_24MHz:
+		if (priv->type == PHY_TYPE_USB3 || priv->type == PHY_TYPE_SATA) {
+			/* Set ssc_cnt[9:0]=0101111101 & 31.5KHz. */
+			val = PHYREG15_SSC_CNT_VALUE << PHYREG15_SSC_CNT_SHIFT;
+			rockchip_combphy_updatel(priv, PHYREG15_SSC_CNT_MASK,
+						 val, PHYREG15);
+
+			writel(PHYREG16_SSC_CNT_VALUE, priv->mmio + PHYREG16);
+		}
+		break;
+
+	case REF_CLOCK_25MHz:
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_clk_25m, true);
+		break;
+
+	case REF_CLOCK_100MHz:
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_clk_100m, true);
+		if (priv->type == PHY_TYPE_PCIE) {
+			/* PLL KVCO  fine tuning. */
+			val = PHYREG33_PLL_KVCO_VALUE << PHYREG33_PLL_KVCO_SHIFT;
+			rockchip_combphy_updatel(priv, PHYREG33_PLL_KVCO_MASK,
+						 val, PHYREG33);
+
+			/* Enable controlling random jitter. */
+			writel(PHYREG12_PLL_LPF_ADJ_VALUE, priv->mmio + PHYREG12);
+
+			val = PHYREG6_PLL_DIV_2 << PHYREG6_PLL_DIV_SHIFT;
+			rockchip_combphy_updatel(priv, PHYREG6_PLL_DIV_MASK,
+						 val, PHYREG6);
+
+			writel(PHYREG18_PLL_LOOP, priv->mmio + PHYREG18);
+			writel(PHYREG11_SU_TRIM_0_7, priv->mmio + PHYREG11);
+		} else if (priv->type == PHY_TYPE_SATA) {
+			/* downward spread spectrum +500ppm */
+			val = PHYREG32_SSC_DOWNWARD << PHYREG32_SSC_DIR_SHIFT;
+			val |= PHYREG32_SSC_OFFSET_500PPM << PHYREG32_SSC_OFFSET_SHIFT;
+			rockchip_combphy_updatel(priv, PHYREG32_SSC_MASK, val, PHYREG32);
+		}
+		break;
+
+	default:
+		dev_err(priv->dev, "unsupported rate: %lu\n", rate);
+		return -EINVAL;
+	}
+
+	if (priv->ext_refclk) {
+		rockchip_combphy_param_write(priv->phy_grf, &cfg->pipe_clk_ext, true);
+		if (priv->type == PHY_TYPE_PCIE && rate == REF_CLOCK_100MHz) {
+			val = PHYREG13_RESISTER_HIGH_Z << PHYREG13_RESISTER_SHIFT;
+			val |= PHYREG13_CKRCV_AMP0;
+			rockchip_combphy_updatel(priv, PHYREG13_RESISTER_MASK, val, PHYREG13);
+
+			val = readl(priv->mmio + PHYREG14);
+			val |= PHYREG14_CKRCV_AMP1;
+			writel(val, priv->mmio + PHYREG14);
+		}
+	}
+
+	if (priv->enable_ssc) {
+		val = readl(priv->mmio + PHYREG8);
+		val |= PHYREG8_SSC_EN;
+		writel(val, priv->mmio + PHYREG8);
+	}
+
+	return 0;
+}
+
+static const struct rockchip_combphy_grfcfg rk3568_combphy_grfcfgs = {
+	/* pipe-phy-grf */
+	.pcie_mode_set		= { 0x0000, 5, 0, 0x00, 0x11 },
+	.usb_mode_set		= { 0x0000, 5, 0, 0x00, 0x04 },
+	.sgmii_mode_set		= { 0x0000, 5, 0, 0x00, 0x01 },
+	.qsgmii_mode_set	= { 0x0000, 5, 0, 0x00, 0x21 },
+	.pipe_rxterm_set	= { 0x0000, 12, 12, 0x00, 0x01 },
+	.pipe_txelec_set	= { 0x0004, 1, 1, 0x00, 0x01 },
+	.pipe_txcomp_set	= { 0x0004, 4, 4, 0x00, 0x01 },
+	.pipe_clk_25m		= { 0x0004, 14, 13, 0x00, 0x01 },
+	.pipe_clk_100m		= { 0x0004, 14, 13, 0x00, 0x02 },
+	.pipe_phymode_sel	= { 0x0008, 1, 1, 0x00, 0x01 },
+	.pipe_rate_sel		= { 0x0008, 2, 2, 0x00, 0x01 },
+	.pipe_rxterm_sel	= { 0x0008, 8, 8, 0x00, 0x01 },
+	.pipe_txelec_sel	= { 0x0008, 12, 12, 0x00, 0x01 },
+	.pipe_txcomp_sel	= { 0x0008, 15, 15, 0x00, 0x01 },
+	.pipe_clk_ext		= { 0x000c, 9, 8, 0x02, 0x01 },
+	.pipe_sel_usb		= { 0x000c, 14, 13, 0x00, 0x01 },
+	.pipe_sel_qsgmii	= { 0x000c, 15, 13, 0x00, 0x07 },
+	.pipe_phy_status	= { 0x0034, 6, 6, 0x01, 0x00 },
+	.con0_for_pcie		= { 0x0000, 15, 0, 0x00, 0x1000 },
+	.con1_for_pcie		= { 0x0004, 15, 0, 0x00, 0x0000 },
+	.con2_for_pcie		= { 0x0008, 15, 0, 0x00, 0x0101 },
+	.con3_for_pcie		= { 0x000c, 15, 0, 0x00, 0x0200 },
+	.con0_for_sata		= { 0x0000, 15, 0, 0x00, 0x0119 },
+	.con1_for_sata		= { 0x0004, 15, 0, 0x00, 0x0040 },
+	.con2_for_sata		= { 0x0008, 15, 0, 0x00, 0x80c3 },
+	.con3_for_sata		= { 0x000c, 15, 0, 0x00, 0x4407 },
+	/* pipe-grf */
+	.pipe_con0_for_sata	= { 0x0000, 15, 0, 0x00, 0x2220 },
+	.pipe_xpcs_phy_ready	= { 0x0040, 2, 2, 0x00, 0x01 },
+};
+
+static const struct rockchip_combphy_cfg rk3568_combphy_cfgs = {
+	.grfcfg		= &rk3568_combphy_grfcfgs,
+	.combphy_cfg	= rk3568_combphy_cfg,
+};
+
+static const struct of_device_id rockchip_combphy_of_match[] = {
+	{
+		.compatible = "rockchip,rk3568-naneng-combphy",
+		.data = &rk3568_combphy_cfgs,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, rockchip_combphy_of_match);
+
+static struct platform_driver rockchip_combphy_driver = {
+	.probe	= rockchip_combphy_probe,
+	.driver = {
+		.name = "rockchip-naneng-combphy",
+		.of_match_table = rockchip_combphy_of_match,
+	},
+};
+module_platform_driver(rockchip_combphy_driver);
+
+MODULE_DESCRIPTION("Rockchip NANENG COMBPHY driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/ti/phy-tusb1210.c b/drivers/phy/ti/phy-tusb1210.c
index 15c1c79e5c29..a0cdbcadf09e 100644
--- a/drivers/phy/ti/phy-tusb1210.c
+++ b/drivers/phy/ti/phy-tusb1210.c
@@ -8,24 +8,93 @@
  */
 #include <linux/module.h>
 #include <linux/bitfield.h>
+#include <linux/delay.h>
 #include <linux/ulpi/driver.h>
 #include <linux/ulpi/regs.h>
 #include <linux/gpio/consumer.h>
 #include <linux/phy/ulpi_phy.h>
-
-#define TUSB1210_VENDOR_SPECIFIC2		0x80
-#define TUSB1210_VENDOR_SPECIFIC2_IHSTX_MASK	GENMASK(3, 0)
-#define TUSB1210_VENDOR_SPECIFIC2_ZHSDRV_MASK	GENMASK(5, 4)
-#define TUSB1210_VENDOR_SPECIFIC2_DP_MASK	BIT(6)
+#include <linux/power_supply.h>
+#include <linux/workqueue.h>
+
+#define TUSB1211_POWER_CONTROL				0x3d
+#define TUSB1211_POWER_CONTROL_SET			0x3e
+#define TUSB1211_POWER_CONTROL_CLEAR			0x3f
+#define TUSB1211_POWER_CONTROL_SW_CONTROL		BIT(0)
+#define TUSB1211_POWER_CONTROL_DET_COMP			BIT(1)
+#define TUSB1211_POWER_CONTROL_DP_VSRC_EN		BIT(6)
+
+#define TUSB1210_VENDOR_SPECIFIC2			0x80
+#define TUSB1210_VENDOR_SPECIFIC2_IHSTX_MASK		GENMASK(3, 0)
+#define TUSB1210_VENDOR_SPECIFIC2_ZHSDRV_MASK		GENMASK(5, 4)
+#define TUSB1210_VENDOR_SPECIFIC2_DP_MASK		BIT(6)
+
+#define TUSB1211_VENDOR_SPECIFIC3			0x85
+#define TUSB1211_VENDOR_SPECIFIC3_SET			0x86
+#define TUSB1211_VENDOR_SPECIFIC3_CLEAR			0x87
+#define TUSB1211_VENDOR_SPECIFIC3_SW_USB_DET		BIT(4)
+#define TUSB1211_VENDOR_SPECIFIC3_CHGD_IDP_SRC_EN	BIT(6)
+
+#define TUSB1210_RESET_TIME_MS				50
+
+#define TUSB1210_CHG_DET_MAX_RETRIES			5
+
+/* TUSB1210 charger detection work states */
+enum tusb1210_chg_det_state {
+	TUSB1210_CHG_DET_CONNECTING,
+	TUSB1210_CHG_DET_START_DET,
+	TUSB1210_CHG_DET_READ_DET,
+	TUSB1210_CHG_DET_FINISH_DET,
+	TUSB1210_CHG_DET_CONNECTED,
+	TUSB1210_CHG_DET_DISCONNECTING,
+	TUSB1210_CHG_DET_DISCONNECTING_DONE,
+	TUSB1210_CHG_DET_DISCONNECTED,
+};
 
 struct tusb1210 {
 	struct ulpi *ulpi;
 	struct phy *phy;
 	struct gpio_desc *gpio_reset;
 	struct gpio_desc *gpio_cs;
+	u8 otg_ctrl;
 	u8 vendor_specific2;
+#ifdef CONFIG_POWER_SUPPLY
+	enum power_supply_usb_type chg_type;
+	enum tusb1210_chg_det_state chg_det_state;
+	int chg_det_retries;
+	struct delayed_work chg_det_work;
+	struct notifier_block psy_nb;
+	struct power_supply *psy;
+	struct power_supply *charger;
+#endif
 };
 
+static int tusb1210_ulpi_write(struct tusb1210 *tusb, u8 reg, u8 val)
+{
+	int ret;
+
+	ret = ulpi_write(tusb->ulpi, reg, val);
+	if (ret)
+		dev_err(&tusb->ulpi->dev, "error %d writing val 0x%02x to reg 0x%02x\n",
+			ret, val, reg);
+
+	return ret;
+}
+
+static int tusb1210_ulpi_read(struct tusb1210 *tusb, u8 reg, u8 *val)
+{
+	int ret;
+
+	ret = ulpi_read(tusb->ulpi, reg);
+	if (ret >= 0) {
+		*val = ret;
+		ret = 0;
+	} else {
+		dev_err(&tusb->ulpi->dev, "error %d reading reg 0x%02x\n", ret, reg);
+	}
+
+	return ret;
+}
+
 static int tusb1210_power_on(struct phy *phy)
 {
 	struct tusb1210 *tusb = phy_get_drvdata(phy);
@@ -33,12 +102,11 @@ static int tusb1210_power_on(struct phy *phy)
 	gpiod_set_value_cansleep(tusb->gpio_reset, 1);
 	gpiod_set_value_cansleep(tusb->gpio_cs, 1);
 
-	/* Restore the optional eye diagram optimization value */
-	if (tusb->vendor_specific2)
-		ulpi_write(tusb->ulpi, TUSB1210_VENDOR_SPECIFIC2,
-			   tusb->vendor_specific2);
+	msleep(TUSB1210_RESET_TIME_MS);
 
-	return 0;
+	/* Restore the optional eye diagram optimization value */
+	return tusb1210_ulpi_write(tusb, TUSB1210_VENDOR_SPECIFIC2,
+				   tusb->vendor_specific2);
 }
 
 static int tusb1210_power_off(struct phy *phy)
@@ -55,35 +123,357 @@ static int tusb1210_set_mode(struct phy *phy, enum phy_mode mode, int submode)
 {
 	struct tusb1210 *tusb = phy_get_drvdata(phy);
 	int ret;
+	u8 reg;
 
-	ret = ulpi_read(tusb->ulpi, ULPI_OTG_CTRL);
+	ret = tusb1210_ulpi_read(tusb, ULPI_OTG_CTRL, &reg);
 	if (ret < 0)
 		return ret;
 
 	switch (mode) {
 	case PHY_MODE_USB_HOST:
-		ret |= (ULPI_OTG_CTRL_DRVVBUS_EXT
+		reg |= (ULPI_OTG_CTRL_DRVVBUS_EXT
 			| ULPI_OTG_CTRL_ID_PULLUP
 			| ULPI_OTG_CTRL_DP_PULLDOWN
 			| ULPI_OTG_CTRL_DM_PULLDOWN);
-		ulpi_write(tusb->ulpi, ULPI_OTG_CTRL, ret);
-		ret |= ULPI_OTG_CTRL_DRVVBUS;
+		tusb1210_ulpi_write(tusb, ULPI_OTG_CTRL, reg);
+		reg |= ULPI_OTG_CTRL_DRVVBUS;
 		break;
 	case PHY_MODE_USB_DEVICE:
-		ret &= ~(ULPI_OTG_CTRL_DRVVBUS
+		reg &= ~(ULPI_OTG_CTRL_DRVVBUS
 			 | ULPI_OTG_CTRL_DP_PULLDOWN
 			 | ULPI_OTG_CTRL_DM_PULLDOWN);
-		ulpi_write(tusb->ulpi, ULPI_OTG_CTRL, ret);
-		ret &= ~ULPI_OTG_CTRL_DRVVBUS_EXT;
+		tusb1210_ulpi_write(tusb, ULPI_OTG_CTRL, reg);
+		reg &= ~ULPI_OTG_CTRL_DRVVBUS_EXT;
 		break;
 	default:
 		/* nothing */
 		return 0;
 	}
 
-	return ulpi_write(tusb->ulpi, ULPI_OTG_CTRL, ret);
+	tusb->otg_ctrl = reg;
+	return tusb1210_ulpi_write(tusb, ULPI_OTG_CTRL, reg);
 }
 
+#ifdef CONFIG_POWER_SUPPLY
+const char * const tusb1210_chg_det_states[] = {
+	"CHG_DET_CONNECTING",
+	"CHG_DET_START_DET",
+	"CHG_DET_READ_DET",
+	"CHG_DET_FINISH_DET",
+	"CHG_DET_CONNECTED",
+	"CHG_DET_DISCONNECTING",
+	"CHG_DET_DISCONNECTING_DONE",
+	"CHG_DET_DISCONNECTED",
+};
+
+static void tusb1210_reset(struct tusb1210 *tusb)
+{
+	gpiod_set_value_cansleep(tusb->gpio_reset, 0);
+	usleep_range(200, 500);
+	gpiod_set_value_cansleep(tusb->gpio_reset, 1);
+}
+
+static void tusb1210_chg_det_set_type(struct tusb1210 *tusb,
+				      enum power_supply_usb_type type)
+{
+	dev_dbg(&tusb->ulpi->dev, "charger type: %d\n", type);
+	tusb->chg_type = type;
+	tusb->chg_det_retries = 0;
+	power_supply_changed(tusb->psy);
+}
+
+static void tusb1210_chg_det_set_state(struct tusb1210 *tusb,
+				       enum tusb1210_chg_det_state new_state,
+				       int delay_ms)
+{
+	if (delay_ms)
+		dev_dbg(&tusb->ulpi->dev, "chg_det new state %s in %d ms\n",
+			tusb1210_chg_det_states[new_state], delay_ms);
+
+	tusb->chg_det_state = new_state;
+	mod_delayed_work(system_long_wq, &tusb->chg_det_work,
+			 msecs_to_jiffies(delay_ms));
+}
+
+static void tusb1210_chg_det_handle_ulpi_error(struct tusb1210 *tusb)
+{
+	tusb1210_reset(tusb);
+	if (tusb->chg_det_retries < TUSB1210_CHG_DET_MAX_RETRIES) {
+		tusb->chg_det_retries++;
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_START_DET,
+					   TUSB1210_RESET_TIME_MS);
+	} else {
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_FINISH_DET,
+					   TUSB1210_RESET_TIME_MS);
+	}
+}
+
+/*
+ * Boards using a TUSB121x for charger-detection have 3 power_supply class devs:
+ *
+ * tusb1211-charger-detect(1) -> charger -> fuel-gauge
+ *
+ * To determine if an USB charger is connected to the board, the online prop of
+ * the charger psy needs to be read. Since the tusb1211-charger-detect psy is
+ * the start of the supplier -> supplied-to chain, power_supply_am_i_supplied()
+ * cannot be used here.
+ *
+ * Instead, below is a list of the power_supply names of known chargers for
+ * these boards and the charger psy is looked up by name from this list.
+ *
+ * (1) modelling the external USB charger
+ */
+static const char * const tusb1210_chargers[] = {
+	"bq24190-charger",
+};
+
+static bool tusb1210_get_online(struct tusb1210 *tusb)
+{
+	union power_supply_propval val;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tusb1210_chargers) && !tusb->charger; i++)
+		tusb->charger = power_supply_get_by_name(tusb1210_chargers[i]);
+
+	if (!tusb->charger)
+		return false;
+
+	if (power_supply_get_property(tusb->charger, POWER_SUPPLY_PROP_ONLINE, &val))
+		return false;
+
+	return val.intval;
+}
+
+static void tusb1210_chg_det_work(struct work_struct *work)
+{
+	struct tusb1210 *tusb = container_of(work, struct tusb1210, chg_det_work.work);
+	bool vbus_present = tusb1210_get_online(tusb);
+	int ret;
+	u8 val;
+
+	dev_dbg(&tusb->ulpi->dev, "chg_det state %s vbus_present %d\n",
+		tusb1210_chg_det_states[tusb->chg_det_state], vbus_present);
+
+	switch (tusb->chg_det_state) {
+	case TUSB1210_CHG_DET_CONNECTING:
+		tusb->chg_type = POWER_SUPPLY_USB_TYPE_UNKNOWN;
+		tusb->chg_det_retries = 0;
+		/* Power on USB controller for ulpi_read()/_write() */
+		ret = pm_runtime_resume_and_get(tusb->ulpi->dev.parent);
+		if (ret < 0) {
+			dev_err(&tusb->ulpi->dev, "error %d runtime-resuming\n", ret);
+			/* Should never happen, skip charger detection */
+			tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_CONNECTED, 0);
+			return;
+		}
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_START_DET, 0);
+		break;
+	case TUSB1210_CHG_DET_START_DET:
+		/*
+		 * Use the builtin charger detection FSM to keep things simple.
+		 * This only detects DCP / SDP. This is good enough for the few
+		 * boards which actually rely on the phy for charger detection.
+		 */
+		mutex_lock(&tusb->phy->mutex);
+		ret = tusb1210_ulpi_write(tusb, TUSB1211_VENDOR_SPECIFIC3_SET,
+					  TUSB1211_VENDOR_SPECIFIC3_SW_USB_DET);
+		mutex_unlock(&tusb->phy->mutex);
+		if (ret) {
+			tusb1210_chg_det_handle_ulpi_error(tusb);
+			break;
+		}
+
+		/* Wait 400 ms for the charger detection FSM to finish */
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_READ_DET, 400);
+		break;
+	case TUSB1210_CHG_DET_READ_DET:
+		mutex_lock(&tusb->phy->mutex);
+		ret = tusb1210_ulpi_read(tusb, TUSB1211_POWER_CONTROL, &val);
+		mutex_unlock(&tusb->phy->mutex);
+		if (ret) {
+			tusb1210_chg_det_handle_ulpi_error(tusb);
+			break;
+		}
+
+		if (val & TUSB1211_POWER_CONTROL_DET_COMP)
+			tusb1210_chg_det_set_type(tusb, POWER_SUPPLY_USB_TYPE_DCP);
+		else
+			tusb1210_chg_det_set_type(tusb, POWER_SUPPLY_USB_TYPE_SDP);
+
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_FINISH_DET, 0);
+		break;
+	case TUSB1210_CHG_DET_FINISH_DET:
+		mutex_lock(&tusb->phy->mutex);
+
+		/* Set SW_CONTROL to stop the charger-det FSM */
+		ret = tusb1210_ulpi_write(tusb, TUSB1211_POWER_CONTROL_SET,
+					  TUSB1211_POWER_CONTROL_SW_CONTROL);
+
+		/* Clear DP_VSRC_EN which may have been enabled by the charger-det FSM */
+		ret |= tusb1210_ulpi_write(tusb, TUSB1211_POWER_CONTROL_CLEAR,
+					   TUSB1211_POWER_CONTROL_DP_VSRC_EN);
+
+		/* Clear CHGD_IDP_SRC_EN (may have been enabled by the charger-det FSM) */
+		ret |= tusb1210_ulpi_write(tusb, TUSB1211_VENDOR_SPECIFIC3_CLEAR,
+					   TUSB1211_VENDOR_SPECIFIC3_CHGD_IDP_SRC_EN);
+
+		/* If any of the above fails reset the phy */
+		if (ret) {
+			tusb1210_reset(tusb);
+			msleep(TUSB1210_RESET_TIME_MS);
+		}
+
+		/* Restore phy-parameters and OTG_CTRL register */
+		tusb1210_ulpi_write(tusb, ULPI_OTG_CTRL, tusb->otg_ctrl);
+		tusb1210_ulpi_write(tusb, TUSB1210_VENDOR_SPECIFIC2,
+				    tusb->vendor_specific2);
+
+		mutex_unlock(&tusb->phy->mutex);
+
+		pm_runtime_put(tusb->ulpi->dev.parent);
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_CONNECTED, 0);
+		break;
+	case TUSB1210_CHG_DET_CONNECTED:
+		if (!vbus_present)
+			tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_DISCONNECTING, 0);
+		break;
+	case TUSB1210_CHG_DET_DISCONNECTING:
+		/*
+		 * The phy seems to take approx. 600ms longer then the charger
+		 * chip (which is used to get vbus_present) to determine Vbus
+		 * session end. Wait 800ms to ensure the phy has detected and
+		 * signalled Vbus session end.
+		 */
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_DISCONNECTING_DONE, 800);
+		break;
+	case TUSB1210_CHG_DET_DISCONNECTING_DONE:
+		/*
+		 * The phy often stops reacting to ulpi_read()/_write requests
+		 * after a Vbus-session end. Reset it to work around this.
+		 */
+		tusb1210_reset(tusb);
+		tusb1210_chg_det_set_type(tusb, POWER_SUPPLY_USB_TYPE_UNKNOWN);
+		tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_DISCONNECTED, 0);
+		break;
+	case TUSB1210_CHG_DET_DISCONNECTED:
+		if (vbus_present)
+			tusb1210_chg_det_set_state(tusb, TUSB1210_CHG_DET_CONNECTING, 0);
+		break;
+	}
+}
+
+static int tusb1210_psy_notifier(struct notifier_block *nb,
+	unsigned long event, void *ptr)
+{
+	struct tusb1210 *tusb = container_of(nb, struct tusb1210, psy_nb);
+	struct power_supply *psy = ptr;
+
+	if (psy != tusb->psy && psy->desc->type == POWER_SUPPLY_TYPE_USB)
+		queue_delayed_work(system_long_wq, &tusb->chg_det_work, 0);
+
+	return NOTIFY_OK;
+}
+
+static int tusb1210_psy_get_prop(struct power_supply *psy,
+				 enum power_supply_property psp,
+				 union power_supply_propval *val)
+{
+	struct tusb1210 *tusb = power_supply_get_drvdata(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = tusb1210_get_online(tusb);
+		break;
+	case POWER_SUPPLY_PROP_USB_TYPE:
+		val->intval = tusb->chg_type;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_MAX:
+		if (tusb->chg_type == POWER_SUPPLY_USB_TYPE_DCP)
+			val->intval = 2000000;
+		else
+			val->intval = 500000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const enum power_supply_usb_type tusb1210_psy_usb_types[] = {
+	POWER_SUPPLY_USB_TYPE_SDP,
+	POWER_SUPPLY_USB_TYPE_DCP,
+	POWER_SUPPLY_USB_TYPE_UNKNOWN,
+};
+
+static const enum power_supply_property tusb1210_psy_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_USB_TYPE,
+	POWER_SUPPLY_PROP_CURRENT_MAX,
+};
+
+static const struct power_supply_desc tusb1210_psy_desc = {
+	.name = "tusb1211-charger-detect",
+	.type = POWER_SUPPLY_TYPE_USB,
+	.usb_types = tusb1210_psy_usb_types,
+	.num_usb_types = ARRAY_SIZE(tusb1210_psy_usb_types),
+	.properties = tusb1210_psy_props,
+	.num_properties = ARRAY_SIZE(tusb1210_psy_props),
+	.get_property = tusb1210_psy_get_prop,
+};
+
+/* Setup charger detection if requested, on errors continue without chg-det */
+static void tusb1210_probe_charger_detect(struct tusb1210 *tusb)
+{
+	struct power_supply_config psy_cfg = { .drv_data = tusb };
+	struct device *dev = &tusb->ulpi->dev;
+	int ret;
+
+	if (!device_property_read_bool(dev->parent, "linux,phy_charger_detect"))
+		return;
+
+	if (tusb->ulpi->id.product != 0x1508) {
+		dev_err(dev, "error charger detection is only supported on the TUSB1211\n");
+		return;
+	}
+
+	ret = tusb1210_ulpi_read(tusb, ULPI_OTG_CTRL, &tusb->otg_ctrl);
+	if (ret)
+		return;
+
+	tusb->psy = power_supply_register(dev, &tusb1210_psy_desc, &psy_cfg);
+	if (IS_ERR(tusb->psy))
+		return;
+
+	/*
+	 * Delay initial run by 2 seconds to allow the charger driver,
+	 * which is used to determine vbus_present, to load.
+	 */
+	tusb->chg_det_state = TUSB1210_CHG_DET_DISCONNECTED;
+	INIT_DELAYED_WORK(&tusb->chg_det_work, tusb1210_chg_det_work);
+	queue_delayed_work(system_long_wq, &tusb->chg_det_work, 2 * HZ);
+
+	tusb->psy_nb.notifier_call = tusb1210_psy_notifier;
+	power_supply_reg_notifier(&tusb->psy_nb);
+}
+
+static void tusb1210_remove_charger_detect(struct tusb1210 *tusb)
+{
+
+	if (!IS_ERR_OR_NULL(tusb->psy)) {
+		power_supply_unreg_notifier(&tusb->psy_nb);
+		cancel_delayed_work_sync(&tusb->chg_det_work);
+		power_supply_unregister(tusb->psy);
+	}
+
+	if (tusb->charger)
+		power_supply_put(tusb->charger);
+}
+#else
+static void tusb1210_probe_charger_detect(struct tusb1210 *tusb) { }
+static void tusb1210_remove_charger_detect(struct tusb1210 *tusb) { }
+#endif
+
 static const struct phy_ops phy_ops = {
 	.power_on = tusb1210_power_on,
 	.power_off = tusb1210_power_off,
@@ -95,11 +485,14 @@ static int tusb1210_probe(struct ulpi *ulpi)
 {
 	struct tusb1210 *tusb;
 	u8 val, reg;
+	int ret;
 
 	tusb = devm_kzalloc(&ulpi->dev, sizeof(*tusb), GFP_KERNEL);
 	if (!tusb)
 		return -ENOMEM;
 
+	tusb->ulpi = ulpi;
+
 	tusb->gpio_reset = devm_gpiod_get_optional(&ulpi->dev, "reset",
 						   GPIOD_OUT_LOW);
 	if (IS_ERR(tusb->gpio_reset))
@@ -119,7 +512,9 @@ static int tusb1210_probe(struct ulpi *ulpi)
 	 * diagram optimization and DP/DM swap.
 	 */
 
-	reg = ulpi_read(ulpi, TUSB1210_VENDOR_SPECIFIC2);
+	ret = tusb1210_ulpi_read(tusb, TUSB1210_VENDOR_SPECIFIC2, &reg);
+	if (ret)
+		return ret;
 
 	/* High speed output drive strength configuration */
 	if (!device_property_read_u8(&ulpi->dev, "ihstx", &val))
@@ -133,15 +528,18 @@ static int tusb1210_probe(struct ulpi *ulpi)
 	if (!device_property_read_u8(&ulpi->dev, "datapolarity", &val))
 		u8p_replace_bits(&reg, val, (u8)TUSB1210_VENDOR_SPECIFIC2_DP_MASK);
 
-	ulpi_write(ulpi, TUSB1210_VENDOR_SPECIFIC2, reg);
+	ret = tusb1210_ulpi_write(tusb, TUSB1210_VENDOR_SPECIFIC2, reg);
+	if (ret)
+		return ret;
+
 	tusb->vendor_specific2 = reg;
 
+	tusb1210_probe_charger_detect(tusb);
+
 	tusb->phy = ulpi_phy_create(ulpi, &phy_ops);
 	if (IS_ERR(tusb->phy))
 		return PTR_ERR(tusb->phy);
 
-	tusb->ulpi = ulpi;
-
 	phy_set_drvdata(tusb->phy, tusb);
 	ulpi_set_drvdata(ulpi, tusb);
 	return 0;
@@ -152,6 +550,7 @@ static void tusb1210_remove(struct ulpi *ulpi)
 	struct tusb1210 *tusb = ulpi_get_drvdata(ulpi);
 
 	ulpi_phy_destroy(ulpi, tusb->phy);
+	tusb1210_remove_charger_detect(tusb);
 }
 
 #define TI_VENDOR_ID 0x0451
author	Linus Torvalds <torvalds@linux-foundation.org>	2022-03-28 12:27:35 -0700
committer	Linus Torvalds <torvalds@linux-foundation.org>	2022-03-28 12:27:35 -0700
commit	02e2af20f4f9f2aa0c84e9a30a35c02f0fbb7daa (patch)
tree	d126449a7d2ea2270627183f7cebd726fbe56a9d /drivers/phy
parent	Merge tag 'pinctrl-v5.18-1' of git://git.kernel.org/pub/scm/linux/kernel/git/linusw/linux-pinctrl (diff)
parent	firmware: google: Properly state IOMEM dependency (diff)
download	linux-dev-02e2af20f4f9f2aa0c84e9a30a35c02f0fbb7daa.tar.xz linux-dev-02e2af20f4f9f2aa0c84e9a30a35c02f0fbb7daa.zip