3 files changed, 43 insertions, 2 deletions

diff --git a/drivers/net/fm/eth.c b/drivers/net/fm/eth.c
index 422c2c691e..cb099cd849 100644
--- a/drivers/net/fm/eth.c
+++ b/drivers/net/fm/eth.c
@@ -341,7 +341,9 @@ static int fm_eth_startup(struct fm_eth *fm_eth)
 	mac->init_mac(mac);
 
 	/* For some reason we need to set SPEED_100 */
-	if ((fm_eth->enet_if == PHY_INTERFACE_MODE_SGMII) && mac->set_if_mode)
+	if (((fm_eth->enet_if == PHY_INTERFACE_MODE_SGMII) ||
+	     (fm_eth->enet_if == PHY_INTERFACE_MODE_QSGMII)) &&
+	      mac->set_if_mode)
 		mac->set_if_mode(mac, fm_eth->enet_if, SPEED_100);
 
 	/* init bmi rx port, IM mode and disable */
diff --git a/drivers/net/fm/memac.c b/drivers/net/fm/memac.c
index 144e109fd0..592a67f2a5 100644
--- a/drivers/net/fm/memac.c
+++ b/drivers/net/fm/memac.c
@@ -90,6 +90,7 @@ static void memac_set_interface_mode(struct fsl_enet_mac *mac,
 		if_mode |= (IF_MODE_GMII | IF_MODE_RM);
 		break;
 	case PHY_INTERFACE_MODE_SGMII:
+	case PHY_INTERFACE_MODE_QSGMII:
 		if_mode &= ~IF_MODE_MASK;
 		if_mode |= (IF_MODE_GMII);
 		break;
diff --git a/drivers/net/fm/t4240.c b/drivers/net/fm/t4240.c
index 10c141fa26..6253f22f75 100644
--- a/drivers/net/fm/t4240.c
+++ b/drivers/net/fm/t4240.c
@@ -114,7 +114,45 @@ phy_interface_t fman_port_enet_if(enum fm_port port)
 			return PHY_INTERFACE_MODE_SGMII;
 		break;
 	default:
-		return PHY_INTERFACE_MODE_NONE;
+		break;
+	}
+
+	/* handle QSGMII */
+	switch (port) {
+	case FM1_DTSEC1:
+	case FM1_DTSEC2:
+	case FM1_DTSEC3:
+	case FM1_DTSEC4:
+		/* check lane G on SerDes1 */
+		if (is_serdes_configured(QSGMII_FM1_A))
+			return PHY_INTERFACE_MODE_QSGMII;
+		break;
+	case FM1_DTSEC5:
+	case FM1_DTSEC6:
+	case FM1_DTSEC9:
+	case FM1_DTSEC10:
+		/* check lane C on SerDes1 */
+		if (is_serdes_configured(QSGMII_FM1_B))
+			return PHY_INTERFACE_MODE_QSGMII;
+		break;
+	case FM2_DTSEC1:
+	case FM2_DTSEC2:
+	case FM2_DTSEC3:
+	case FM2_DTSEC4:
+		/* check lane G on SerDes2 */
+		if (is_serdes_configured(QSGMII_FM2_A))
+			return PHY_INTERFACE_MODE_QSGMII;
+		break;
+	case FM2_DTSEC5:
+	case FM2_DTSEC6:
+	case FM2_DTSEC9:
+	case FM2_DTSEC10:
+		/* check lane C on SerDes2 */
+		if (is_serdes_configured(QSGMII_FM2_B))
+			return PHY_INTERFACE_MODE_QSGMII;
+		break;
+	default:
+		break;
 	}
 
 	return PHY_INTERFACE_MODE_NONE;