++ mdelay(10);
++}
++
++void mtk_sgmii_reset(struct mtk_eth *eth, int mac_id)
++{
++ u32 xgmii_id = mtk_mac2xgmii_id(eth, mac_id);
++
++ mtk_usxgmii_reset(eth, xgmii_id);
++}
++
++
++static int mtk_usxgmii_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
++ phy_interface_t interface,
++ const unsigned long *advertising,
++ bool permit_pause_to_mac)
++{
++ struct mtk_usxgmii_pcs *mpcs = pcs_to_mtk_usxgmii_pcs(pcs);
++ struct mtk_eth *eth = mpcs->eth;
++ unsigned int an_ctrl = 0, link_timer = 0, xfi_mode = 0, adapt_mode = 0;
++ bool mode_changed = false;
++
++ if (interface == PHY_INTERFACE_MODE_USXGMII) {
++ an_ctrl = FIELD_PREP(USXGMII_AN_SYNC_CNT, 0x1FF) |
++ USXGMII_AN_ENABLE;
++ link_timer = FIELD_PREP(USXGMII_LINK_TIMER_IDLE_DETECT, 0x7B) |
++ FIELD_PREP(USXGMII_LINK_TIMER_COMP_ACK_DETECT, 0x7B) |
++ FIELD_PREP(USXGMII_LINK_TIMER_AN_RESTART, 0x7B);
++ xfi_mode = FIELD_PREP(USXGMII_XFI_RX_MODE, USXGMII_XFI_RX_MODE_10G) |
++ FIELD_PREP(USXGMII_XFI_TX_MODE, USXGMII_XFI_TX_MODE_10G);
++ } else if (interface == PHY_INTERFACE_MODE_10GKR) {
++ an_ctrl = FIELD_PREP(USXGMII_AN_SYNC_CNT, 0x1FF);
++ link_timer = FIELD_PREP(USXGMII_LINK_TIMER_IDLE_DETECT, 0x7B) |
++ FIELD_PREP(USXGMII_LINK_TIMER_COMP_ACK_DETECT, 0x7B) |
++ FIELD_PREP(USXGMII_LINK_TIMER_AN_RESTART, 0x7B);
++ xfi_mode = FIELD_PREP(USXGMII_XFI_RX_MODE, USXGMII_XFI_RX_MODE_10G) |
++ FIELD_PREP(USXGMII_XFI_TX_MODE, USXGMII_XFI_TX_MODE_10G);
++ adapt_mode = USXGMII_RATE_UPDATE_MODE;
++ } else if (interface == PHY_INTERFACE_MODE_5GBASER) {
++ an_ctrl = FIELD_PREP(USXGMII_AN_SYNC_CNT, 0xFF);
++ link_timer = FIELD_PREP(USXGMII_LINK_TIMER_IDLE_DETECT, 0x3D) |
++ FIELD_PREP(USXGMII_LINK_TIMER_COMP_ACK_DETECT, 0x3D) |
++ FIELD_PREP(USXGMII_LINK_TIMER_AN_RESTART, 0x3D);
++ xfi_mode = FIELD_PREP(USXGMII_XFI_RX_MODE, USXGMII_XFI_RX_MODE_5G) |
++ FIELD_PREP(USXGMII_XFI_TX_MODE, USXGMII_XFI_TX_MODE_5G);
++ adapt_mode = USXGMII_RATE_UPDATE_MODE;
++ } else
++ return -EINVAL;
++
++ adapt_mode |= FIELD_PREP(USXGMII_RATE_ADAPT_MODE, USXGMII_RATE_ADAPT_MODE_X1);
++
++ if (mpcs->interface != interface) {
++ mpcs->interface = interface;
++ mode_changed = true;
++ }
++
++ mtk_xfi_pll_enable(eth);
++ mtk_usxgmii_reset(eth, mpcs->id);
++
++ /* Setup USXGMII AN ctrl */
++ regmap_update_bits(mpcs->regmap, RG_PCS_AN_CTRL0,
++ USXGMII_AN_SYNC_CNT | USXGMII_AN_ENABLE,
++ an_ctrl);
++
++ regmap_update_bits(mpcs->regmap, RG_PCS_AN_CTRL2,
++ USXGMII_LINK_TIMER_IDLE_DETECT |
++ USXGMII_LINK_TIMER_COMP_ACK_DETECT |
++ USXGMII_LINK_TIMER_AN_RESTART,
++ link_timer);
++
++ /* Gated MAC CK */
++ regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++ USXGMII_MAC_CK_GATED, USXGMII_MAC_CK_GATED);
++
++ /* Enable interface force mode */
++ regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++ USXGMII_IF_FORCE_EN, USXGMII_IF_FORCE_EN);
++
++ /* Setup USXGMII adapt mode */
++ regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++ USXGMII_RATE_UPDATE_MODE | USXGMII_RATE_ADAPT_MODE,
++ adapt_mode);
++
++ /* Setup USXGMII speed */
++ regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++ USXGMII_XFI_RX_MODE | USXGMII_XFI_TX_MODE,
++ xfi_mode);
++
++ udelay(1);
++
++ /* Un-gated MAC CK */
++ regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++ USXGMII_MAC_CK_GATED, 0);
++
++ udelay(1);
++
++ /* Disable interface force mode for the AN mode */
++ if (an_ctrl & USXGMII_AN_ENABLE)
++ regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++ USXGMII_IF_FORCE_EN, 0);
++
++ /* Setup USXGMIISYS with the determined property */
++ if (interface == PHY_INTERFACE_MODE_USXGMII)
++ mtk_usxgmii_setup_phya_usxgmii(mpcs);
++ else if (interface == PHY_INTERFACE_MODE_10GKR)
++ mtk_usxgmii_setup_phya_10gbaser(mpcs);
++ else if (interface == PHY_INTERFACE_MODE_5GBASER)
++ mtk_usxgmii_setup_phya_5gbaser(mpcs);
++
++ return mode_changed;
++}
++
++static void mtk_usxgmii_pcs_get_state(struct phylink_pcs *pcs,
++ struct phylink_link_state *state)
++{
++ struct mtk_usxgmii_pcs *mpcs = pcs_to_mtk_usxgmii_pcs(pcs);
++ struct mtk_eth *eth = mpcs->eth;
++ struct mtk_mac *mac = eth->mac[mtk_xgmii2mac_id(eth, mpcs->id)];
++ u32 val = 0;
++
++ regmap_read(mpcs->regmap, RG_PCS_AN_CTRL0, &val);
++ if (FIELD_GET(USXGMII_AN_ENABLE, val)) {
++ /* Refresh LPA by inverting LPA_LATCH */
++ regmap_read(mpcs->regmap, RG_PCS_AN_STS0, &val);
++ regmap_update_bits(mpcs->regmap, RG_PCS_AN_STS0,
++ USXGMII_LPA_LATCH,
++ !(val & USXGMII_LPA_LATCH));
++
++ regmap_read(mpcs->regmap, RG_PCS_AN_STS0, &val);
++
++ state->interface = mpcs->interface;
++ state->link = FIELD_GET(USXGMII_LPA_LINK, val);
++ state->duplex = FIELD_GET(USXGMII_LPA_DUPLEX, val);
++
++ switch (FIELD_GET(USXGMII_LPA_SPEED_MASK, val)) {
++ case USXGMII_LPA_SPEED_10:
++ state->speed = SPEED_10;
++ break;
++ case USXGMII_LPA_SPEED_100:
++ state->speed = SPEED_100;
++ break;
++ case USXGMII_LPA_SPEED_1000:
++ state->speed = SPEED_1000;
++ break;
++ case USXGMII_LPA_SPEED_2500:
++ state->speed = SPEED_2500;
++ break;
++ case USXGMII_LPA_SPEED_5000:
++ state->speed = SPEED_5000;
++ break;
++ case USXGMII_LPA_SPEED_10000:
++ state->speed = SPEED_10000;
++ break;
++ }
++ } else {
++ val = mtk_r32(mac->hw, MTK_XGMAC_STS(mac->id));
++
++ if (mac->id == MTK_GMAC2_ID)
++ val = val >> 16;
++
++ switch (FIELD_GET(MTK_USXGMII_PCS_MODE, val)) {
++ case 0:
++ state->speed = SPEED_10000;
++ break;
++ case 1:
++ state->speed = SPEED_5000;
++ break;
++ case 2:
++ state->speed = SPEED_2500;
++ break;
++ case 3:
++ state->speed = SPEED_1000;
++ break;
++ }
++
++ state->interface = mpcs->interface;
++ state->link = FIELD_GET(MTK_USXGMII_PCS_LINK, val);
++ state->duplex = DUPLEX_FULL;
++ }
++
++ if (state->link == 0)
++ mtk_usxgmii_pcs_config(pcs, MLO_AN_INBAND,
++ state->interface, NULL, false);