net: dsa: b53: Add SerDes support

Add support for the Northstar Plus SerDes which is accessed through a
special page of the switch. Since this is something that most people
probably will not want to use, make it a configurable option with a
default on ARCH_BCM_NSP where it is the most useful currently.

The SerDes supports both SGMII and 1000baseX modes for both lanes, and
2500baseX for one of the lanes, and is internally looking like a
seemingly standard MII PHY, except for the few bits that got repurposed.
Signed-off-by: Florian Fainelli <f.fainelli@gmail.com>
Reviewed-by: Andrew Lunn <andrew@lunn.ch>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/dsa/b53/Kconfig

@@ -35,3 +35,10 @@ config B53_SRAB_DRIVER
 	help
 	  Select to enable support for memory-mapped Switch Register Access
 	  Bridge Registers (SRAB) like it is found on the BCM53010
+
+config B53_SERDES
+	tristate "B53 SerDes support"
+	depends on B53
+	default ARCH_BCM_NSP
+	help
+	  Select to enable support for SerDes on e.g: Northstar Plus SoCs.

drivers/net/dsa/b53/Makefile

@@ -5,3 +5,4 @@ obj-$(CONFIG_B53_SPI_DRIVER)	+= b53_spi.o
 obj-$(CONFIG_B53_MDIO_DRIVER)	+= b53_mdio.o
 obj-$(CONFIG_B53_MMAP_DRIVER)	+= b53_mmap.o
 obj-$(CONFIG_B53_SRAB_DRIVER)	+= b53_srab.o
+obj-$(CONFIG_B53_SERDES)	+= b53_serdes.o

drivers/net/dsa/b53/b53_common.c

@@ -765,6 +765,8 @@ static int b53_reset_switch(struct b53_device *priv)
 	memset(priv->vlans, 0, sizeof(*priv->vlans) * priv->num_vlans);
 	memset(priv->ports, 0, sizeof(*priv->ports) * priv->num_ports);
 
+	priv->serdes_lane = B53_INVALID_LANE;
+
 	return b53_switch_reset(priv);
 }
 
@@ -1128,6 +1130,9 @@ void b53_phylink_validate(struct dsa_switch *ds, int port,
 	struct b53_device *dev = ds->priv;
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
 
+	if (dev->ops->serdes_phylink_validate)
+		dev->ops->serdes_phylink_validate(dev, port, mask, state);
+
 	/* Allow all the expected bits */
 	phylink_set(mask, Autoneg);
 	phylink_set_port_modes(mask);
@@ -1164,8 +1169,13 @@ EXPORT_SYMBOL(b53_phylink_validate);
 int b53_phylink_mac_link_state(struct dsa_switch *ds, int port,
 			       struct phylink_link_state *state)
 {
+	struct b53_device *dev = ds->priv;
 	int ret = -EOPNOTSUPP;
 
+	if (phy_interface_mode_is_8023z(state->interface) &&
+	    dev->ops->serdes_link_state)
+		ret = dev->ops->serdes_link_state(dev, port, state);
+
 	return ret;
 }
 EXPORT_SYMBOL(b53_phylink_mac_link_state);
@@ -1184,11 +1194,19 @@ void b53_phylink_mac_config(struct dsa_switch *ds, int port,
 				      state->duplex, state->pause);
 		return;
 	}
+
+	if (phy_interface_mode_is_8023z(state->interface) &&
+	    dev->ops->serdes_config)
+		dev->ops->serdes_config(dev, port, mode, state);
 }
 EXPORT_SYMBOL(b53_phylink_mac_config);
 
 void b53_phylink_mac_an_restart(struct dsa_switch *ds, int port)
 {
+	struct b53_device *dev = ds->priv;
+
+	if (dev->ops->serdes_an_restart)
+		dev->ops->serdes_an_restart(dev, port);
 }
 EXPORT_SYMBOL(b53_phylink_mac_an_restart);
 
@@ -1205,6 +1223,10 @@ void b53_phylink_mac_link_down(struct dsa_switch *ds, int port,
 		b53_force_link(dev, port, false);
 		return;
 	}
+
+	if (phy_interface_mode_is_8023z(interface) &&
+	    dev->ops->serdes_link_set)
+		dev->ops->serdes_link_set(dev, port, mode, interface, false);
 }
 EXPORT_SYMBOL(b53_phylink_mac_link_down);
 
@@ -1222,6 +1244,10 @@ void b53_phylink_mac_link_up(struct dsa_switch *ds, int port,
 		b53_force_link(dev, port, true);
 		return;
 	}
+
+	if (phy_interface_mode_is_8023z(interface) &&
+	    dev->ops->serdes_link_set)
+		dev->ops->serdes_link_set(dev, port, mode, interface, true);
 }
 EXPORT_SYMBOL(b53_phylink_mac_link_up);
 

drivers/net/dsa/b53/b53_priv.h

@@ -29,6 +29,7 @@
 
 struct b53_device;
 struct net_device;
+struct phylink_link_state;
 
 struct b53_io_ops {
 	int (*read8)(struct b53_device *dev, u8 page, u8 reg, u8 *value);
@@ -45,8 +46,23 @@ struct b53_io_ops {
 	int (*phy_write16)(struct b53_device *dev, int addr, int reg, u16 value);
 	int (*irq_enable)(struct b53_device *dev, int port);
 	void (*irq_disable)(struct b53_device *dev, int port);
+	u8 (*serdes_map_lane)(struct b53_device *dev, int port);
+	int (*serdes_link_state)(struct b53_device *dev, int port,
+				 struct phylink_link_state *state);
+	void (*serdes_config)(struct b53_device *dev, int port,
+			      unsigned int mode,
+			      const struct phylink_link_state *state);
+	void (*serdes_an_restart)(struct b53_device *dev, int port);
+	void (*serdes_link_set)(struct b53_device *dev, int port,
+				unsigned int mode, phy_interface_t interface,
+				bool link_up);
+	void (*serdes_phylink_validate)(struct b53_device *dev, int port,
+					unsigned long *supported,
+					struct phylink_link_state *state);
 };
 
+#define B53_INVALID_LANE	0xff
+
 enum {
 	BCM5325_DEVICE_ID = 0x25,
 	BCM5365_DEVICE_ID = 0x65,
@@ -109,6 +125,7 @@ struct b53_device {
 	/* connect specific data */
 	u8 current_page;
 	struct device *dev;
+	u8 serdes_lane;
 
 	/* Master MDIO bus we got probed from */
 	struct mii_bus *bus;

drivers/net/dsa/b53/b53_serdes.c

+// SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause
+/*
+ * Northstar Plus switch SerDes/SGMII PHY main logic
+ *
+ * Copyright (C) 2018 Florian Fainelli <f.fainelli@gmail.com>
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/phy.h>
+#include <linux/phylink.h>
+#include <net/dsa.h>
+
+#include "b53_priv.h"
+#include "b53_serdes.h"
+#include "b53_regs.h"
+
+static void b53_serdes_write_blk(struct b53_device *dev, u8 offset, u16 block,
+				 u16 value)
+{
+	b53_write16(dev, B53_SERDES_PAGE, B53_SERDES_BLKADDR, block);
+	b53_write16(dev, B53_SERDES_PAGE, offset, value);
+}
+
+static u16 b53_serdes_read_blk(struct b53_device *dev, u8 offset, u16 block)
+{
+	u16 value;
+
+	b53_write16(dev, B53_SERDES_PAGE, B53_SERDES_BLKADDR, block);
+	b53_read16(dev, B53_SERDES_PAGE, offset, &value);
+
+	return value;
+}
+
+static void b53_serdes_set_lane(struct b53_device *dev, u8 lane)
+{
+	if (dev->serdes_lane == lane)
+		return;
+
+	WARN_ON(lane > 1);
+
+	b53_serdes_write_blk(dev, B53_SERDES_LANE,
+			     SERDES_XGXSBLK0_BLOCKADDRESS, lane);
+	dev->serdes_lane = lane;
+}
+
+static void b53_serdes_write(struct b53_device *dev, u8 lane,
+			     u8 offset, u16 block, u16 value)
+{
+	b53_serdes_set_lane(dev, lane);
+	b53_serdes_write_blk(dev, offset, block, value);
+}
+
+static u16 b53_serdes_read(struct b53_device *dev, u8 lane,
+			   u8 offset, u16 block)
+{
+	b53_serdes_set_lane(dev, lane);
+	return b53_serdes_read_blk(dev, offset, block);
+}
+
+void b53_serdes_config(struct b53_device *dev, int port, unsigned int mode,
+		       const struct phylink_link_state *state)
+{
+	u8 lane = b53_serdes_map_lane(dev, port);
+	u16 reg;
+
+	if (lane == B53_INVALID_LANE)
+		return;
+
+	reg = b53_serdes_read(dev, lane, B53_SERDES_DIGITAL_CONTROL(1),
+			      SERDES_DIGITAL_BLK);
+	if (state->interface == PHY_INTERFACE_MODE_1000BASEX)
+		reg |= FIBER_MODE_1000X;
+	else
+		reg &= ~FIBER_MODE_1000X;
+	b53_serdes_write(dev, lane, B53_SERDES_DIGITAL_CONTROL(1),
+			 SERDES_DIGITAL_BLK, reg);
+}
+EXPORT_SYMBOL(b53_serdes_config);
+
+void b53_serdes_an_restart(struct b53_device *dev, int port)
+{
+	u8 lane = b53_serdes_map_lane(dev, port);
+	u16 reg;
+
+	if (lane == B53_INVALID_LANE)
+		return;
+
+	reg = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
+			      SERDES_MII_BLK);
+	reg |= BMCR_ANRESTART;
+	b53_serdes_write(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
+			 SERDES_MII_BLK, reg);
+}
+EXPORT_SYMBOL(b53_serdes_an_restart);
+
+int b53_serdes_link_state(struct b53_device *dev, int port,
+			  struct phylink_link_state *state)
+{
+	u8 lane = b53_serdes_map_lane(dev, port);
+	u16 dig, bmcr, bmsr;
+
+	if (lane == B53_INVALID_LANE)
+		return 1;
+
+	dig = b53_serdes_read(dev, lane, B53_SERDES_DIGITAL_STATUS,
+			      SERDES_DIGITAL_BLK);
+	bmcr = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
+			       SERDES_MII_BLK);
+	bmsr = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_BMSR),
+			       SERDES_MII_BLK);
+
+	switch ((dig >> SPEED_STATUS_SHIFT) & SPEED_STATUS_MASK) {
+	case SPEED_STATUS_10:
+		state->speed = SPEED_10;
+		break;
+	case SPEED_STATUS_100:
+		state->speed = SPEED_100;
+		break;
+	case SPEED_STATUS_1000:
+		state->speed = SPEED_1000;
+		break;
+	default:
+	case SPEED_STATUS_2500:
+		state->speed = SPEED_2500;
+		break;
+	}
+
+	state->duplex = dig & DUPLEX_STATUS ? DUPLEX_FULL : DUPLEX_HALF;
+	state->an_enabled = !!(bmcr & BMCR_ANENABLE);
+	state->an_complete = !!(bmsr & BMSR_ANEGCOMPLETE);
+	state->link = !!(dig & LINK_STATUS);
+	if (dig & PAUSE_RESOLUTION_RX_SIDE)
+		state->pause |= MLO_PAUSE_RX;
+	if (dig & PAUSE_RESOLUTION_TX_SIDE)
+		state->pause |= MLO_PAUSE_TX;
+
+	return 0;
+}
+EXPORT_SYMBOL(b53_serdes_link_state);
+
+void b53_serdes_link_set(struct b53_device *dev, int port, unsigned int mode,
+			 phy_interface_t interface, bool link_up)
+{
+	u8 lane = b53_serdes_map_lane(dev, port);
+	u16 reg;
+
+	if (lane == B53_INVALID_LANE)
+		return;
+
+	reg = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
+			      SERDES_MII_BLK);
+	if (link_up)
+		reg &= ~BMCR_PDOWN;
+	else
+		reg |= BMCR_PDOWN;
+	b53_serdes_write(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
+			 SERDES_MII_BLK, reg);
+}
+EXPORT_SYMBOL(b53_serdes_link_set);
+
+void b53_serdes_phylink_validate(struct b53_device *dev, int port,
+				 unsigned long *supported,
+				 struct phylink_link_state *state)
+{
+	u8 lane = b53_serdes_map_lane(dev, port);
+
+	if (lane == B53_INVALID_LANE)
+		return;
+
+	switch (lane) {
+	case 0:
+		phylink_set(supported, 2500baseX_Full);
+		/* fallthrough */
+	case 1:
+		phylink_set(supported, 1000baseX_Full);
+		break;
+	default:
+		break;
+	}
+}
+EXPORT_SYMBOL(b53_serdes_phylink_validate);
+
+int b53_serdes_init(struct b53_device *dev, int port)
+{
+	u8 lane = b53_serdes_map_lane(dev, port);
+	u16 id0, msb, lsb;
+
+	if (lane == B53_INVALID_LANE)
+		return -EINVAL;
+
+	id0 = b53_serdes_read(dev, lane, B53_SERDES_ID0, SERDES_ID0);
+	msb = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_PHYSID1),
+			      SERDES_MII_BLK);
+	lsb = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_PHYSID2),
+			      SERDES_MII_BLK);
+	if (id0 == 0 || id0 == 0xffff) {
+		dev_err(dev->dev, "SerDes not initialized, check settings\n");
+		return -ENODEV;
+	}
+
+	dev_info(dev->dev,
+		 "SerDes lane %d, model: %d, rev %c%d (OUI: 0x%08x)\n",
+		 lane, id0 & SERDES_ID0_MODEL_MASK,
+		 (id0 >> SERDES_ID0_REV_LETTER_SHIFT) + 0x41,
+		 (id0 >> SERDES_ID0_REV_NUM_SHIFT) & SERDES_ID0_REV_NUM_MASK,
+		 (u32)msb << 16 | lsb);
+
+	return 0;
+}
+EXPORT_SYMBOL(b53_serdes_init);
+
+MODULE_AUTHOR("Florian Fainelli <f.fainelli@gmail.com>");
+MODULE_DESCRIPTION("B53 Switch SerDes driver");
+MODULE_LICENSE("Dual BSD/GPL");

drivers/net/dsa/b53/b53_serdes.h

+/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause
+ *
+ * Northstar Plus switch SerDes/SGMII PHY definitions
+ *
+ * Copyright (C) 2018 Florian Fainelli <f.fainelli@gmail.com>
+ */
+
+#include <linux/phy.h>
+#include <linux/types.h>
+
+/* Non-standard page used to access SerDes PHY registers on NorthStar Plus */
+#define B53_SERDES_PAGE			0x16
+#define B53_SERDES_BLKADDR		0x3e
+#define B53_SERDES_LANE			0x3c
+
+#define B53_SERDES_ID0			0x20
+#define  SERDES_ID0_MODEL_MASK		0x3f
+#define  SERDES_ID0_REV_NUM_SHIFT	11
+#define  SERDES_ID0_REV_NUM_MASK	0x7
+#define  SERDES_ID0_REV_LETTER_SHIFT	14
+
+#define B53_SERDES_MII_REG(x)		(0x20 + (x) * 2)
+#define B53_SERDES_DIGITAL_CONTROL(x)	(0x18 + (x) * 2)
+#define B53_SERDES_DIGITAL_STATUS	0x28
+
+/* SERDES_DIGITAL_CONTROL1 */
+#define  FIBER_MODE_1000X		BIT(0)
+#define  TBI_INTERFACE			BIT(1)
+#define  SIGNAL_DETECT_EN		BIT(2)
+#define  INVERT_SIGNAL_DETECT		BIT(3)
+#define  AUTODET_EN			BIT(4)
+#define  SGMII_MASTER_MODE		BIT(5)
+#define  DISABLE_DLL_PWRDOWN		BIT(6)
+#define  CRC_CHECKER_DIS		BIT(7)
+#define  COMMA_DET_EN			BIT(8)
+#define  ZERO_COMMA_DET_EN		BIT(9)
+#define  REMOTE_LOOPBACK		BIT(10)
+#define  SEL_RX_PKTS_FOR_CNTR		BIT(11)
+#define  MASTER_MDIO_PHY_SEL		BIT(13)
+#define  DISABLE_SIGNAL_DETECT_FLT	BIT(14)
+
+/* SERDES_DIGITAL_CONTROL2 */
+#define  EN_PARALLEL_DET		BIT(0)
+#define  DIS_FALSE_LINK			BIT(1)
+#define  FLT_FORCE_LINK			BIT(2)
+#define  EN_AUTONEG_ERR_TIMER		BIT(3)
+#define  DIS_REMOTE_FAULT_SENSING	BIT(4)
+#define  FORCE_XMIT_DATA		BIT(5)
+#define  AUTONEG_FAST_TIMERS		BIT(6)
+#define  DIS_CARRIER_EXTEND		BIT(7)
+#define  DIS_TRRR_GENERATION		BIT(8)
+#define  BYPASS_PCS_RX			BIT(9)
+#define  BYPASS_PCS_TX			BIT(10)
+#define  TEST_CNTR_EN			BIT(11)
+#define  TX_PACKET_SEQ_TEST		BIT(12)
+#define  TX_IDLE_JAM_SEQ_TEST		BIT(13)
+#define  CLR_BER_CNTR			BIT(14)
+
+/* SERDES_DIGITAL_CONTROL3 */
+#define  TX_FIFO_RST			BIT(0)
+#define  FIFO_ELAST_TX_RX_SHIFT		1
+#define  FIFO_ELAST_TX_RX_5K		0
+#define  FIFO_ELAST_TX_RX_10K		1
+#define  FIFO_ELAST_TX_RX_13_5K		2
+#define  FIFO_ELAST_TX_RX_18_5K		3
+#define  BLOCK_TXEN_MODE		BIT(9)
+#define  JAM_FALSE_CARRIER_MODE		BIT(10)
+#define  EXT_PHY_CRS_MODE		BIT(11)
+#define  INVERT_EXT_PHY_CRS		BIT(12)
+#define  DISABLE_TX_CRS			BIT(13)
+
+/* SERDES_DIGITAL_STATUS */
+#define  SGMII_MODE			BIT(0)
+#define  LINK_STATUS			BIT(1)
+#define  DUPLEX_STATUS			BIT(2)
+#define  SPEED_STATUS_SHIFT		3
+#define  SPEED_STATUS_10		0
+#define  SPEED_STATUS_100		1
+#define  SPEED_STATUS_1000		2
+#define  SPEED_STATUS_2500		3
+#define  SPEED_STATUS_MASK		SPEED_STATUS_2500
+#define  PAUSE_RESOLUTION_TX_SIDE	BIT(5)
+#define  PAUSE_RESOLUTION_RX_SIDE	BIT(6)
+#define  LINK_STATUS_CHANGE		BIT(7)
+#define  EARLY_END_EXT_DET		BIT(8)
+#define  CARRIER_EXT_ERR_DET		BIT(9)
+#define  RX_ERR_DET			BIT(10)
+#define  TX_ERR_DET			BIT(11)
+#define  CRC_ERR_DET			BIT(12)
+#define  FALSE_CARRIER_ERR_DET		BIT(13)
+#define  RXFIFO_ERR_DET			BIT(14)
+#define  TXFIFO_ERR_DET			BIT(15)
+
+/* Block offsets */
+#define SERDES_DIGITAL_BLK		0x8300
+#define SERDES_ID0			0x8310
+#define SERDES_MII_BLK			0xffe0
+#define SERDES_XGXSBLK0_BLOCKADDRESS	0xffd0
+
+struct phylink_link_state;
+
+static inline u8 b53_serdes_map_lane(struct b53_device *dev, int port)
+{
+	if (!dev->ops->serdes_map_lane)
+		return B53_INVALID_LANE;
+
+	return dev->ops->serdes_map_lane(dev, port);
+}
+
+int b53_serdes_get_link(struct b53_device *dev, int port);
+int b53_serdes_link_state(struct b53_device *dev, int port,
+			  struct phylink_link_state *state);
+void b53_serdes_config(struct b53_device *dev, int port, unsigned int mode,
+		       const struct phylink_link_state *state);
+void b53_serdes_an_restart(struct b53_device *dev, int port);
+void b53_serdes_link_set(struct b53_device *dev, int port, unsigned int mode,
+			 phy_interface_t interface, bool link_up);
+void b53_serdes_phylink_validate(struct b53_device *dev, int port,
+				unsigned long *supported,
+				struct phylink_link_state *state);
+int b53_serdes_init(struct b53_device *dev, int port);

drivers/net/dsa/b53/b53_srab.c

@@ -25,6 +25,7 @@
 #include <linux/of.h>
 
 #include "b53_priv.h"
+#include "b53_serdes.h"
 
 /* command and status register of the SRAB */
 #define B53_SRAB_CMDSTAT		0x2c
@@ -62,15 +63,28 @@
 #define  B53_SRAB_P7_SLEEP_TIMER	BIT(11)
 #define  B53_SRAB_IMP0_SLEEP_TIMER	BIT(12)
 
+/* Port mux configuration registers */
+#define B53_MUX_CONFIG_P5		0x00
+#define  MUX_CONFIG_SGMII		0
+#define  MUX_CONFIG_MII_LITE		1
+#define  MUX_CONFIG_RGMII		2
+#define  MUX_CONFIG_GMII		3
+#define  MUX_CONFIG_GPHY		4
+#define  MUX_CONFIG_INTERNAL		5
+#define  MUX_CONFIG_MASK		0x7
+#define B53_MUX_CONFIG_P4		0x04
+
 struct b53_srab_port_priv {
 	int irq;
 	bool irq_enabled;
 	struct b53_device *dev;
 	unsigned int num;
+	phy_interface_t mode;
 };
 
 struct b53_srab_priv {
 	void __iomem *regs;
+	void __iomem *mux_config;
 	struct b53_srab_port_priv port_intrs[B53_N_PORTS];
 };
 
@@ -356,6 +370,11 @@ static int b53_srab_write64(struct b53_device *dev, u8 page, u8 reg,
 
 static irqreturn_t b53_srab_port_thread(int irq, void *dev_id)
 {
+	struct b53_srab_port_priv *port = dev_id;
+	struct b53_device *dev = port->dev;
+
+	b53_port_event(dev->ds, port->num);
+
 	return IRQ_HANDLED;
 }
 
@@ -371,6 +390,24 @@ static irqreturn_t b53_srab_port_isr(int irq, void *dev_id)
 	return IRQ_WAKE_THREAD;
 }
 
+static u8 b53_srab_serdes_map_lane(struct b53_device *dev, int port)
+{
+	struct b53_srab_priv *priv = dev->priv;
+	struct b53_srab_port_priv *p = &priv->port_intrs[port];
+
+	if (p->mode != PHY_INTERFACE_MODE_SGMII)
+		return B53_INVALID_LANE;
+
+	switch (port) {
+	case 5:
+		return 0;
+	case 4:
+		return 1;
+	default:
+		return B53_INVALID_LANE;
+	}
+}
+
 static int b53_srab_irq_enable(struct b53_device *dev, int port)
 {
 	struct b53_srab_priv *priv = dev->priv;
@@ -410,6 +447,14 @@ static const struct b53_io_ops b53_srab_ops = {
 	.write64 = b53_srab_write64,
 	.irq_enable = b53_srab_irq_enable,
 	.irq_disable = b53_srab_irq_disable,
+#if IS_ENABLED(CONFIG_B53_SERDES)
+	.serdes_map_lane = b53_srab_serdes_map_lane,
+	.serdes_link_state = b53_serdes_link_state,
+	.serdes_config = b53_serdes_config,
+	.serdes_an_restart = b53_serdes_an_restart,
+	.serdes_link_set = b53_serdes_link_set,
+	.serdes_phylink_validate = b53_serdes_phylink_validate,
+#endif
 };
 
 static const struct of_device_id b53_srab_of_match[] = {
@@ -480,6 +525,61 @@ static void b53_srab_prepare_irq(struct platform_device *pdev)
 	b53_srab_intr_set(priv, true);
 }
 
+static void b53_srab_mux_init(struct platform_device *pdev)
+{
+	struct b53_device *dev = platform_get_drvdata(pdev);
+	struct b53_srab_priv *priv = dev->priv;
+	struct b53_srab_port_priv *p;
+	struct resource *r;
+	unsigned int port;
+	u32 reg, off = 0;
+	int ret;
+
+	if (dev->pdata && dev->pdata->chip_id != BCM58XX_DEVICE_ID)
+		return;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	priv->mux_config = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(priv->mux_config))
+		return;
+
+	/* Obtain the port mux configuration so we know which lanes
+	 * actually map to SerDes lanes
+	 */
+	for (port = 5; port > 3; port--, off += 4) {
+		p = &priv->port_intrs[port];
+
+		reg = readl(priv->mux_config + B53_MUX_CONFIG_P5 + off);
+		switch (reg & MUX_CONFIG_MASK) {
+		case MUX_CONFIG_SGMII:
+			p->mode = PHY_INTERFACE_MODE_SGMII;
+			ret = b53_serdes_init(dev, port);
+			if (ret)
+				continue;
+			break;
+		case MUX_CONFIG_MII_LITE:
+			p->mode = PHY_INTERFACE_MODE_MII;
+			break;
+		case MUX_CONFIG_GMII:
+			p->mode = PHY_INTERFACE_MODE_GMII;
+			break;
+		case MUX_CONFIG_RGMII:
+			p->mode = PHY_INTERFACE_MODE_RGMII;
+			break;
+		case MUX_CONFIG_INTERNAL:
+			p->mode = PHY_INTERFACE_MODE_INTERNAL;
+			break;
+		default:
+			p->mode = PHY_INTERFACE_MODE_NA;
+			break;
+		}
+
+		if (p->mode != PHY_INTERFACE_MODE_NA)
+			dev_info(&pdev->dev, "Port %d mode: %s\n",
+				 port, phy_modes(p->mode));
+	}
+}
+
 static int b53_srab_probe(struct platform_device *pdev)
 {
 	struct b53_platform_data *pdata = pdev->dev.platform_data;
@@ -519,6 +619,7 @@ static int b53_srab_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, dev);
 
 	b53_srab_prepare_irq(pdev);
+	b53_srab_mux_init(pdev);
 
 	return b53_switch_register(dev);
 }