4 files changed, 61 insertions, 4 deletions

diff --git a/src/soc/mediatek/common/include/soc/usb_common.h b/src/soc/mediatek/common/include/soc/usb_common.h
index 6fa050dcdf..d390b70ffa 100644
--- a/src/soc/mediatek/common/include/soc/usb_common.h
+++ b/src/soc/mediatek/common/include/soc/usb_common.h
@@ -130,9 +130,13 @@ struct sif_u2_phy_com {
 check_member(sif_u2_phy_com, u2phydtm0, 0x68);
 
 struct sif_u3phyd {
-	u32 reserved0[23];
+	u32 reserved0[4];
+	u32 phyd_cal0;
+	u32 phyd_cal1;
+	u32 reserved1[15];
+	u32 phyd_reserved;
 	u32 phyd_cdr1;
-	u32 reserved1[40];
+	u32 reserved2[41];
 };
 
 struct sif_u3phya {
@@ -155,6 +159,7 @@ struct sif_u3phya_da {
  * SOCs will not need it.
  */
 void mtk_usb_prepare(void);
+void mtk_usb_adjust_phy_shift(void);
 
 void setup_usb_host(void);
 
diff --git a/src/soc/mediatek/common/usb.c b/src/soc/mediatek/common/usb.c
index 52daaac117..b9fe83529d 100644
--- a/src/soc/mediatek/common/usb.c
+++ b/src/soc/mediatek/common/usb.c
@@ -140,7 +140,15 @@ static inline void ssusb_soft_reset(void)
 	clrbits32(&ippc_regs->ip_pw_ctr0, CTRL0_IP_SW_RST);
 }
 
-__weak void mtk_usb_prepare(void) { /* do nothing */ }
+__weak void mtk_usb_prepare(void)
+{
+	/* do nothing */
+}
+
+__weak void mtk_usb_adjust_phy_shift(void)
+{
+	/* do nothing */
+}
 
 void setup_usb_host(void)
 {
@@ -153,5 +161,6 @@ void setup_usb_host(void)
 		return;
 	}
 	u3phy_power_on();
+	mtk_usb_adjust_phy_shift();
 	u3p_msg("phy power-on done.\n");
 }
diff --git a/src/soc/mediatek/mt8195/include/soc/usb.h b/src/soc/mediatek/mt8195/include/soc/usb.h
index e39ec3841d..c3b3c586f6 100644
--- a/src/soc/mediatek/mt8195/include/soc/usb.h
+++ b/src/soc/mediatek/mt8195/include/soc/usb.h
@@ -19,6 +19,21 @@ check_member(ssusb_sif_port, u3phya, 0x800);
 check_member(ssusb_sif_port, u3phya_da, 0x900);
 check_member(ssusb_sif_port, reserved2, 0xa00);
 
-#define USB_PORT_NUMBER 1
+DEFINE_BIT(AUTO_LOAD_DIS, 12)
+DEFINE_BITFIELD(TX_IMP_CAL, 28, 24)
+DEFINE_BIT(TX_IMP_CAL_EN, 31)
+DEFINE_BITFIELD(RX_IMP_CAL, 28, 24)
+DEFINE_BIT(RX_IMP_CAL_EN, 31)
+DEFINE_BITFIELD(INTR_CAL, 15, 10)
+
+#define TX_IMP_MASK		0x1F
+#define TX_IMP_SHIFT		0
+#define RX_IMP_MASK		0x3E0
+#define RX_IMP_SHIFT		5
+#define INTR_CAL_MASK		0xFC00
+#define INTR_CAL_SHIFT		10
+
+#define USB_PHY_SETTING_REG	0x11C10184
+#define USB_PORT_NUMBER		1
 
 #endif
diff --git a/src/soc/mediatek/mt8195/usb.c b/src/soc/mediatek/mt8195/usb.c
index 7b6347e605..b3f00a97cc 100644
--- a/src/soc/mediatek/mt8195/usb.c
+++ b/src/soc/mediatek/mt8195/usb.c
@@ -10,3 +10,31 @@ void mtk_usb_prepare(void)
 	setbits32(&mtk_topckgen->clk_cfg_11_clr, BIT(7) | BIT(15));
 	setbits32(&mt8195_infracfg_ao->module_sw_cg_2_clr, BIT(1) | BIT(31));
 }
+
+void mtk_usb_adjust_phy_shift(void)
+{
+	u32 phy_set_val, write_val;
+	struct ssusb_sif_port *phy = (void *)(SSUSB_SIF_BASE);
+
+	SET32_BITFIELDS(&phy->u3phyd.phyd_reserved,
+			AUTO_LOAD_DIS, 1);
+
+	phy_set_val = read32((void *)USB_PHY_SETTING_REG);
+
+	/* TX imp */
+	write_val = (phy_set_val & TX_IMP_MASK) >> TX_IMP_SHIFT;
+	SET32_BITFIELDS(&phy->u3phyd.phyd_cal0,
+			TX_IMP_CAL, write_val,
+			TX_IMP_CAL_EN, 1);
+
+	/* RX imp */
+	write_val = (phy_set_val & RX_IMP_MASK) >> RX_IMP_SHIFT;
+	SET32_BITFIELDS(&phy->u3phyd.phyd_cal1,
+			RX_IMP_CAL, write_val,
+			RX_IMP_CAL_EN, 1);
+
+	/* Intr_cal */
+	write_val = (phy_set_val & INTR_CAL_MASK) >> INTR_CAL_SHIFT;
+	SET32_BITFIELDS(&phy->u3phya.phya_reg0,
+			INTR_CAL, write_val);
+}