From: Jonas Jelonek <jelonek.jonas@gmail.com>
Date: Wed, 8 Oct 2025 11:11:10 +0000 (+0000)
Subject: realtek: pcs: rtl931x: import SerDes setup code from PHY driver
X-Git-Url: http://git.ipfire.org/gitweb.cgi?a=commitdiff_plain;h=ddf94f7489;p=thirdparty%2Fopenwrt.git

realtek: pcs: rtl931x: import SerDes setup code from PHY driver

Let's start this transition with RTL931X.

Import all functions starting with 'rtl931x_' or 'rtsds_931x' from PHY
driver into the PCS driver, rename all functions to match a common
naming scheme and adjust signature, helper calls and function calls
accordingly to make it work within the PCS driver.

This is just copy&paste and tries to do only mandatory adjustments. The
code will be refactored in succeeding commits.

Also remove 'unused' attribute from helpers as they are used now.

Signed-off-by: Jonas Jelonek <jelonek.jonas@gmail.com>
Link: https://github.com/openwrt/openwrt/pull/20369
Signed-off-by: Hauke Mehrtens <hauke@hauke-m.de>
---

diff --git a/target/linux/realtek/files-6.12/drivers/net/pcs/pcs-rtl-otto.c b/target/linux/realtek/files-6.12/drivers/net/pcs/pcs-rtl-otto.c
index 594bdecd61c..9e6768706b0 100644
--- a/target/linux/realtek/files-6.12/drivers/net/pcs/pcs-rtl-otto.c
+++ b/target/linux/realtek/files-6.12/drivers/net/pcs/pcs-rtl-otto.c
@@ -10,6 +10,8 @@
 #include <linux/phylink.h>
 #include <linux/regmap.h>
 
+#include <asm/mach-rtl838x/mach-rtl83xx.h>
+
 #define RTPCS_PORT_CNT				57
 
 #define RTPCS_SPEED_10				0
@@ -52,6 +54,11 @@
 
 #define RTPCS_93XX_MAC_LINK_SPD_BITS		4
 
+/* Registers of the internal SerDes of the 9310 */
+#define RTL931X_SERDES_MODE_CTRL		(0x13cc)
+#define RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR	(0x13F4)
+#define RTL931X_MAC_SERDES_MODE_CTRL(sds)	(0x136C + (((sds) << 2)))
+
 struct rtpcs_ctrl {
 	struct device *dev;
 	struct regmap *map;
@@ -81,6 +88,12 @@ struct rtpcs_config {
 	int (*setup_serdes)(struct rtpcs_ctrl *ctrl, int sds, phy_interface_t mode);
 };
 
+typedef struct {
+	u8 page;
+	u8 reg;
+	u16 data;
+} sds_config;
+
 static int rtpcs_sds_to_mmd(int sds_page, int sds_regnum)
 {
 	return (sds_page << 8) + sds_regnum;
@@ -93,7 +106,6 @@ static int rtpcs_sds_read(struct rtpcs_ctrl *ctrl, int sds, int page, int regnum
 	return mdiobus_c45_read(ctrl->bus, sds, MDIO_MMD_VEND1, mmd_regnum);
 }
 
-__attribute__((unused))
 static int rtpcs_sds_read_bits(struct rtpcs_ctrl *ctrl, int sds, int page,
 			       int regnum, int bithigh, int bitlow)
 {
@@ -109,7 +121,6 @@ static int rtpcs_sds_read_bits(struct rtpcs_ctrl *ctrl, int sds, int page,
 	return (val & mask) >> bitlow;
 }
 
-__attribute__((unused))
 static int rtpcs_sds_write(struct rtpcs_ctrl *ctrl, int sds, int page, int regnum, u16 value)
 {
 	int mmd_regnum = rtpcs_sds_to_mmd(page, regnum);
@@ -117,7 +128,6 @@ static int rtpcs_sds_write(struct rtpcs_ctrl *ctrl, int sds, int page, int regnu
 	return mdiobus_c45_write(ctrl->bus, sds, MDIO_MMD_VEND1, mmd_regnum, value);
 }
 
-__attribute__((unused))
 static int rtpcs_sds_write_bits(struct rtpcs_ctrl *ctrl, int sds, int page,
 				int regnum, int bithigh, int bitlow, u16 value)
 {
@@ -163,6 +173,573 @@ static struct rtpcs_link *rtpcs_phylink_pcs_to_link(struct phylink_pcs *pcs)
 	return container_of(pcs, struct rtpcs_link, pcs);
 }
 
+/* Variant-specific functions */
+
+/* RTL931X */
+
+static void rtpcs_931x_sds_reset(u32 sds)
+{
+	u32 o, v, o_mode;
+	int shift = ((sds & 0x3) << 3);
+
+	/* TODO: We need to lock this! */
+
+	o = sw_r32(RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR);
+	v = o | BIT(sds);
+	sw_w32(v, RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR);
+
+	o_mode = sw_r32(RTL931X_SERDES_MODE_CTRL + 4 * (sds >> 2));
+	v = BIT(7) | 0x1F;
+	sw_w32_mask(0xff << shift, v << shift, RTL931X_SERDES_MODE_CTRL + 4 * (sds >> 2));
+	sw_w32(o_mode, RTL931X_SERDES_MODE_CTRL + 4 * (sds >> 2));
+
+	sw_w32(o, RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR);
+}
+
+static void rtpcs_931x_sds_symerr_clear(struct rtpcs_ctrl *ctrl, u32 sds,
+					phy_interface_t mode)
+{
+
+	switch (mode) {
+	case PHY_INTERFACE_MODE_NA:
+		break;
+	case PHY_INTERFACE_MODE_XGMII:
+		for (int i = 0; i < 4; ++i) {
+			rtpcs_sds_write_bits(ctrl, sds, 0x101, 24,  2, 0, i);
+			rtpcs_sds_write_bits(ctrl, sds, 0x101,  3, 15, 8, 0x0);
+			rtpcs_sds_write_bits(ctrl, sds, 0x101,  2, 15, 0, 0x0);
+		}
+
+		for (int i = 0; i < 4; ++i) {
+			rtpcs_sds_write_bits(ctrl, sds, 0x201, 24,  2, 0, i);
+			rtpcs_sds_write_bits(ctrl, sds, 0x201,  3, 15, 8, 0x0);
+			rtpcs_sds_write_bits(ctrl, sds, 0x201,  2, 15, 0, 0x0);
+		}
+
+		rtpcs_sds_write_bits(ctrl, sds, 0x101, 0, 15, 0, 0x0);
+		rtpcs_sds_write_bits(ctrl, sds, 0x101, 1, 15, 8, 0x0);
+		rtpcs_sds_write_bits(ctrl, sds, 0x201, 0, 15, 0, 0x0);
+		rtpcs_sds_write_bits(ctrl, sds, 0x201, 1, 15, 8, 0x0);
+		break;
+	default:
+		break;
+	}
+
+	return;
+}
+
+__attribute__((unused))
+static void rtpcs_931x_sds_fiber_disable(struct rtpcs_ctrl *ctrl, u32 sds)
+{
+	u32 v = 0x3F;
+
+	rtpcs_sds_write_bits(ctrl, sds, 0x1F, 0x9, 11, 6, v);
+}
+
+static void rtpcs_931x_sds_fiber_mode_set(struct rtpcs_ctrl *ctrl, u32 sds,
+					  phy_interface_t mode)
+{
+	u32 val;
+
+	/* clear symbol error count before changing mode */
+	rtpcs_931x_sds_symerr_clear(ctrl, sds, mode);
+
+	val = 0x9F;
+	sw_w32(val, RTL931X_SERDES_MODE_CTRL + 4 * (sds >> 2));
+
+	switch (mode) {
+	case PHY_INTERFACE_MODE_SGMII:
+		val = 0x5;
+		break;
+
+	case PHY_INTERFACE_MODE_1000BASEX:
+		/* serdes mode FIBER1G */
+		val = 0x9;
+		break;
+
+	case PHY_INTERFACE_MODE_10GBASER:
+	case PHY_INTERFACE_MODE_10GKR:
+		val = 0x35;
+		break;
+/*	case MII_10GR1000BX_AUTO:
+		val = 0x39;
+		break; */
+
+
+	case PHY_INTERFACE_MODE_USXGMII:
+		val = 0x1B;
+		break;
+	default:
+		val = 0x25;
+	}
+
+	pr_info("%s writing analog SerDes Mode value %02x\n", __func__, val);
+	rtpcs_sds_write_bits(ctrl, sds, 0x1F, 0x9, 11, 6, val);
+
+	return;
+}
+
+static int rtpcs_931x_sds_cmu_page_get(phy_interface_t mode)
+{
+	switch (mode) {
+	case PHY_INTERFACE_MODE_SGMII:
+	case PHY_INTERFACE_MODE_1000BASEX:	/* MII_1000BX_FIBER / 100BX_FIBER / 1000BX100BX_AUTO */
+		return 0x24;
+	case PHY_INTERFACE_MODE_2500BASEX:	/* MII_2500Base_X: */
+		return 0x28;
+/*	case MII_HISGMII_5G: */
+/*		return 0x2a; */
+	case PHY_INTERFACE_MODE_QSGMII:
+		return 0x2a;			/* Code also has 0x34 */
+	case PHY_INTERFACE_MODE_XAUI:		/* MII_RXAUI_LITE: */
+		return 0x2c;
+	case PHY_INTERFACE_MODE_XGMII:		/* MII_XSGMII */
+	case PHY_INTERFACE_MODE_10GKR:
+	case PHY_INTERFACE_MODE_10GBASER:	/* MII_10GR */
+		return 0x2e;
+	default:
+		return -1;
+	}
+
+	return -1;
+}
+
+static void rtpcs_931x_sds_cmu_type_set(struct rtpcs_ctrl *ctrl, u32 sds,
+					phy_interface_t mode, int chiptype)
+{
+	int cmu_type = 0; /* Clock Management Unit */
+	u32 cmu_page = 0;
+	u32 frc_cmu_spd;
+	u32 evenSds;
+	u32 lane, frc_lc_mode_bitnum, frc_lc_mode_val_bitnum;
+
+	switch (mode) {
+	case PHY_INTERFACE_MODE_NA:
+	case PHY_INTERFACE_MODE_10GKR:
+	case PHY_INTERFACE_MODE_XGMII:
+	case PHY_INTERFACE_MODE_10GBASER:
+	case PHY_INTERFACE_MODE_USXGMII:
+		return;
+
+/*	case MII_10GR1000BX_AUTO:
+		if (chiptype)
+			rtpcs_sds_write_bits(ctrl, sds, 0x24, 0xd, 14, 14, 0);
+		return; */
+
+	case PHY_INTERFACE_MODE_QSGMII:
+		cmu_type = 1;
+		frc_cmu_spd = 0;
+		break;
+
+	case PHY_INTERFACE_MODE_1000BASEX:
+		cmu_type = 1;
+		frc_cmu_spd = 0;
+		break;
+
+/*	case MII_1000BX100BX_AUTO:
+		cmu_type = 1;
+		frc_cmu_spd = 0;
+		break; */
+
+	case PHY_INTERFACE_MODE_SGMII:
+		cmu_type = 1;
+		frc_cmu_spd = 0;
+		break;
+
+	case PHY_INTERFACE_MODE_2500BASEX:
+		cmu_type = 1;
+		frc_cmu_spd = 1;
+		break;
+
+	default:
+		pr_info("SerDes %d mode is invalid\n", sds);
+		return;
+	}
+
+	if (cmu_type == 1)
+		cmu_page = rtpcs_931x_sds_cmu_page_get(mode);
+
+	lane = sds % 2;
+
+	if (!lane) {
+		frc_lc_mode_bitnum = 4;
+		frc_lc_mode_val_bitnum = 5;
+	} else {
+		frc_lc_mode_bitnum = 6;
+		frc_lc_mode_val_bitnum = 7;
+	}
+
+	evenSds = sds - lane;
+
+	pr_info("%s: cmu_type %0d cmu_page %x frc_cmu_spd %d lane %d sds %d\n",
+	        __func__, cmu_type, cmu_page, frc_cmu_spd, lane, sds);
+
+	if (cmu_type == 1) {
+		pr_info("%s A CMU page 0x28 0x7 %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x28, 0x7));
+		rtpcs_sds_write_bits(ctrl, sds, cmu_page, 0x7, 15, 15, 0);
+		pr_info("%s B CMU page 0x28 0x7 %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x28, 0x7));
+		if (chiptype) {
+			rtpcs_sds_write_bits(ctrl, sds, cmu_page, 0xd, 14, 14, 0);
+		}
+
+		rtpcs_sds_write_bits(ctrl, evenSds, 0x20, 0x12, 3, 2, 0x3);
+		rtpcs_sds_write_bits(ctrl, evenSds, 0x20, 0x12, frc_lc_mode_bitnum, frc_lc_mode_bitnum, 1);
+		rtpcs_sds_write_bits(ctrl, evenSds, 0x20, 0x12, frc_lc_mode_val_bitnum, frc_lc_mode_val_bitnum, 0);
+		rtpcs_sds_write_bits(ctrl, evenSds, 0x20, 0x12, 12, 12, 1);
+		rtpcs_sds_write_bits(ctrl, evenSds, 0x20, 0x12, 15, 13, frc_cmu_spd);
+	}
+
+	pr_info("%s CMU page 0x28 0x7 %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x28, 0x7));
+	return;
+}
+
+static void rtpcs_931x_sds_rx_reset(struct rtpcs_ctrl *ctrl, u32 sds)
+{
+	if (sds < 2)
+		return;
+
+	rtpcs_sds_write(ctrl, sds, 0x2e, 0x12, 0x2740);
+	rtpcs_sds_write(ctrl, sds, 0x2f, 0x0, 0x0);
+	rtpcs_sds_write(ctrl, sds, 0x2f, 0x2, 0x2010);
+	rtpcs_sds_write(ctrl, sds, 0x20, 0x0, 0xc10);
+
+	rtpcs_sds_write(ctrl, sds, 0x2e, 0x12, 0x27c0);
+	rtpcs_sds_write(ctrl, sds, 0x2f, 0x0, 0xc000);
+	rtpcs_sds_write(ctrl, sds, 0x2f, 0x2, 0x6010);
+	rtpcs_sds_write(ctrl, sds, 0x20, 0x0, 0xc30);
+
+	mdelay(50);
+}
+
+__attribute__((unused))
+static void rtpcs_931x_sds_disable(u32 sds)
+{
+	u32 v = 0x1f;
+
+	v |= BIT(7);
+	sw_w32(v, RTL931X_SERDES_MODE_CTRL + (sds >> 2) * 4);
+}
+
+static void rtpcs_931x_sds_mii_mode_set(u32 sds, phy_interface_t mode)
+{
+	u32 val;
+
+	switch (mode) {
+	case PHY_INTERFACE_MODE_QSGMII:
+		val = 0x6;
+		break;
+	case PHY_INTERFACE_MODE_XGMII:
+		val = 0x10; /* serdes mode XSGMII */
+		break;
+	case PHY_INTERFACE_MODE_USXGMII:
+	case PHY_INTERFACE_MODE_2500BASEX:
+		val = 0xD;
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		val = 0x2;
+		break;
+	default:
+		return;
+	}
+
+	val |= (1 << 7);
+
+	sw_w32(val, RTL931X_SERDES_MODE_CTRL + 4 * (sds >> 2));
+}
+
+static int rtpcs_931x_sds_cmu_band_set(struct rtpcs_ctrl *ctrl, int sds,
+				       bool enable, u32 band,
+				       phy_interface_t mode)
+{
+	int page = rtpcs_931x_sds_cmu_page_get(mode);
+
+	sds -= (sds % 2);
+	sds = sds & ~1;
+	page += 1;
+
+	if (enable) {
+		rtpcs_sds_write_bits(ctrl, sds, page, 0x7, 13, 13, 0);
+		rtpcs_sds_write_bits(ctrl, sds, page, 0x7, 11, 11, 0);
+	} else {
+		rtpcs_sds_write_bits(ctrl, sds, page, 0x7, 13, 13, 0);
+		rtpcs_sds_write_bits(ctrl, sds, page, 0x7, 11, 11, 0);
+	}
+
+	rtpcs_sds_write_bits(ctrl, sds, page, 0x7, 4, 0, band);
+
+	rtpcs_931x_sds_reset(sds);
+
+	return 0;
+}
+
+static int rtpcs_931x_sds_cmu_band_get(struct rtpcs_ctrl *ctrl, int sds,
+				       phy_interface_t mode)
+{
+	int page = rtpcs_931x_sds_cmu_page_get(mode);
+	u32 band;
+
+	sds -= (sds % 2);
+	page += 1;
+	rtpcs_sds_write(ctrl, sds, 0x1f, 0x02, 73);
+
+	rtpcs_sds_write_bits(ctrl, sds, page, 0x5, 15, 15, 1);
+	band = rtpcs_sds_read_bits(ctrl, sds, 0x1f, 0x15, 8, 3);
+	pr_info("%s band is: %d\n", __func__, band);
+
+	return band;
+}
+
+__attribute__((unused))
+static int rtpcs_931x_sds_link_sts_get(struct rtpcs_ctrl *ctrl, u32 sds)
+{
+	u32 sts, sts1, latch_sts, latch_sts1;
+	if (0){
+		sts = rtpcs_sds_read_bits(ctrl, sds, 0x101, 29, 8, 0);
+		sts1 = rtpcs_sds_read_bits(ctrl, sds, 0x201, 29, 8, 0);
+		latch_sts = rtpcs_sds_read_bits(ctrl, sds, 0x101, 30, 8, 0);
+		latch_sts1 = rtpcs_sds_read_bits(ctrl, sds, 0x201, 30, 8, 0);
+	} else {
+		sts = rtpcs_sds_read_bits(ctrl, sds, 0x5, 0, 12, 12);
+		latch_sts = rtpcs_sds_read_bits(ctrl, sds, 0x4, 1, 2, 2);
+		latch_sts1 = rtpcs_sds_read_bits(ctrl, sds, 0x102, 1, 2, 2);
+		sts1 = rtpcs_sds_read_bits(ctrl, sds, 0x102, 1, 2, 2);
+	}
+
+	pr_info("%s: serdes %d sts %d, sts1 %d, latch_sts %d, latch_sts1 %d\n", __func__,
+		sds, sts, sts1, latch_sts, latch_sts1);
+
+	return sts1;
+}
+
+static sds_config sds_config_10p3125g_type1[] = {
+	{ 0x2E, 0x00, 0x0107 }, { 0x2E, 0x01, 0x01A3 }, { 0x2E, 0x02, 0x6A24 },
+	{ 0x2E, 0x03, 0xD10D }, { 0x2E, 0x04, 0x8000 }, { 0x2E, 0x05, 0xA17E },
+	{ 0x2E, 0x06, 0xE31D }, { 0x2E, 0x07, 0x800E }, { 0x2E, 0x08, 0x0294 },
+	{ 0x2E, 0x09, 0x0CE4 }, { 0x2E, 0x0A, 0x7FC8 }, { 0x2E, 0x0B, 0xE0E7 },
+	{ 0x2E, 0x0C, 0x0200 }, { 0x2E, 0x0D, 0xDF80 }, { 0x2E, 0x0E, 0x0000 },
+	{ 0x2E, 0x0F, 0x1FC2 }, { 0x2E, 0x10, 0x0C3F }, { 0x2E, 0x11, 0x0000 },
+	{ 0x2E, 0x12, 0x27C0 }, { 0x2E, 0x13, 0x7E1D }, { 0x2E, 0x14, 0x1300 },
+	{ 0x2E, 0x15, 0x003F }, { 0x2E, 0x16, 0xBE7F }, { 0x2E, 0x17, 0x0090 },
+	{ 0x2E, 0x18, 0x0000 }, { 0x2E, 0x19, 0x4000 }, { 0x2E, 0x1A, 0x0000 },
+	{ 0x2E, 0x1B, 0x8000 }, { 0x2E, 0x1C, 0x011F }, { 0x2E, 0x1D, 0x0000 },
+	{ 0x2E, 0x1E, 0xC8FF }, { 0x2E, 0x1F, 0x0000 }, { 0x2F, 0x00, 0xC000 },
+	{ 0x2F, 0x01, 0xF000 }, { 0x2F, 0x02, 0x6010 }, { 0x2F, 0x12, 0x0EE7 },
+	{ 0x2F, 0x13, 0x0000 }
+};
+
+static sds_config sds_config_10p3125g_cmu_type1[] = {
+	{ 0x2F, 0x03, 0x4210 }, { 0x2F, 0x04, 0x0000 }, { 0x2F, 0x05, 0x0019 },
+	{ 0x2F, 0x06, 0x18A6 }, { 0x2F, 0x07, 0x2990 }, { 0x2F, 0x08, 0xFFF4 },
+	{ 0x2F, 0x09, 0x1F08 }, { 0x2F, 0x0A, 0x0000 }, { 0x2F, 0x0B, 0x8000 },
+	{ 0x2F, 0x0C, 0x4224 }, { 0x2F, 0x0D, 0x0000 }, { 0x2F, 0x0E, 0x0000 },
+	{ 0x2F, 0x0F, 0xA470 }, { 0x2F, 0x10, 0x8000 }, { 0x2F, 0x11, 0x037B }
+};
+
+__attribute__((unused))
+static int rtpcs_931x_setup_serdes(struct rtpcs_ctrl *ctrl, int sds,
+				   phy_interface_t mode)
+{
+	u32 board_sds_tx_type1[] = {
+		0x01c3, 0x01c3, 0x01c3, 0x01a3, 0x01a3, 0x01a3,
+		0x0143, 0x0143, 0x0143, 0x0143, 0x0163, 0x0163,
+	};
+	u32 board_sds_tx[] = {
+		0x1a00, 0x1a00, 0x0200, 0x0200, 0x0200, 0x0200,
+		0x01a3, 0x01a3, 0x01a3, 0x01a3, 0x01e3, 0x01e3
+	};
+	u32 board_sds_tx2[] = {
+		0x0dc0, 0x01c0, 0x0200, 0x0180, 0x0160, 0x0123,
+		0x0123, 0x0163, 0x01a3, 0x01a0, 0x01c3, 0x09c3,
+	};
+	u32 band, ori, model_info, val;
+	int chiptype = 0;
+
+	if (sds < 0 || sds > 13)
+		return -EINVAL;
+
+	pr_info("%s: set sds %d to mode %d\n", __func__, sds, mode);
+	val = rtpcs_sds_read_bits(ctrl, sds, 0x1F, 0x9, 11, 6);
+
+	pr_info("%s: fibermode %08X stored mode 0x%x", __func__,
+			rtpcs_sds_read(ctrl, sds, 0x1f, 0x9), val);
+	pr_info("%s: SGMII mode %08X in 0x24 0x9", __func__,
+			rtpcs_sds_read(ctrl, sds, 0x24, 0x9));
+	pr_info("%s: CMU mode %08X stored even SDS %d", __func__,
+			rtpcs_sds_read(ctrl, sds & ~1, 0x20, 0x12), sds & ~1);
+	pr_info("%s: serdes_mode_ctrl %08X", __func__,  RTL931X_SERDES_MODE_CTRL + 4 * (sds >> 2));
+	pr_info("%s CMU page 0x24 0x7 %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x24, 0x7));
+	pr_info("%s CMU page 0x26 0x7 %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x26, 0x7));
+	pr_info("%s CMU page 0x28 0x7 %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x28, 0x7));
+	pr_info("%s XSG page 0x0 0xe %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x100, 0xe));
+	pr_info("%s XSG2 page 0x0 0xe %08x\n", __func__, rtpcs_sds_read(ctrl, sds, 0x200, 0xe));
+
+	model_info = sw_r32(RTL93XX_MODEL_NAME_INFO);
+	if ((model_info >> 4) & 0x1) {
+		pr_info("detected chiptype 1\n");
+		chiptype = 1;
+	} else {
+		pr_info("detected chiptype 0\n");
+	}
+
+	pr_info("%s: 2.5gbit %08X", __func__,
+	        rtpcs_sds_read(ctrl, sds, 0x101, 0x14));
+
+	pr_info("%s: RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR 0x%08X\n", __func__, sw_r32(RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR));
+	ori = sw_r32(RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR);
+	val = ori | (1 << sds);
+	sw_w32(val, RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR);
+
+	/* this was in rtl931x_phylink_mac_config in dsa/rtl83xx/dsa.c before */
+	band = rtpcs_931x_sds_cmu_band_get(ctrl, sds, mode);
+
+	switch (mode) {
+	case PHY_INTERFACE_MODE_NA:
+		break;
+
+	case PHY_INTERFACE_MODE_XGMII: /* MII_XSGMII */
+
+		if (chiptype) {
+			/* fifo inv clk */
+			rtpcs_sds_write_bits(ctrl, sds, 0x101, 0x1, 7, 4, 0xf);
+			rtpcs_sds_write_bits(ctrl, sds, 0x101, 0x1, 3, 0, 0xf);
+
+			rtpcs_sds_write_bits(ctrl, sds, 0x201, 0x1, 7, 4, 0xf);
+			rtpcs_sds_write_bits(ctrl, sds, 0x201, 0x1, 3, 0, 0xf);
+
+		}
+
+		rtpcs_sds_write_bits(ctrl, sds, 0x100, 0xE, 12, 12, 1);
+		rtpcs_sds_write_bits(ctrl, sds, 0x200, 0xE, 12, 12, 1);
+		break;
+
+	case PHY_INTERFACE_MODE_USXGMII: /* MII_USXGMII_10GSXGMII/10GDXGMII/10GQXGMII: */
+		u32 op_code = 0x6003;
+		u32 evenSds;
+
+		if (chiptype) {
+			rtpcs_sds_write_bits(ctrl, sds, 0x6, 0x2, 12, 12, 1);
+
+			for (int i = 0; i < sizeof(sds_config_10p3125g_type1) / sizeof(sds_config); ++i) {
+				rtpcs_sds_write(ctrl, sds,
+						sds_config_10p3125g_type1[i].page - 0x4,
+						sds_config_10p3125g_type1[i].reg,
+						sds_config_10p3125g_type1[i].data);
+			}
+
+			evenSds = sds & ~1;
+
+			for (int i = 0; i < sizeof(sds_config_10p3125g_cmu_type1) / sizeof(sds_config); ++i) {
+				rtpcs_sds_write(ctrl, evenSds,
+						sds_config_10p3125g_cmu_type1[i].page - 0x4,
+						sds_config_10p3125g_cmu_type1[i].reg,
+						sds_config_10p3125g_cmu_type1[i].data);
+			}
+
+			rtpcs_sds_write_bits(ctrl, sds, 0x6, 0x2, 12, 12, 0);
+		} else {
+
+			rtpcs_sds_write_bits(ctrl, sds, 0x2e, 0xd, 6, 0, 0x0);
+			rtpcs_sds_write_bits(ctrl, sds, 0x2e, 0xd, 7, 7, 0x1);
+
+			rtpcs_sds_write_bits(ctrl, sds, 0x2e, 0x1c, 5, 0, 0x1E);
+			rtpcs_sds_write_bits(ctrl, sds, 0x2e, 0x1d, 11, 0, 0x00);
+			rtpcs_sds_write_bits(ctrl, sds, 0x2e, 0x1f, 11, 0, 0x00);
+			rtpcs_sds_write_bits(ctrl, sds, 0x2f, 0x0, 11, 0, 0x00);
+			rtpcs_sds_write_bits(ctrl, sds, 0x2f, 0x1, 11, 0, 0x00);
+
+			rtpcs_sds_write_bits(ctrl, sds, 0x2e, 0xf, 12, 6, 0x7F);
+			rtpcs_sds_write(ctrl, sds, 0x2f, 0x12, 0xaaa);
+
+			rtpcs_931x_sds_rx_reset(ctrl, sds);
+
+			rtpcs_sds_write(ctrl, sds, 0x7, 0x10, op_code);
+			rtpcs_sds_write(ctrl, sds, 0x6, 0x1d, 0x0480);
+			rtpcs_sds_write(ctrl, sds, 0x6, 0xe, 0x0400);
+		}
+		break;
+
+	case PHY_INTERFACE_MODE_10GBASER: /* MII_10GR / MII_10GR1000BX_AUTO: */
+	                                  /* configure 10GR fiber mode=1 */
+		rtpcs_sds_write_bits(ctrl, sds, 0x1f, 0xb, 1, 1, 1);
+
+		/* init fiber_1g */
+		rtpcs_sds_write_bits(ctrl, sds, 0x103, 0x13, 15, 14, 0);
+
+		rtpcs_sds_write_bits(ctrl, sds, 0x102, 0x0, 12, 12, 1);
+		rtpcs_sds_write_bits(ctrl, sds, 0x102, 0x0, 6, 6, 1);
+		rtpcs_sds_write_bits(ctrl, sds, 0x102, 0x0, 13, 13, 0);
+
+		/* init auto */
+		rtpcs_sds_write_bits(ctrl, sds, 0x1f, 13, 15, 0, 0x109e);
+		rtpcs_sds_write_bits(ctrl, sds, 0x1f, 0x6, 14, 10, 0x8);
+		rtpcs_sds_write_bits(ctrl, sds, 0x1f, 0x7, 10, 4, 0x7f);
+		break;
+
+	case PHY_INTERFACE_MODE_1000BASEX: /* MII_1000BX_FIBER */
+		rtpcs_sds_write_bits(ctrl, sds, 0x103, 0x13, 15, 14, 0);
+
+		rtpcs_sds_write_bits(ctrl, sds, 0x102, 0x0, 12, 12, 1);
+		rtpcs_sds_write_bits(ctrl, sds, 0x102, 0x0, 6, 6, 1);
+		rtpcs_sds_write_bits(ctrl, sds, 0x102, 0x0, 13, 13, 0);
+		break;
+
+	case PHY_INTERFACE_MODE_SGMII:
+		rtpcs_sds_write_bits(ctrl, sds, 0x24, 0x9, 15, 15, 0);
+
+		/* this was in rtl931x_phylink_mac_config in dsa/rtl83xx/dsa.c before */
+		band = rtpcs_931x_sds_cmu_band_set(ctrl, sds, true, 62, PHY_INTERFACE_MODE_SGMII);
+		break;
+
+	case PHY_INTERFACE_MODE_2500BASEX:
+		rtpcs_sds_write_bits(ctrl, sds, 0x101, 0x14, 8, 8, 1);
+		break;
+
+	case PHY_INTERFACE_MODE_QSGMII:
+	default:
+		pr_info("%s: PHY mode %s not supported by SerDes %d\n",
+		        __func__, phy_modes(mode), sds);
+		return -ENOTSUPP;
+	}
+
+	rtpcs_931x_sds_cmu_type_set(ctrl, sds, mode, chiptype);
+
+	if (sds >= 2 && sds <= 13) {
+		if (chiptype)
+			rtpcs_sds_write(ctrl, sds, 0x2E, 0x1, board_sds_tx_type1[sds - 2]);
+		else {
+			val = 0xa0000;
+			sw_w32(val, RTL93XX_CHIP_INFO);
+			val = sw_r32(RTL93XX_CHIP_INFO);
+			if (val & BIT(28)) /* consider 9311 etc. RTL9313_CHIP_ID == HWP_CHIP_ID(unit)) */
+			{
+				rtpcs_sds_write(ctrl, sds, 0x2E, 0x1, board_sds_tx2[sds - 2]);
+			} else {
+				rtpcs_sds_write(ctrl, sds, 0x2E, 0x1, board_sds_tx[sds - 2]);
+			}
+			val = 0;
+			sw_w32(val, RTL93XX_CHIP_INFO);
+		}
+	}
+
+	val = ori & ~BIT(sds);
+	sw_w32(val, RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR);
+	pr_debug("%s: RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR 0x%08X\n", __func__, sw_r32(RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR));
+
+	if (mode == PHY_INTERFACE_MODE_XGMII ||
+	    mode == PHY_INTERFACE_MODE_QSGMII ||
+	    mode == PHY_INTERFACE_MODE_SGMII ||
+	    mode == PHY_INTERFACE_MODE_USXGMII) {
+		if (mode == PHY_INTERFACE_MODE_XGMII)
+			rtpcs_931x_sds_mii_mode_set(sds, mode);
+		else
+			rtpcs_931x_sds_fiber_mode_set(ctrl, sds, mode);
+	}
+
+	return 0;
+}
+
+/* Common functions */
+
 static void rtpcs_pcs_get_state(struct phylink_pcs *pcs, struct phylink_link_state *state)
 {
 	struct rtpcs_link *link = rtpcs_phylink_pcs_to_link(pcs);