Merge branch 'i2c/for-current' of git://git.kernel.org/pub/scm/linux/kernel/git/wsa...

[karo-tx-linux.git] / drivers / net / ethernet / cavium / thunder / thunder_bgx.c

diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
index 79112563a25ae08db35bfb2de214c2b3e84a15a6..5e5c4d7796b8882168eba6fe858ac4bfff2aaf53 100644 (file)
--- a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
@@ -292,11 +292,30 @@ static void bgx_sgmii_change_link_state(struct lmac *lmac)
        u64 cmr_cfg;
        u64 port_cfg = 0;
        u64 misc_ctl = 0;
+       bool tx_en, rx_en;
 
        cmr_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_CMRX_CFG);
-       cmr_cfg &= ~CMR_EN;
+       tx_en = cmr_cfg & CMR_PKT_TX_EN;
+       rx_en = cmr_cfg & CMR_PKT_RX_EN;
+       cmr_cfg &= ~(CMR_PKT_RX_EN | CMR_PKT_TX_EN);
        bgx_reg_write(bgx, lmac->lmacid, BGX_CMRX_CFG, cmr_cfg);
 
+       /* Wait for BGX RX to be idle */
+       if (bgx_poll_reg(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG,
+                        GMI_PORT_CFG_RX_IDLE, false)) {
+               dev_err(&bgx->pdev->dev, "BGX%d LMAC%d GMI RX not idle\n",
+                       bgx->bgx_id, lmac->lmacid);
+               return;
+       }
+
+       /* Wait for BGX TX to be idle */
+       if (bgx_poll_reg(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG,
+                        GMI_PORT_CFG_TX_IDLE, false)) {
+               dev_err(&bgx->pdev->dev, "BGX%d LMAC%d GMI TX not idle\n",
+                       bgx->bgx_id, lmac->lmacid);
+               return;
+       }
+
        port_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG);
        misc_ctl = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_PCS_MISCX_CTL);
 
@@ -347,10 +366,8 @@ static void bgx_sgmii_change_link_state(struct lmac *lmac)
        bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_PCS_MISCX_CTL, misc_ctl);
        bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG, port_cfg);
 
-       port_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG);
-
-       /* Re-enable lmac */
-       cmr_cfg |= CMR_EN;
+       /* Restore CMR config settings */
+       cmr_cfg |= (rx_en ? CMR_PKT_RX_EN : 0) | (tx_en ? CMR_PKT_TX_EN : 0);
        bgx_reg_write(bgx, lmac->lmacid, BGX_CMRX_CFG, cmr_cfg);
 
        if (bgx->is_rgx && (cmr_cfg & (CMR_PKT_RX_EN | CMR_PKT_TX_EN)))