// SPDX-License-Identifier: GPL-2.0-or-later

#include <linux/mdio.h>
#include <linux/mfd/syscon.h>
#include <linux/of.h>
#include <linux/of_mdio.h>
#include <linux/of_platform.h>
#include <linux/phy.h>
#include <linux/platform_device.h>
#include <linux/phylink.h>
#include <linux/regmap.h>

#define RTPCS_SDS_CNT				14
#define RTPCS_PORT_CNT				57
#define RTPCS_MAX_LINKS_PER_SDS			8

#define RTPCS_SPEED_10				0
#define RTPCS_SPEED_100				1
#define RTPCS_SPEED_1000			2
#define RTPCS_SPEED_10000_LEGACY		3
#define RTPCS_SPEED_10000			4
#define RTPCS_SPEED_2500			5
#define RTPCS_SPEED_5000			6

#define RTPCS_838X_CPU_PORT			28
#define RTPCS_838X_SERDES_CNT			6
#define RTPCS_838X_MAC_LINK_DUP_STS		0xa19c
#define RTPCS_838X_MAC_LINK_SPD_STS		0xa190
#define RTPCS_838X_MAC_LINK_STS			0xa188
#define RTPCS_838X_MAC_RX_PAUSE_STS		0xa1a4
#define RTPCS_838X_MAC_TX_PAUSE_STS		0xa1a0

#define RTPCS_839X_CPU_PORT			52
#define RTPCS_839X_SERDES_CNT			14
#define RTPCS_839X_MAC_LINK_DUP_STS		0x03b0
#define RTPCS_839X_MAC_LINK_SPD_STS		0x03a0
#define RTPCS_839X_MAC_LINK_STS			0x0390
#define RTPCS_839X_MAC_RX_PAUSE_STS		0x03c0
#define RTPCS_839X_MAC_TX_PAUSE_STS		0x03b8

#define RTPCS_83XX_MAC_LINK_SPD_BITS		2

#define RTPCS_930X_CPU_PORT			28
#define RTPCS_930X_SERDES_CNT			12
#define RTPCS_930X_MAC_LINK_DUP_STS		0xcb28
#define RTPCS_930X_MAC_LINK_SPD_STS		0xcb18
#define RTPCS_930X_MAC_LINK_STS			0xcb10
#define RTPCS_930X_MAC_RX_PAUSE_STS		0xcb30
#define RTPCS_930X_MAC_TX_PAUSE_STS		0xcb2c

#define RTPCS_931X_CPU_PORT			56
#define RTPCS_931X_SERDES_CNT			14
#define RTPCS_931X_MAC_LINK_DUP_STS		0x0ef0
#define RTPCS_931X_MAC_LINK_SPD_STS		0x0ed0
#define RTPCS_931X_MAC_LINK_STS			0x0ec0
#define RTPCS_931X_MAC_RX_PAUSE_STS		0x0f00
#define RTPCS_931X_MAC_TX_PAUSE_STS		0x0ef8

#define RTPCS_838X_SDS_CFG_REG			0x34
#define RTPCS_838X_RST_GLB_CTRL_0		0x3c
#define RTPCS_838X_SDS_MODE_SEL			0x0028
#define RTPCS_838X_INT_MODE_CTRL		0x005c
#define RTPCS_838X_PLL_CML_CTRL			0x0ff8

#define RTPCS_839X_MAC_SERDES_IF_CTRL		0x0008

#define RTPCS_93XX_MAC_LINK_SPD_BITS		4

#define RTPCS_93XX_MODEL_NAME_INFO		(0x0004)
#define RTPCS_93XX_CHIP_INFO			(0x0008)

#define PHY_PAGE_2	2
#define PHY_PAGE_4	4

#define RTL9300_PHY_ID_MASK 0xf0ffffff

/* RTL930X SerDes supports the following modes:
 * 0x02: SGMII		0x04: 1000BX_FIBER	0x05: FIBER100
 * 0x06: QSGMII		0x09: RSGMII		0x0d: USXGMII
 * 0x10: XSGMII		0x12: HISGMII		0x16: 2500Base_X
 * 0x17: RXAUI_LITE	0x19: RXAUI_PLUS	0x1a: 10G Base-R
 * 0x1b: 10GR1000BX_AUTO			0x1f: OFF
 */
#define RTPCS_930X_SDS_MODE_SGMII	0x02
#define RTPCS_930X_SDS_MODE_1000BASEX	0x04
#define RTPCS_930X_SDS_MODE_QSGMII	0x06
#define RTPCS_930X_SDS_MODE_USXGMII	0x0d
#define RTPCS_930X_SDS_MODE_XSGMII	0x10
#define RTPCS_930X_SDS_MODE_2500BASEX	0x16
#define RTPCS_930X_SDS_MODE_10GBASER	0x1a
#define RTPCS_930X_SDS_OFF		0x1f
#define RTPCS_930X_SDS_MASK		0x1f

/* RTL930X SerDes supports two submodes when mode is USXGMII:
 * 0x00: USXGMII (aka USXGMII_SX)
 * 0x02: 10G_QXGMII (aka USXGMII_QX)
 */
#define RTPCS_930X_SDS_SUBMODE_USXGMII_SX	0x0
#define RTPCS_930X_SDS_SUBMODE_USXGMII_QX	0x2

/* Registers of the internal SerDes of the 9310 */
#define RTPCS_931X_MAC_GROUP0_1_CTRL		(0x13a4)
#define RTPCS_931X_MAC_GROUP2_3_CTRL		(0x13a8)
#define RTPCS_931X_MAC_GROUP4_CTRL		(0x13ac)
#define RTPCS_931X_MAC_GROUP5_CTRL		(0x13b0)
#define RTPCS_931X_MAC_GROUP6_7_CTRL		(0x13b4)
#define RTPCS_931X_MAC_GROUP8_11_CTRL		(0x13b8)
#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)))
#define RTPCS_931X_ISR_SERDES_RXIDLE		(0x12f8)

enum rtpcs_sds_mode {
	RTPCS_SDS_MODE_OFF = 0,

	/* fiber modes */
	RTPCS_SDS_MODE_100BASEX,
	RTPCS_SDS_MODE_1000BASEX,
	RTPCS_SDS_MODE_2500BASEX,
	RTPCS_SDS_MODE_10GBASER,

	/* mii modes */
	RTPCS_SDS_MODE_SGMII,
	RTPCS_SDS_MODE_HISGMII,
	RTPCS_SDS_MODE_QSGMII,
	RTPCS_SDS_MODE_QHSGMII,
	RTPCS_SDS_MODE_XSGMII,

	RTPCS_SDS_MODE_USXGMII_10GSXGMII,
	RTPCS_SDS_MODE_USXGMII_10GDXGMII,
	RTPCS_SDS_MODE_USXGMII_10GQXGMII,
	RTPCS_SDS_MODE_USXGMII_5GSXGMII,
	RTPCS_SDS_MODE_USXGMII_5GDXGMII,
	RTPCS_SDS_MODE_USXGMII_2_5GSXGMII,
};

enum rtpcs_port_media {
	RTPCS_PORT_MEDIA_NONE,
	RTPCS_PORT_MEDIA_FIBER_100M,
	RTPCS_PORT_MEDIA_FIBER_1G,
	RTPCS_PORT_MEDIA_FIBER_2_5G,
	RTPCS_PORT_MEDIA_FIBER_10G,
	RTPCS_PORT_MEDIA_DAC_50CM,
	RTPCS_PORT_MEDIA_DAC_100CM,
	RTPCS_PORT_MEDIA_DAC_300CM,
	RTPCS_PORT_MEDIA_DAC_500CM,
};

enum rtpcs_sds_pll_type {
	RTPCS_SDS_PLL_TYPE_RING = 0,
	RTPCS_SDS_PLL_TYPE_LC = 1,
	RTPCS_SDS_PLL_TYPE_END,
};

enum rtpcs_sds_pll_speed {
	RTPCS_SDS_PLL_SPD_1000 = 0,
	RTPCS_SDS_PLL_SPD_2500 = 1,
	RTPCS_SDS_PLL_SPD_10000 = 2,
	RTPCS_SDS_PLL_SPD_END,
};

enum rtpcs_chip_version {
	RTPCS_CHIP_V1 = 0,
	RTPCS_CHIP_V2,
};

struct rtpcs_ctrl;
struct rtpcs_serdes;

struct rtpcs_serdes_ops {
	int (*read)(struct rtpcs_serdes *sds, int page, int regnum, int bithigh, int bitlow);
	int (*write)(struct rtpcs_serdes *sds, int page, int regnum, int bithigh, int bitlow,
		     u16 value);

	/* optional */
	int (*xsg_write)(struct rtpcs_serdes *sds, int page, int regnum, int bithigh, int bitlow,
			 u16 value);

	int (*set_autoneg)(struct rtpcs_serdes *sds, unsigned int neg_mode,
			   const unsigned long *advertising);
	void (*restart_autoneg)(struct rtpcs_serdes *sds);

	/* CMU management */
	int (*get_pll_select)(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type *pll);
	int (*set_pll_select)(struct rtpcs_serdes *sds, enum rtpcs_sds_mode hw_mode,
			      enum rtpcs_sds_pll_type pll);
	int (*reset_cmu)(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll);
	/* online reconfiguration of a running SerDes to another PLL */
	int (*reconfigure_to_pll)(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll);
};

struct rtpcs_sds_reg_field {
	u8 page;
	u8 reg;
	u8 msb;
	u8 lsb;
};

struct rtpcs_sds_regs {
	struct rtpcs_sds_reg_field an_enable;
	struct rtpcs_sds_reg_field an_restart;
	struct rtpcs_sds_reg_field an_advertise;
};

struct rtpcs_serdes {
	struct rtpcs_ctrl *ctrl;
	const struct rtpcs_serdes_ops *ops;
	const struct rtpcs_sds_regs *regs;
	enum rtpcs_sds_mode hw_mode;
	u8 id;
	u8 num_of_links;
	bool first_start;

	bool rx_pol_inv;
	bool tx_pol_inv;
};

struct rtpcs_ctrl {
	struct device *dev;
	struct regmap *map;
	struct mii_bus *bus;
	const struct rtpcs_config *cfg;
	struct rtpcs_serdes serdes[RTPCS_SDS_CNT];
	struct rtpcs_link *link[RTPCS_PORT_CNT];
	struct mutex lock;

	/* meaning and source may be family-specific */
	enum rtpcs_chip_version chip_version;
};

struct rtpcs_link {
	struct rtpcs_ctrl *ctrl;
	struct phylink_pcs pcs;
	struct rtpcs_serdes *sds;
	int port;
};

struct rtpcs_config {
	int cpu_port;
	int mac_link_dup_sts;
	int mac_link_spd_bits;
	int mac_link_spd_sts;
	int mac_link_sts;
	int mac_rx_pause_sts;
	int mac_tx_pause_sts;
	u8 serdes_count;

	const struct phylink_pcs_ops *pcs_ops;
	const struct rtpcs_serdes_ops *sds_ops;
	const struct rtpcs_sds_regs *sds_regs;
	int (*init_serdes_common)(struct rtpcs_ctrl *ctrl);
	int (*setup_serdes)(struct rtpcs_serdes *sds, enum rtpcs_sds_mode hw_mode);
};

struct rtpcs_sds_config {
	u8 page;
	u8 reg;
	u16 data;
};

static int rtpcs_sds_to_mmd(int sds_page, int sds_regnum)
{
	return (sds_page << 8) + sds_regnum;
}

/*
 * Basic helpers
 *
 * These work on the plain SerDes ID. They shouldn't be used except for
 * implementing the SerDes read/write ops.
 */

static int __rtpcs_sds_read_raw(struct rtpcs_ctrl *ctrl, int sds_id, int page, int regnum,
				int bithigh, int bitlow)
{
	int mmd_regnum = rtpcs_sds_to_mmd(page, regnum);
	u16 mask;
	int val;

	if (WARN_ON(bithigh < bitlow))
		return -EINVAL;

	mask = GENMASK(bithigh, bitlow);
	val = mdiobus_c45_read(ctrl->bus, sds_id, MDIO_MMD_VEND1, mmd_regnum);
	if (val < 0)
		return val;

	return (val & mask) >> bitlow;
}

static int __rtpcs_sds_write_raw(struct rtpcs_ctrl *ctrl, int sds_id, int page, int regnum,
				 int bithigh, int bitlow, u16 value)
{
	int mmd_regnum = rtpcs_sds_to_mmd(page, regnum);
	u16 mask, set;

	if (WARN_ON(bithigh < bitlow))
		return -EINVAL;

	if (bithigh == 15 && bitlow == 0)
		return mdiobus_c45_write(ctrl->bus, sds_id, MDIO_MMD_VEND1, mmd_regnum, value);

	mask = GENMASK(bithigh, bitlow);
	set = (value << bitlow) & mask;
	return mdiobus_c45_modify(ctrl->bus, sds_id, MDIO_MMD_VEND1, mmd_regnum, mask, set);
}

/* Generic implementations, if no special behavior is needed */

static int rtpcs_generic_sds_op_read(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
				     int bitlow)
{
	return __rtpcs_sds_read_raw(sds->ctrl, sds->id, page, regnum, bithigh, bitlow);
}

static int rtpcs_generic_sds_op_write(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
				      int bitlow, u16 value)
{
	return __rtpcs_sds_write_raw(sds->ctrl, sds->id, page, regnum, bithigh, bitlow, value);
}

/* Convenience helpers */

static int rtpcs_sds_read_bits(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
			       int bitlow)
{
	return sds->ops->read(sds, page, regnum, bithigh, bitlow);
}

static int rtpcs_sds_write_bits(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
				int bitlow, u16 value)
{
	return sds->ops->write(sds, page, regnum, bithigh, bitlow, value);
}

static int rtpcs_sds_read(struct rtpcs_serdes *sds, int page, int regnum)
{
	return sds->ops->read(sds, page, regnum, 15, 0);
}

static int rtpcs_sds_write(struct rtpcs_serdes *sds, int page, int regnum, u16 value)
{
	return sds->ops->write(sds, page, regnum, 15, 0, value);
}

__maybe_unused
static int rtpcs_sds_read_field(struct rtpcs_serdes *sds, const struct rtpcs_sds_reg_field *field)
{
	return sds->ops->read(sds, field->page, field->reg, field->msb, field->lsb);
}

static int rtpcs_sds_write_field(struct rtpcs_serdes *sds, const struct rtpcs_sds_reg_field *field,
				 u16 value)
{
	return sds->ops->write(sds, field->page, field->reg, field->msb, field->lsb, value);
}

__maybe_unused
static int rtpcs_sds_xsg_write_bits(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
				    int bitlow, u16 value)
{
	if (!sds->ops->xsg_write)
		return -ENOTSUPP;

	return sds->ops->xsg_write(sds, page, regnum, bithigh, bitlow, value);
}

__maybe_unused
static int rtpcs_sds_xsg_write(struct rtpcs_serdes *sds, int page, int regnum, u16 value)
{
	if (!sds->ops->xsg_write)
		return -ENOTSUPP;

	return sds->ops->xsg_write(sds, page, regnum, 15, 0, value);
}

/* Other helpers */

__maybe_unused
static int rtpcs_sds_modify(struct rtpcs_serdes *sds, int page, int regnum,
			    u16 mask, u16 set)
{
	int mmd_regnum = rtpcs_sds_to_mmd(page, regnum);

	return mdiobus_c45_modify(sds->ctrl->bus, sds->id, MDIO_MMD_VEND1,
				  mmd_regnum, mask, set);
}

static struct rtpcs_serdes *rtpcs_sds_get_even(struct rtpcs_serdes *sds)
{
	u32 even_sds = sds->id & ~1;
	return &sds->ctrl->serdes[even_sds];
}

static struct rtpcs_serdes *rtpcs_sds_get_odd(struct rtpcs_serdes *sds)
{
	u32 odd_sds = sds->id | 1;
	return &sds->ctrl->serdes[odd_sds];
}

static struct rtpcs_serdes *rtpcs_sds_get_neighbor(struct rtpcs_serdes *sds)
{
	u32 nb_sds = sds->id ^ 1;
	return &sds->ctrl->serdes[nb_sds];
}

static int rtpcs_regmap_read_bits(struct rtpcs_ctrl *ctrl, int base, int bithigh, int bitlow)
{
	int offset = base + (bitlow / 32) * 4;
	int bits = bithigh + 1 - bitlow;
	int shift = bitlow % 32;
	int value;

	regmap_read(ctrl->map, offset, &value);
	value = (value >> shift) & (BIT(bits) - 1);

	return value;
}

static struct rtpcs_link *rtpcs_phylink_pcs_to_link(struct phylink_pcs *pcs)
{
	return container_of(pcs, struct rtpcs_link, pcs);
}

static int rtpcs_sds_determine_hw_mode(struct rtpcs_serdes *sds,
                                       phy_interface_t if_mode,
                                       enum rtpcs_sds_mode *hw_mode)
{
	u8 n_links = sds->num_of_links;

	/* turn off SerDes when there are no links */
	if (!n_links) {
		*hw_mode = RTPCS_SDS_MODE_OFF;
		return 0;
	}

	switch (if_mode) {
	case PHY_INTERFACE_MODE_NA:
		*hw_mode = RTPCS_SDS_MODE_OFF;
		break;
	case PHY_INTERFACE_MODE_100BASEX:
		*hw_mode = RTPCS_SDS_MODE_100BASEX;
		break;
	case PHY_INTERFACE_MODE_1000BASEX:
		*hw_mode = RTPCS_SDS_MODE_1000BASEX;
		break;
	case PHY_INTERFACE_MODE_2500BASEX:
		*hw_mode = RTPCS_SDS_MODE_2500BASEX;
		break;
	case PHY_INTERFACE_MODE_10GBASER:
		*hw_mode = RTPCS_SDS_MODE_10GBASER;
		break;
	case PHY_INTERFACE_MODE_SGMII:
		*hw_mode = RTPCS_SDS_MODE_SGMII;
		break;
	case PHY_INTERFACE_MODE_QSGMII:
		*hw_mode = RTPCS_SDS_MODE_QSGMII;
		break;
	case PHY_INTERFACE_MODE_USXGMII:
		if (n_links == 1)
			*hw_mode = RTPCS_SDS_MODE_USXGMII_10GSXGMII;
		else if (n_links == 2)
			*hw_mode = RTPCS_SDS_MODE_USXGMII_10GDXGMII;
		else if (n_links <= 4)
			*hw_mode = RTPCS_SDS_MODE_USXGMII_10GQXGMII;
		else if (n_links <= 8)
			*hw_mode = RTPCS_SDS_MODE_XSGMII;

		break;
	case PHY_INTERFACE_MODE_10G_QXGMII:
		*hw_mode = RTPCS_SDS_MODE_USXGMII_10GQXGMII;
		break;
	default:
		return -ENOTSUPP;
	}

	/* TODO: check if the particular SerDes supports the mode */

	return 0;
}

/* Generic auto-negotiation config */

static int rtpcs_generic_sds_set_autoneg(struct rtpcs_serdes *sds, unsigned int neg_mode,
					 const unsigned long *advertising)
{
	u16 bmcr, adv, adv_old;
	bool changed = false;
	int ret;

	if ((sds->hw_mode == RTPCS_SDS_MODE_1000BASEX) ||
	    (sds->hw_mode == RTPCS_SDS_MODE_2500BASEX)) {
		adv = ADVERTISE_1000XFULL;
		if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
				      advertising))
			adv |= ADVERTISE_1000XPAUSE;
		if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT,
				      advertising))
			adv |= ADVERTISE_1000XPSE_ASYM;

		adv_old = rtpcs_sds_read_field(sds, &sds->regs->an_advertise);
		if (adv_old < 0)
			return adv_old;

		if (adv != adv_old) {
			changed = true;
			ret = rtpcs_sds_write_field(sds, &sds->regs->an_advertise, adv);
			if (ret < 0)
				return ret;
		}
	}

	bmcr = neg_mode == PHYLINK_PCS_NEG_INBAND_ENABLED ? 1 : 0;

	ret = rtpcs_sds_write_field(sds, &sds->regs->an_enable, bmcr);
	if (ret < 0)
		return ret;

	return changed;
}

static void rtpcs_generic_sds_restart_autoneg(struct rtpcs_serdes *sds)
{
	rtpcs_sds_write_field(sds, &sds->regs->an_restart, 0x1);
}

static int rtpcs_sds_select_pll_speed(enum rtpcs_sds_mode hw_mode, enum rtpcs_sds_pll_speed *speed)
{
	switch (hw_mode) {
	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_QSGMII:
		*speed = RTPCS_SDS_PLL_SPD_1000;
		break;
	case RTPCS_SDS_MODE_2500BASEX:
		*speed = RTPCS_SDS_PLL_SPD_2500;
		break;
	case RTPCS_SDS_MODE_10GBASER:
	case RTPCS_SDS_MODE_XSGMII:
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_2_5GSXGMII:
		*speed = RTPCS_SDS_PLL_SPD_10000;
		break;
	default:
		return -ENOTSUPP;
	}

	return 0;
}

/* Variant-specific functions */

/* RTL838X */

#define SDS(ctrl,n)	(&(ctrl)->serdes[n])

static void rtpcs_838x_sds_patch_01_qsgmii_6275b(struct rtpcs_ctrl *ctrl)
{
	/* CKREFBUF_S0S1 for QSGMII */
	regmap_write_bits(ctrl->map, RTPCS_838X_PLL_CML_CTRL, 0xf, 0xf);

	rtpcs_sds_write(SDS(ctrl, 0), 1, 3, 0xf46f);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 6, 0x20d8);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 6, 0x20d8);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 17, 0xb7c9);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 11, 0x482);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 10, 0x80c7);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 18, 0xab8e);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 11, 0x482);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 19, 0x24ab);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 17, 0x4208);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 18, 0xc208);
	rtpcs_sds_write(SDS(ctrl, 0), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 1), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 14, 0xfcc2);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 14, 0xfcc2);

	rtpcs_sds_write(SDS(ctrl, 0), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 0), 1, 9, 0x8c64);

	rtpcs_sds_write(SDS(ctrl, 1), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 1), 1, 9, 0x8c64);
}

static void rtpcs_838x_sds_patch_23_qsgmii_6275b(struct rtpcs_ctrl *ctrl)
{
	rtpcs_sds_write(SDS(ctrl, 2), 1, 3, 0xf46d);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 6, 0x20d8);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 6, 0x20d8);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 17, 0xb7c9);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 18, 0xab8e);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 11, 0x482);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 11, 0x482);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 19, 0x24ab);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 17, 0x4208);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 18, 0xc208);
	rtpcs_sds_write(SDS(ctrl, 2), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 3), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 14, 0xfcc2);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 14, 0xfcc2);

	rtpcs_sds_write(SDS(ctrl, 2), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 2), 1, 9, 0x8c64);

	rtpcs_sds_write(SDS(ctrl, 3), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 3), 1, 9, 0x8c64);
}

static void rtpcs_838x_sds_patch_4_fiber_6275b(struct rtpcs_ctrl *ctrl)
{
	rtpcs_sds_write(SDS(ctrl, 4), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 11, 0x1482);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 6, 0x20d8);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 10, 0xc3);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 17, 0xb7c9);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 18, 0xab8e);
	rtpcs_sds_write(SDS(ctrl, 4), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 14, 0xfcc2);

	rtpcs_sds_write(SDS(ctrl, 4), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 9, 0x8c64);
}

static void rtpcs_838x_sds_patch_4_qsgmii_6275b(struct rtpcs_ctrl *ctrl)
{
	rtpcs_sds_write(SDS(ctrl, 4), 1, 3, 0xf46d);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 11, 0x0482);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 6, 0x20d8);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 10, 0x58c7);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 17, 0xb7c9);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 18, 0xab8e);
	rtpcs_sds_write(SDS(ctrl, 4), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 14, 0xfcc2);

	rtpcs_sds_write(SDS(ctrl, 4), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 4), 1, 9, 0x8c64);
}

static void rtpcs_838x_sds_patch_5_fiber_6275b(struct rtpcs_ctrl *ctrl)
{
	rtpcs_sds_write(SDS(ctrl, 5), 1, 2, 0x85fa);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 3, 0x00);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 4, 0xdccc);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 5, 0x00);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 6, 0x3600);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 7, 0x03);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 8, 0x79aa);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 9, 0x8c64);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 10, 0xc3);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 11, 0x1482);
	rtpcs_sds_write(SDS(ctrl, 5), 2, 24, 0x14aa);
	rtpcs_sds_write(SDS(ctrl, 5), 2, 25, 0x303);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 14, 0xf002);
	rtpcs_sds_write(SDS(ctrl, 5), 2, 27, 0x4bf);

	rtpcs_sds_write(SDS(ctrl, 5), 1, 9, 0x8e64);
	rtpcs_sds_write(SDS(ctrl, 5), 1, 9, 0x8c64);
}

static void rtpcs_838x_sds_reset(struct rtpcs_serdes *sds)
{
	rtpcs_sds_write_bits(sds, 2, 0, 11, 11, 0x0);	/* FIB_REG0 CFG_FIB_PDOWN */

	/* analog reset */
	rtpcs_sds_write_bits(sds, 0, 0, 1, 0, 0x0);	/* REG0 EN_RX/EN_TX */
	rtpcs_sds_write_bits(sds, 0, 0, 1, 0, 0x3);	/* REG0 EN_RX/EN_TX */

	/* digital reset */
	rtpcs_sds_write_bits(sds, 0, 3, 6, 6, 0x1);	/* REG3 SOFT_RST */
	rtpcs_sds_write_bits(sds, 0, 3, 6, 6, 0x0);	/* REG3 SOFT_RST */

	dev_info(sds->ctrl->dev, "SerDes %d reset\n", sds->id);
}

static bool rtpcs_838x_sds_is_hw_mode_supported(struct rtpcs_serdes *sds,
						enum rtpcs_sds_mode hw_mode)
{
	switch (sds->id) {
	case 0 ... 3:
		return hw_mode == RTPCS_SDS_MODE_QSGMII;
	case 4:
		return hw_mode == RTPCS_SDS_MODE_QSGMII ||
		       hw_mode == RTPCS_SDS_MODE_SGMII ||
		       hw_mode == RTPCS_SDS_MODE_1000BASEX;
	case 5:
		return hw_mode == RTPCS_SDS_MODE_SGMII ||
		       hw_mode == RTPCS_SDS_MODE_1000BASEX;
	default:
		return false;
	}
}

static int rtpcs_838x_sds_power(struct rtpcs_serdes *sds, bool power_on)
{
	u8 sds_id = sds->id;
	int ret;
	u8 val;

	val = power_on ? 0 : BIT(sds_id);

	ret = regmap_write_bits(sds->ctrl->map, RTPCS_838X_SDS_CFG_REG, BIT(sds_id), val);
	if (ret)
		return ret;

	if (sds_id >= 4)
		ret = regmap_write_bits(sds->ctrl->map, RTPCS_838X_SDS_CFG_REG,
					BIT(sds_id) << 2, val << 2); /* SDS*_PHY_MODE */

	return ret;
}

static int rtpcs_838x_sds_set_mode(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	u8 sds_mode_shift, int_mode_shift;
	u32 sds_mode_val, int_mode_val;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_1000BASEX:
		sds_mode_val = 0x4;
		int_mode_val = 0x1;
		break;
	case RTPCS_SDS_MODE_SGMII:
		sds_mode_val = 0x2;
		int_mode_val = 0x2;
		break;
	case RTPCS_SDS_MODE_QSGMII:
		sds_mode_val = 0x6;
		int_mode_val = 0x5;
		break;
	default:
		return -EINVAL;
	}

	/* Configure SerDes module mode (all SDS 0-5) */
	sds_mode_shift = (5 - sds->id) * 5;
	regmap_write_bits(sds->ctrl->map, RTPCS_838X_SDS_MODE_SEL,
			  0x1f << sds_mode_shift, sds_mode_val << sds_mode_shift);

	/* Configure MAC interface mode (only SDS 4-5) */
	if (sds->id >= 4) {
		int_mode_shift = (sds->id == 5) ? 3 : 0;
		regmap_write_bits(sds->ctrl->map, RTPCS_838X_INT_MODE_CTRL,
				  0x7 << int_mode_shift, int_mode_val << int_mode_shift);
	}

	return 0;
}

static int rtpcs_838x_sds_patch(struct rtpcs_serdes *sds,
				enum rtpcs_sds_mode hw_mode)
{
	struct rtpcs_ctrl *ctrl = sds->ctrl;
	u8 sds_id = sds->id;

	rtpcs_sds_write(sds, 0, 1, 0xf00);
	mdelay(1);
	rtpcs_sds_write(sds, 0, 2, 0x7060);
	mdelay(1);

	if (sds_id >= 4) {
		rtpcs_sds_write(sds, 2, 30, 0x71e);
		mdelay(1);
		rtpcs_sds_write(sds, 0, 4, 0x74d);
		mdelay(1);
	}

	switch (hw_mode) {
	case RTPCS_SDS_MODE_1000BASEX:
		if (sds_id == 4)
			rtpcs_838x_sds_patch_4_fiber_6275b(ctrl);
		else if (sds_id == 5)
			rtpcs_838x_sds_patch_5_fiber_6275b(ctrl);

		break;
	case RTPCS_SDS_MODE_QSGMII:
		if (sds_id == 0 || sds_id == 1)
			rtpcs_838x_sds_patch_01_qsgmii_6275b(ctrl);
		else if (sds_id == 2 || sds_id == 3)
			rtpcs_838x_sds_patch_23_qsgmii_6275b(ctrl);
		else if (sds_id == 4)
			rtpcs_838x_sds_patch_4_qsgmii_6275b(ctrl);

		break;
	default:
		break;
	}
	
	return 0;
}

static int rtpcs_838x_init_serdes_common(struct rtpcs_ctrl *ctrl)
{
	dev_dbg(ctrl->dev, "Init RTL838X SerDes common\n");

	/* power off and reset all SerDes */
	regmap_write(ctrl->map, RTPCS_838X_SDS_CFG_REG, 0x3f);
	regmap_write(ctrl->map, RTPCS_838X_RST_GLB_CTRL_0, 0x10); /* SW_SERDES_RST */
	return 0;
}

static int rtpcs_838x_setup_serdes(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	int ret;

	if (!rtpcs_838x_sds_is_hw_mode_supported(sds, hw_mode))
		return -ENOTSUPP;

	rtpcs_838x_sds_power(sds, false);

	/* take reset */
	rtpcs_sds_write(sds, 0x0, 0x0, 0xc00);
	rtpcs_sds_write(sds, 0x0, 0x3, 0x7146);

	ret = rtpcs_838x_sds_set_mode(sds, hw_mode);
	if (ret)
		return ret;

	sds->hw_mode = hw_mode;

	rtpcs_838x_sds_patch(sds, hw_mode);
	rtpcs_838x_sds_reset(sds);
	
	/* release reset */
	rtpcs_sds_write(sds, 0, 3, 0x7106);

	rtpcs_838x_sds_power(sds, true);

	/*
	 * Run a switch queue reset after the first start of a SerDes. This recovers ports that
	 * were already connected during boot and will not pass traffic. Sometimes the bug can
	 * be seen in registers INGR_DBG_REG0-INGR_DBG_REG2 but this is quite erratic. The SDK
	 * seems to have no issues because it starts all SerDes then PHYs and runs a queue reset
	 * finally during NIC start.
	 *
	 * Of course this is totally wrong here and should be part of the DSA driver. But
	 * implementing it over there requires more tricks than this (e.g. delayed work).
	 */
	if (sds->first_start)
		regmap_write(sds->ctrl->map, RTPCS_838X_RST_GLB_CTRL_0, 0x4);

	return 0;
}

/* RTL839X */

static void rtpcs_839x_sds_reset(struct rtpcs_serdes *sds)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	struct rtpcs_serdes *odd_sds = rtpcs_sds_get_odd(sds);

	bool is_10g_sds = (sds->id == 8 || sds->id == 9 || sds->id == 12 ||
			   sds->id == 13);

	/* FIXME: The reset sequence seems to break some of the 5G SerDes
	 * though the SDK is calling it for all SerDes during init. Until
	 * this is solved, skip reset.
	 */
	if (!is_10g_sds)
		return;

	if (is_10g_sds) {
		rtpcs_sds_write_bits(odd_sds, 0xb, 0x1d, 3, 0, 0x5);
		msleep(500);
		rtpcs_sds_write_bits(odd_sds, 0xb, 0x1d, 3, 0, 0xf);
		rtpcs_sds_write_bits(odd_sds, 0xb, 0x1d, 3, 0, 0x0);

		rtpcs_sds_write_bits(even_sds, 0xa, 0x10, 3, 3, 0x0);
		rtpcs_sds_write_bits(even_sds, 0xb, 0x0, 15, 15, 0x1);
		msleep(100);
		rtpcs_sds_write_bits(even_sds, 0xb, 0x0, 15, 15, 0x0);
	} else {
		rtpcs_sds_write(odd_sds, 0x9, 0x1, 0x0050);
		rtpcs_sds_write(odd_sds, 0x9, 0x1, 0x00f0);
		rtpcs_sds_write(odd_sds, 0x9, 0x1, 0x0000);

		rtpcs_sds_write_bits(sds, 0x8, 0x14, 0, 0, 0x0);
		rtpcs_sds_write_bits(sds, 0x8, 0x14, 9, 9, 0x1);
		msleep(100);
		rtpcs_sds_write_bits(sds, 0x8, 0x14, 9, 9, 0x0);
	}

	rtpcs_sds_write(even_sds, 0x0, 0x3, 0x7146);
	msleep(100);
	rtpcs_sds_write(even_sds, 0x0, 0x3, 0x7106);

	rtpcs_sds_write(odd_sds, 0x0, 0x3, 0x7146);
	msleep(100);
	rtpcs_sds_write(odd_sds, 0x0, 0x3, 0x7106);
}

static int rtpcs_839x_sds_set_mode(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	u32 mode_val, reg, shift;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_OFF:
		mode_val = 0x0;
		break;
/*
	case RTPCS_SDS_MODE_100BASEX:
		mode_val = 0x8;
		break;
	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_SGMII:
		mode_val = 0x7;
		break;
*/
	case RTPCS_SDS_MODE_QSGMII:
		mode_val = 0x6;
		break;
	default:
		return -ENOTSUPP;
	}

	reg = RTPCS_839X_MAC_SERDES_IF_CTRL + (sds->id / 8) * 4;
	shift = (sds->id % 8) * 4;
	return regmap_write_bits(sds->ctrl->map, reg, 0xf << shift,
				 mode_val << shift);
}

static void rtpcs_839x_sds_init(struct rtpcs_serdes *sds)
{
	bool is_even = sds->id % 2 == 0;

	/*
	 * This function is quite "mystic". It has been taken over from the vendor SDK function
	 * rtl839x_serdes_patch_init(). There is not much documentation about it but one could
	 * lookup the fields from the field headers. The 5G SerDes seem to work out of the box
	 * so only setup the 10G SerDes for now.
	 */
	if (sds->id != 8 && sds->id != 9 && sds->id != 12 && sds->id != 13)
		return;

	/* Part 1: register setup */
	rtpcs_sds_write(sds, 0xa, 0x0, 0x5800);
	rtpcs_sds_write(sds, 0xa, 0x1, 0x4000);
	rtpcs_sds_write(sds, 0xa, 0x2, is_even ? 0x5400 : 0x5000);
	rtpcs_sds_write(sds, 0xa, 0x3, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x4, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x5, 0x4000);
	rtpcs_sds_write(sds, 0xa, 0x6, 0x4000);
	rtpcs_sds_write(sds, 0xa, 0x7, 0xffff);
	rtpcs_sds_write(sds, 0xa, 0x8, 0xffff);
	rtpcs_sds_write(sds, 0xa, 0x9, 0x806f);
	rtpcs_sds_write(sds, 0xa, 0xa, 0x0004);
	rtpcs_sds_write(sds, 0xa, 0xb, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0xc, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0xd, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0xe, 0x0a00);
	rtpcs_sds_write(sds, 0xa, 0xf, 0x2000);
	rtpcs_sds_write(sds, 0xa, 0x10, 0xf00e);
	rtpcs_sds_write(sds, 0xa, 0x11, is_even ? 0xf04a : 0xfdab);
	rtpcs_sds_write(sds, 0xa, 0x12, is_even ? 0x97b3 : 0x96ea);
	rtpcs_sds_write(sds, 0xa, 0x13, 0x5318);
	rtpcs_sds_write(sds, 0xa, 0x14, 0x0f03);
	rtpcs_sds_write(sds, 0xa, 0x15, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x16, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x17, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x18, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x19, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x1a, 0xffff);
	rtpcs_sds_write(sds, 0xa, 0x1b, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x1c, 0x1203);
	rtpcs_sds_write(sds, 0xa, 0x1d, 0x0000);
	rtpcs_sds_write(sds, 0xa, 0x1e, 0xa052);
	rtpcs_sds_write(sds, 0xa, 0x1f, 0x9a00);
	rtpcs_sds_write(sds, 0xb, 0x0, 0x00f5);
	rtpcs_sds_write(sds, 0xb, 0x1, 0xf000);
	rtpcs_sds_write(sds, 0xb, 0x2, is_even ? 0x41ff : 0x4079);
	rtpcs_sds_write(sds, 0xb, 0x3, 0x0000);
	rtpcs_sds_write(sds, 0xb, 0x4, is_even ? 0x39ff : 0x93fa);
	rtpcs_sds_write(sds, 0xb, 0x5, 0x3340);
	rtpcs_sds_write(sds, 0xb, 0x6, is_even ? 0x40aa : 0x4280);
	rtpcs_sds_write(sds, 0xb, 0x7, 0x0000);
	rtpcs_sds_write(sds, 0xb, 0x8, 0x801f);
	rtpcs_sds_write(sds, 0xb, 0x9, 0x0000);
	rtpcs_sds_write(sds, 0xb, 0xa, 0x619c);
	rtpcs_sds_write(sds, 0xb, 0xb, 0xffed);
	rtpcs_sds_write(sds, 0xb, 0xc, 0x29ff);
	rtpcs_sds_write(sds, 0xb, 0xd, 0x29ff);
	rtpcs_sds_write(sds, 0xb, 0xe, is_even ? 0x4e10 : 0x4c50);
	rtpcs_sds_write(sds, 0xb, 0xf, is_even ? 0x4e10 : 0x4c50);
	rtpcs_sds_write(sds, 0xb, 0x10, 0x0000);
	rtpcs_sds_write(sds, 0xb, 0x11, 0x0000);
	rtpcs_sds_write(sds, 0x0, 0xc, 0x08ec);
	if (!is_even)
		rtpcs_sds_write(sds, 0xb, 0x1f, 0x003f);

	/* Part 2: register bit patching (contains some "reset flips") */
	rtpcs_sds_write_bits(sds, 0x0, 0x7, 14, 14, 0x0001);
	rtpcs_sds_write_bits(sds, 0xb, 0x9, 15, 0, 0x417f);
	rtpcs_sds_write_bits(sds, 0xa, 0x1c, 9, 9, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x1c, 12, 10, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x1c, 5, 3, 0x0005);
	rtpcs_sds_write_bits(sds, 0xa, 0x1c, 8, 6, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x1c, 2, 0, 0x0002);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 15, 0, 0xc440);
	if (is_even)
		rtpcs_sds_write_bits(sds, 0xb, 0x6, 3, 3, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x5, 15, 0, 0x8000);
	rtpcs_sds_write_bits(sds, 0xa, 0x6, 15, 0, 0x8000);
	rtpcs_sds_write_bits(sds, 0xa, 0xa, 15, 0, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x1e, 15, 0, 0x0002);
	rtpcs_sds_write_bits(sds, 0xa, 0x1f, 15, 0, 0xbe00);
	if (is_even) {
		rtpcs_sds_write_bits(sds, 0xb, 0xe, 10, 10, 0x0000);
		rtpcs_sds_write_bits(sds, 0xb, 0xf, 10, 10, 0x0000);
		rtpcs_sds_write_bits(sds, 0xb, 0xe, 14, 14, 0x0000);
		rtpcs_sds_write_bits(sds, 0xb, 0xf, 14, 14, 0x0000);
	}
	rtpcs_sds_write_bits(sds, 0xa, 0x10, 5, 5, 0x0000);
	rtpcs_sds_write_bits(sds, 0xb, 0x9, 8, 8, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x3, 15, 12, 0x000f);
	rtpcs_sds_write_bits(sds, 0xa, 0x1f, 13, 12, 0x0003);
	rtpcs_sds_write_bits(sds, 0xa, 0x1f, 11, 9, 0x0007);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 15, 15, 0x0001);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 14, 14, 0x0001);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 13, 13, 0x0000);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 12, 12, 0x0000);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 11, 9, 0x0002);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 8, 6, 0x0002);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 5, 3, 0x0000);
	rtpcs_sds_write_bits(sds, 0xb, 0x1, 2, 0, 0x0000);
	rtpcs_sds_write_bits(sds, 0xb, 0xc, 9, 9, 0x0001);
	rtpcs_sds_write_bits(sds, 0xb, 0xd, 9, 9, 0x0001);
	rtpcs_sds_write_bits(sds, 0xb, 0x8, 5, 5, 0x0001);
	rtpcs_sds_write_bits(sds, 0xb, 0x8, 6, 6, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x1c, 15, 15, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x10, 15, 12, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x13, 4, 4, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x13, 9, 9, 0x0000);
	rtpcs_sds_write_bits(sds, 0xa, 0x13, 3, 0, 0x0008);
	rtpcs_sds_write_bits(sds, 0xa, 0x13, 8, 5, 0x0008);
}

static int rtpcs_839x_init_serdes_common(struct rtpcs_ctrl *ctrl)
{
	for (int sds_id = 0; sds_id < ctrl->cfg->serdes_count; sds_id++)
		rtpcs_839x_sds_init(&ctrl->serdes[sds_id]);

	for (int sds_id = 0; sds_id < ctrl->cfg->serdes_count; sds_id++)
		rtpcs_839x_sds_reset(&ctrl->serdes[sds_id]);

	return 0;
}

static int rtpcs_839x_setup_serdes(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	int ret;

	/* Don't touch 5G SerDes, they are already properly configured
	 * at startup for QSGMII. Thus, connected PHYs should work out
	 * of the box.
	 */
	if (sds->id != 8 && sds->id != 9 && sds->id != 12 && sds->id != 13)
		return 0;

	ret = rtpcs_839x_sds_set_mode(sds, hw_mode);
	if (ret < 0)
		return ret;

	sds->hw_mode = hw_mode;

	rtpcs_839x_sds_reset(sds);
	return 0;
}

/* RTL93XX */

static int rtpcs_93xx_sds_set_autoneg(struct rtpcs_serdes *sds, unsigned int neg_mode,
				      const unsigned long *advertising)
{
	u16 en_val;

	switch (sds->hw_mode) {
	case RTPCS_SDS_MODE_XSGMII: /* XSG N-way state */
		en_val = neg_mode == PHYLINK_PCS_NEG_INBAND_ENABLED ? 0x0 : 0x1;

		return rtpcs_sds_xsg_write_bits(sds, 0x0, 0x2, 9, 8, en_val);
	default:
		return rtpcs_generic_sds_set_autoneg(sds, neg_mode, advertising);
	}
}

static int rtpcs_93xx_init_serdes_common(struct rtpcs_ctrl *ctrl)
{
	u32 model_info = 0;
	int rl_vid, val;

	regmap_read(ctrl->map, RTPCS_93XX_MODEL_NAME_INFO, &model_info);
	if (model_info & BIT(4))
		dev_warn(ctrl->dev, "ES chip variants may not work properly!\n");

	val = 0xa0000; /* CHIP_INFO_EN */
	regmap_write(ctrl->map, RTPCS_93XX_CHIP_INFO, val);
	regmap_read(ctrl->map, RTPCS_93XX_CHIP_INFO, &val);
	rl_vid = FIELD_GET(GENMASK(31, 28), val);

	if (rl_vid & BIT(0))
		ctrl->chip_version = RTPCS_CHIP_V2;

	val = 0;
	regmap_write(ctrl->map, RTPCS_93XX_CHIP_INFO, val);

	dev_dbg(ctrl->dev, "chip_version %u\n", ctrl->chip_version + 1);
	return 0;
}

static int rtpcs_93xx_sds_get_pll_config(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll,
					 enum rtpcs_sds_pll_speed *speed)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int sbit, speed_val;

	/*
	 * PLL config is shared between adjacent SerDes in the even lane. Each SerDes defines
	 * what PLL it needs (ring or LC) while the PLL itself stores the current speed.
	 */

	sbit = pll == RTPCS_SDS_PLL_TYPE_LC ? 8 : 12;
	speed_val = rtpcs_sds_read_bits(even_sds, 0x20, 0x12, sbit + 3, sbit);
	if (speed_val < 0)
		return speed_val;

	/* bit 0 is force-bit, bits [3:1] are speed selector */
	*speed = (enum rtpcs_sds_pll_speed)(speed_val >> 1);
	return 0;
}

static int rtpcs_93xx_sds_set_pll_config(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll,
					 enum rtpcs_sds_pll_speed speed)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int sbit = pll == RTPCS_SDS_PLL_TYPE_LC ? 8 : 12;
	int ret;

	if (speed >= RTPCS_SDS_PLL_SPD_END)
		return -EINVAL;

	if (pll >= RTPCS_SDS_PLL_TYPE_END)
		return -EINVAL;

	if ((pll == RTPCS_SDS_PLL_TYPE_RING) && (speed == RTPCS_SDS_PLL_SPD_10000))
		return -EINVAL;

	/*
	 * A SerDes clock can either be taken from the low speed ring PLL or the high speed
	 * LC PLL. As it is unclear if disabling PLLs has any positive or negative effect,
	 * always activate both.
	 */
	ret = rtpcs_sds_write_bits(even_sds, 0x20, 0x12, 3, 0, 0xf);
	if (ret < 0)
		return ret;

	/* bit 0 is force-bit, bits [3:1] are speed selector */
	ret = rtpcs_sds_write_bits(even_sds, 0x20, 0x12, sbit + 3, sbit, (speed << 1) | BIT(0));
	if (ret < 0)
		return ret;

	if (sds->ops->reset_cmu)
		ret = sds->ops->reset_cmu(sds, pll);

	return ret;
}

static int rtpcs_93xx_sds_config_cmu(struct rtpcs_serdes *sds, enum rtpcs_sds_mode hw_mode)
{
	struct rtpcs_serdes *nb_sds = rtpcs_sds_get_neighbor(sds);
	enum rtpcs_sds_pll_speed speed, neighbor_speed;
	enum rtpcs_sds_pll_type pll, neighbor_pll;
	bool speed_changed = true;
	int ret;

	/*
	 * A SerDes pair on RTL93xx is driven by a shared CMU with two PLLs:
	 *
	 * - a low speed ring PLL which can generate signals of 1.25G and 3.125G for link
	 *   speeds of 1G/2.5G
	 * - a high speed LC PLL which can additionally generate a 10.3125G signal for
	 *   10G link speeds
	 *
	 * To drive the pair at different speeds, each SerDes must use its own PLL and we
	 * must wisely assign the PLLs to the SerDes based on their needs. The logic boils
	 * down to the following rules:
	 *
	 * - use ring PLL for slow 1G speeds
	 * - use LC PLL for fast 10G speeds
	 * - for 2.5G prefer ring over LC PLL
	 *
	 * For the case that we want to configure 10G speed but the LC PLL is already used
	 * by the neighbor SerDes and running with a slower speed, there's no way to avoid
	 * reconfiguration. The neighbor SerDes is reconfigured online to the ring PLL.
	 */

	if (hw_mode == RTPCS_SDS_MODE_OFF)
		return 0;

	ret = rtpcs_sds_select_pll_speed(hw_mode, &speed);
	if (ret < 0)
		return ret;

	if (nb_sds->hw_mode == RTPCS_SDS_MODE_OFF) {
		pll = (speed == RTPCS_SDS_PLL_SPD_10000) ? RTPCS_SDS_PLL_TYPE_LC
							 : RTPCS_SDS_PLL_TYPE_RING;
		goto pll_setup;
	}

	ret = nb_sds->ops->get_pll_select(nb_sds, &neighbor_pll);
	if (ret < 0)
		return ret;

	ret = rtpcs_93xx_sds_get_pll_config(nb_sds, neighbor_pll, &neighbor_speed);
	if (ret < 0)
		return ret;

	if (speed == neighbor_speed) {
		speed_changed = false;
		pll = neighbor_pll;
	} else if (neighbor_pll == RTPCS_SDS_PLL_TYPE_RING)
		pll = RTPCS_SDS_PLL_TYPE_LC;
	else if (speed == RTPCS_SDS_PLL_SPD_10000) {
		pr_info("%s: SDS %d needs LC PLL, reconfigure SDS %d to use ring PLL\n",
			__func__, sds->id, nb_sds->id);

		ret = nb_sds->ops->reconfigure_to_pll(nb_sds, RTPCS_SDS_PLL_TYPE_RING);
		if (ret < 0)
			return ret;

		pll = RTPCS_SDS_PLL_TYPE_LC;
	} else
		pll = RTPCS_SDS_PLL_TYPE_RING;

pll_setup:
	if (speed_changed) {
		ret = rtpcs_93xx_sds_set_pll_config(sds, pll, speed);
		if (ret < 0)
			return ret;
	}

	ret = sds->ops->set_pll_select(sds, hw_mode, pll);
	if (ret < 0)
		return ret;

	pr_info("%s: SDS %d using %s PLL for mode %d\n", __func__, sds->id,
		pll == RTPCS_SDS_PLL_TYPE_LC ? "LC" : "ring", hw_mode);
	return ret;
}

/* RTL930X */

/* 
 * RTL930X needs a special mapping from logic SerDes ID to physical SerDes ID,
 * which takes the page into account. This applies to most of read/write calls.
 */
static int rtpcs_930x_sds_get_phys_sds_id(int sds_id, int page)
{
        if (sds_id == 3 && page < 4)
                return 10;

        return sds_id;
}

static int rtpcs_930x_sds_op_read(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
				  int bitlow)
{
	int sds_id = rtpcs_930x_sds_get_phys_sds_id(sds->id, page);

	return __rtpcs_sds_read_raw(sds->ctrl, sds_id, page, regnum, bithigh, bitlow);
}

static int rtpcs_930x_sds_op_write(struct rtpcs_serdes *sds, int page, int regnum, int bithigh,
				   int bitlow, u16 value)
{
	int sds_id = rtpcs_930x_sds_get_phys_sds_id(sds->id, page);

	return __rtpcs_sds_write_raw(sds->ctrl, sds_id, page, regnum, bithigh, bitlow, value);
}

/*
 * Realtek uses some nasty logic for digital parts of SerDes 2 and 3.
 *
 * This implements 'dal_longan_sds_xsg_field_write' and a combination of
 * '_rtl9300_serdes_index_to_physical' and '_rtl9300_serdes_reg_write' from the SDK.
 */
static int rtpcs_930x_sds_op_xsg_write(struct rtpcs_serdes *sds, int page, int regnum,
				       int bithigh, int bitlow, u16 value)
{
	int phys_sds_id, ret;

	switch (sds->id) {
	case 2:
		phys_sds_id = 2;
		break;
	case 3:
		phys_sds_id = 10;
		break;
	default:
		return -ENOTSUPP;
	}

	if (page >= 4)
		return sds->ops->write(sds, page, regnum, bithigh, bitlow, value);

	ret = __rtpcs_sds_write_raw(sds->ctrl, phys_sds_id, page, regnum, bithigh, bitlow, value);
	if (ret)
		return ret;

	return __rtpcs_sds_write_raw(sds->ctrl, phys_sds_id + 1, page, regnum, bithigh, bitlow,
				     value);
}

static const u16 rtpcs_930x_sds_mode_regs[] = {
	0x0194, 0x0194, 0x0194, 0x0194,		/* SDS_MODE_SEL_0 */
	0x02a0, 0x02a0, 0x02a0, 0x02a0,		/* SDS_MODE_SEL_1 */
	0x02a4, 0x02a4,				/* SDS_MODE_SEL_2 */
	0x0198, 0x0198				/* SDS_MODE_SEL_3 */
};
static const u8 rtpcs_930x_sds_mode_lsb[]  = { 0, 6, 12, 18, 0, 6, 12, 18, 0, 6, 0, 6 };

static const u16 rtpcs_930x_sds_submode_regs[] = {
	0x1cc, 0x1cc,				/* SDS_SUBMODE_CTRL0 */
	0x2d8, 0x2d8, 0x2d8, 0x2d8,0x2d8, 0x2d8 /* SDS_SUBMODE_CTRL1 */
};
static const u8 rtpcs_930x_sds_submode_lsb[] = { 0, 5, 0, 5, 10, 15, 20, 25 };

__always_unused
static int __rtpcs_930x_sds_get_mac_mode(struct rtpcs_serdes *sds)
{
	u8 sds_id = sds->id;
	int mode_val, ret;

	ret = regmap_read(sds->ctrl->map, rtpcs_930x_sds_mode_regs[sds_id], &mode_val);
	if (ret < 0)
		return ret;

	mode_val >>= rtpcs_930x_sds_mode_lsb[sds_id];
	return mode_val & RTPCS_930X_SDS_MASK;
}

static int __rtpcs_930x_sds_set_mac_mode(struct rtpcs_serdes *sds, u32 mode)
{
	u8 sds_id = sds->id;
	int ret;

	ret = regmap_write_bits(sds->ctrl->map, rtpcs_930x_sds_mode_regs[sds_id],
				RTPCS_930X_SDS_MASK << rtpcs_930x_sds_mode_lsb[sds_id],
				mode << rtpcs_930x_sds_mode_lsb[sds_id]);
	mdelay(10);

	return ret;
}

__always_unused
static int __rtpcs_930x_sds_get_usxgmii_submode(struct rtpcs_serdes *sds)
{
	u8 sds_id = sds->id;
	int submode, ret;

	if (sds_id < 2 || sds_id > 9) {
		pr_err("%s: SerDes %u doesn't support USXGMII submode\n", __func__, sds_id);
		return -ENOTSUPP;
	}

	ret = regmap_read(sds->ctrl->map, rtpcs_930x_sds_submode_regs[sds_id], &submode);
	if (ret < 0)
		return ret;

	submode >>= rtpcs_930x_sds_submode_lsb[sds_id];
	return submode & RTPCS_930X_SDS_MASK;
}

static int __rtpcs_930x_sds_set_usxgmii_submode(struct rtpcs_serdes *sds, u32 submode)
{
	u8 sds_id = sds->id;

	if (sds_id < 2 || sds_id > 9) {
		pr_err("%s: SerDes %u doesn't support USXGMII submode\n", __func__, sds_id);
		return -ENOTSUPP;
	}

	if (submode != RTPCS_930X_SDS_SUBMODE_USXGMII_SX &&
	    submode != RTPCS_930X_SDS_SUBMODE_USXGMII_QX) {
		pr_err("%s: unsupported submode 0x%x\n", __func__, submode);
		return -ENOTSUPP;
	}

	return regmap_write_bits(sds->ctrl->map, rtpcs_930x_sds_submode_regs[sds_id - 2],
				 RTPCS_930X_SDS_MASK << rtpcs_930x_sds_submode_lsb[sds_id - 2],
				 submode << rtpcs_930x_sds_submode_lsb[sds_id - 2]);
}

static void rtpcs_930x_sds_rx_reset(struct rtpcs_serdes *sds,
				    enum rtpcs_sds_mode hw_mode)
{
	int page = 0x2e; /* 10GR and USXGMII */

	if (hw_mode == RTPCS_SDS_MODE_1000BASEX)
		page = 0x24;

	rtpcs_sds_write_bits(sds, page, 0x15, 4, 4, 0x1);
	mdelay(5);
	rtpcs_sds_write_bits(sds, page, 0x15, 4, 4, 0x0);
}

static int rtpcs_930x_sds_get_pll_select(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type *pll)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int pbit = (sds == even_sds) ? 4 : 6;
	int pll_sel;

	pll_sel = rtpcs_sds_read_bits(even_sds, 0x20, 0x12, pbit + 1, pbit);
	if (pll_sel < 0)
		return pll_sel;

	/* bit 0 is force-bit, bit 1 is PLL selector */
	*pll = (enum rtpcs_sds_pll_type)(pll_sel >> 1);
	return 0;
}

static int rtpcs_930x_sds_set_pll_select(struct rtpcs_serdes *sds, enum rtpcs_sds_mode hw_mode,
					 enum rtpcs_sds_pll_type pll)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int pbit = (sds == even_sds) ? 4 : 6;

	/* Selecting the PLL a SerDes is done in the even lane register */

	/* bit 0 is force-bit, bit 1 is PLL selector */
	return rtpcs_sds_write_bits(even_sds, 0x20, 0x12, pbit + 1, pbit, (pll << 1) | BIT(0));
}

static int rtpcs_930x_sds_reset_cmu(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int reset_sequence[4] = { 3, 2, 3, 1 };
	int bit, i, ret;

	/*
	 * After the PLL speed has changed, the CMU must take over the new values. The models
	 * of the Otto platform have different reset sequences. Luckily it always boils down
	 * to flipping two bits in a special sequence.
	 */

	bit = pll == RTPCS_SDS_PLL_TYPE_LC ? 2 : 0;

	for (i = 0; i < ARRAY_SIZE(reset_sequence); i++) {
		ret = rtpcs_sds_write_bits(even_sds, 0x21, 0x0b, bit + 1, bit,
					   reset_sequence[i]);
		if (ret < 0)
			return ret;
	}
	return 0;
}

static int rtpcs_930x_sds_wait_clock_ready(struct rtpcs_serdes *sds)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int i, ready, ready_cnt = 0, bit = (sds == even_sds) ? 4 : 5;

	/*
	 * While reconfiguring a SerDes it might take some time until its clock is in sync with
	 * the PLL. During that timespan the ready signal might toggle randomly. According to
	 * GPL sources it is enough to verify that 3 consecutive clock ready checks say "ok".
	 */

	for (i = 0; i < 20; i++) {
		usleep_range(10000, 15000);

		rtpcs_sds_write(even_sds, 0x1f, 0x02, 53);
		ready = rtpcs_sds_read_bits(even_sds, 0x1f, 0x14, bit, bit);

		ready_cnt = ready ? ready_cnt + 1 : 0;
		if (ready_cnt >= 3)
			return 0;
	}

	return -EBUSY;
}

static int __rtpcs_930x_sds_get_ip_mode(struct rtpcs_serdes *sds)
{
	return rtpcs_sds_read_bits(sds, 0x1f, 0x09, 11, 7);
}

static int __rtpcs_930x_sds_set_ip_mode(struct rtpcs_serdes *sds, u32 mode)
{
	/* BIT(0) is force mode enable bit */
	return rtpcs_sds_write_bits(sds, 0x1f, 0x09, 11, 6, (mode & 0x1f) << 1 | BIT(0));
}

static void rtpcs_930x_sds_set_power(struct rtpcs_serdes *sds, bool on)
{
	int power_down = on ? 0x0 : 0x3;
	int rx_enable = on ? 0x3 : 0x1;

	rtpcs_sds_write_bits(sds, 0x20, 0x00, 7, 6, power_down);
	rtpcs_sds_write_bits(sds, 0x20, 0x00, 5, 4, rx_enable);
}

static int rtpcs_930x_sds_reconfigure_to_pll(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll)
{
	enum rtpcs_sds_pll_speed speed;
	enum rtpcs_sds_pll_type old_pll;
	int mode, ret;

	mode = __rtpcs_930x_sds_get_ip_mode(sds);

	ret = rtpcs_930x_sds_get_pll_select(sds, &old_pll);
	if (ret < 0)
		return ret;

	ret = rtpcs_93xx_sds_get_pll_config(sds, old_pll, &speed);
	if (ret < 0)
		return ret;

	rtpcs_930x_sds_set_power(sds, false);
	__rtpcs_930x_sds_set_ip_mode(sds, RTPCS_930X_SDS_OFF);

	ret = rtpcs_93xx_sds_set_pll_config(sds, pll, speed);
	if (ret < 0)
		return ret;

	ret = rtpcs_930x_sds_set_pll_select(sds, sds->hw_mode, pll);
	if (ret < 0)
		return ret;

	__rtpcs_930x_sds_set_ip_mode(sds, mode);
	if (rtpcs_930x_sds_wait_clock_ready(sds))
		pr_err("%s: SDS %d could not sync clock\n", __func__, sds->id);

	rtpcs_930x_sds_set_power(sds, true);
	return 0;
}

static void rtpcs_930x_sds_reset_state_machine(struct rtpcs_serdes *sds)
{
	rtpcs_sds_write_bits(sds, 0x06, 0x02, 12, 12, 0x01); /* SM_RESET bit */
	usleep_range(10000, 20000);
	rtpcs_sds_write_bits(sds, 0x06, 0x02, 12, 12, 0x00);
	usleep_range(10000, 20000);
}

static int rtpcs_930x_sds_init_state_machine(struct rtpcs_serdes *sds,
					     enum rtpcs_sds_mode hw_mode)
{
	int loopback, link, cnt = 20, ret = -EBUSY;

	if (hw_mode != RTPCS_SDS_MODE_10GBASER)
		return 0;
	/*
	 * After a SerDes mode change it takes some time until the frontend state machine
	 * works properly for 10G. To verify operation readyness run a connection check via
	 * loopback.
	 */
	loopback = rtpcs_sds_read_bits(sds, 0x06, 0x01, 2, 2); /* CFG_AFE_LPK bit */
	rtpcs_sds_write_bits(sds, 0x06, 0x01, 2, 2, 0x01);

	while (cnt-- && ret) {
		rtpcs_930x_sds_reset_state_machine(sds);
		link = rtpcs_sds_read_bits(sds, 0x05, 0x00, 12, 12); /* 10G link state (latched) */
		link = rtpcs_sds_read_bits(sds, 0x05, 0x00, 12, 12);
		if (link)
			ret = 0;
	}

	rtpcs_sds_write_bits(sds, 0x06, 0x01, 2, 2, loopback);
	rtpcs_930x_sds_reset_state_machine(sds);

	return ret;
}

static int rtpcs_930x_sds_get_hw_mode_val(enum rtpcs_sds_mode hw_mode)
{
	switch (hw_mode) {
	case RTPCS_SDS_MODE_OFF:
		return RTPCS_930X_SDS_OFF;
	case RTPCS_SDS_MODE_SGMII:
		return RTPCS_930X_SDS_MODE_SGMII;
	case RTPCS_SDS_MODE_1000BASEX:
		return RTPCS_930X_SDS_MODE_1000BASEX;
	case RTPCS_SDS_MODE_2500BASEX:
		return RTPCS_930X_SDS_MODE_2500BASEX;
	case RTPCS_SDS_MODE_10GBASER:
		return RTPCS_930X_SDS_MODE_10GBASER;
	case RTPCS_SDS_MODE_QSGMII:
		return RTPCS_930X_SDS_MODE_QSGMII;
	case RTPCS_SDS_MODE_XSGMII:
		return RTPCS_930X_SDS_MODE_XSGMII;
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
		return RTPCS_930X_SDS_MODE_USXGMII;
	default:
		return -ENOTSUPP;
	}
}

static int rtpcs_930x_sds_set_ip_mode(struct rtpcs_serdes *sds,
				      enum rtpcs_sds_mode hw_mode)
{
	int mode_val, ret;

	/*
	 * TODO: Usually one would expect that it is enough to modify the SDS_MODE_SEL_*
	 * registers (lets call it MAC setup). It seems as if this complex sequence is only
	 * needed for modes that cannot be set by the SoC itself. Additionally it is unclear
	 * if this sequence should quit early in case of errors.
	 */

	mode_val = rtpcs_930x_sds_get_hw_mode_val(hw_mode);
	if (mode_val < 0) {
		pr_err("%s: SDS %d does not support mode %d\n", __func__,
		       sds->id, hw_mode);
		return mode_val;
	}

	rtpcs_930x_sds_set_power(sds, false);
	ret = __rtpcs_930x_sds_set_ip_mode(sds, RTPCS_930X_SDS_OFF);
	if (ret < 0)
		return ret;

	if (hw_mode == RTPCS_SDS_MODE_OFF)
		return 0;

	ret = rtpcs_93xx_sds_config_cmu(sds, hw_mode);
	if (ret < 0)
		pr_err("%s: SDS %d could not configure PLL for mode %d: %d\n", __func__,
		       sds->id, hw_mode, ret);

	ret = __rtpcs_930x_sds_set_ip_mode(sds, mode_val);
	if (ret < 0)
		return ret;

	if (rtpcs_930x_sds_wait_clock_ready(sds))
		pr_err("%s: SDS %d could not sync clock\n", __func__, sds->id);

	if (rtpcs_930x_sds_init_state_machine(sds, hw_mode))
		pr_err("%s: SDS %d could not reset state machine\n", __func__,
		       sds->id);

	rtpcs_930x_sds_set_power(sds, true);
	rtpcs_930x_sds_rx_reset(sds, hw_mode);
	return 0;
}

static int rtpcs_930x_sds_set_mac_mode(struct rtpcs_serdes *sds,
				       enum rtpcs_sds_mode hw_mode)
{
	int submode_val = -1;
	int mode_val, ret;

	mode_val = rtpcs_930x_sds_get_hw_mode_val(hw_mode);
	if (mode_val < 0)
		return mode_val;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
		submode_val = RTPCS_930X_SDS_SUBMODE_USXGMII_SX;
		break;
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
		submode_val = RTPCS_930X_SDS_SUBMODE_USXGMII_QX;
		break;
	default:
		break;
	}

	/* SerDes off first. */
	ret = __rtpcs_930x_sds_set_mac_mode(sds, RTPCS_930X_SDS_OFF);
	if (ret < 0)
		return ret;

	if (hw_mode == RTPCS_SDS_MODE_OFF)
		return 0;

	/* Set the mode. */
	ret = __rtpcs_930x_sds_set_mac_mode(sds, mode_val);
	if (ret < 0)
		return ret;

	/* Set the submode if needed. */
	if (submode_val >= 0) {
		ret = __rtpcs_930x_sds_set_usxgmii_submode(sds, submode_val);
		if (ret < 0)
			return ret;
	}

	return 0;
}

static int rtpcs_930x_sds_set_mode(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	/*
	 * Several modes can be configured via MAC setup, just by setting
	 * a register to a specific value and the MAC will configure
	 * "everything" as needed. For some modes, this seems incomplete and
	 * we need to do manual configuration in the SerDes IP core itself.
	 */

	switch (hw_mode) {
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_2500BASEX:
	case RTPCS_SDS_MODE_10GBASER:
		return rtpcs_930x_sds_set_ip_mode(sds, hw_mode);
	default:
		return rtpcs_930x_sds_set_mac_mode(sds, hw_mode);
	}
}

static void rtpcs_930x_sds_tx_config(struct rtpcs_serdes *sds,
				     enum rtpcs_sds_mode hw_mode)
{
	/* parameters: rtl9303_80G_txParam_s2 */
	int impedance = 0x8;
	int pre_amp = 0x2;
	int main_amp = 0x9;
	int post_amp = 0x2;
	int pre_en = 0x1;
	int post_en = 0x1;
	int page;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_SGMII:
		pre_amp = 0x1;
		main_amp = 0x9;
		post_amp = 0x1;
		page = 0x25;
		break;
	case RTPCS_SDS_MODE_2500BASEX:
		pre_amp = 0;
		post_amp = 0x8;
		pre_en = 0;
		page = 0x29;
		break;
	case RTPCS_SDS_MODE_10GBASER:
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	case RTPCS_SDS_MODE_XSGMII:
		pre_en = 0;
		pre_amp = 0;
		main_amp = 0x10;
		post_amp = 0;
		post_en	= 0;
		page = 0x2f;
		break;
	case RTPCS_SDS_MODE_QSGMII:
		return;
	default:
		pr_err("%s: unsupported SerDes hw mode\n", __func__);
		return;
	}

	rtpcs_sds_write_bits(sds, page, 0x01, 15, 11, pre_amp);
	rtpcs_sds_write_bits(sds, page, 0x06,  4,  0, post_amp);
	rtpcs_sds_write_bits(sds, page, 0x07,  0,  0, pre_en);
	rtpcs_sds_write_bits(sds, page, 0x07,  3,  3, post_en);
	rtpcs_sds_write_bits(sds, page, 0x07,  8,  4, main_amp);
	rtpcs_sds_write_bits(sds, page, 0x18, 15, 12, impedance);
}

__always_unused
static void rtpcs_930x_sds_rxcal_dcvs_manual(struct rtpcs_serdes *sds,
					     u32 dcvs_id, bool manual, u32 dvcs_list[])
{
	if (manual) {
		switch (dcvs_id) {
		case 0:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 14, 14, 0x1);
			rtpcs_sds_write_bits(sds, 0x2f, 0x03,  5,  5, dvcs_list[0]);
			rtpcs_sds_write_bits(sds, 0x2f, 0x03,  4,  0, dvcs_list[1]);
			break;
		case 1:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 13, 13, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x1d, 15, 15, dvcs_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x1d, 14, 11, dvcs_list[1]);
			break;
		case 2:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 12, 12, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x1d, 10, 10, dvcs_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x1d,  9,  6, dvcs_list[1]);
			break;
		case 3:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 11, 11, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x1d,  5,  5, dvcs_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x1d,  4,  1, dvcs_list[1]);
			break;
		case 4:
			rtpcs_sds_write_bits(sds, 0x2e, 0x01, 15, 15, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x11, 10, 10, dvcs_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x11,  9,  6, dvcs_list[1]);
			break;
		case 5:
			rtpcs_sds_write_bits(sds, 0x2e, 0x02, 11, 11, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x11,  4,  4, dvcs_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x11,  3,  0, dvcs_list[1]);
			break;
		default:
			break;
		}
	} else {
		switch (dcvs_id) {
		case 0:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 14, 14, 0x0);
			break;
		case 1:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 13, 13, 0x0);
			break;
		case 2:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 12, 12, 0x0);
			break;
		case 3:
			rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 11, 11, 0x0);
			break;
		case 4:
			rtpcs_sds_write_bits(sds, 0x2e, 0x01, 15, 15, 0x0);
			break;
		case 5:
			rtpcs_sds_write_bits(sds, 0x2e, 0x02, 11, 11, 0x0);
			break;
		default:
			break;
		}
		mdelay(1);
	}
}

__always_unused
static void rtpcs_930x_sds_rxcal_dcvs_get(struct rtpcs_serdes *sds,
					  u32 dcvs_id, u32 dcvs_list[])
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	u32 dcvs_sign_out = 0, dcvs_coef_bin = 0;
	bool dcvs_manual;

	if (sds == even_sds)
		rtpcs_sds_write(sds, 0x1f, 0x2, 0x2f);
	else
		rtpcs_sds_write(even_sds, 0x1f, 0x2, 0x31);

	/* ##Page0x2E, Reg0x15[9], REG0_RX_EN_TEST=[1] */
	rtpcs_sds_write_bits(sds, 0x2e, 0x15, 9, 9, 0x1);

	/* ##Page0x21, Reg0x06[11 6], REG0_RX_DEBUG_SEL=[1 0 x x x x] */
	rtpcs_sds_write_bits(sds, 0x21, 0x06, 11, 6, 0x20);

	switch (dcvs_id) {
	case 0:
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0x22);
		mdelay(1);

		/* ##DCVS0 Read Out */
		dcvs_sign_out = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  4,  4);
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  3,  0);
		dcvs_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x1e, 14, 14);
		break;

	case 1:
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0x23);
		mdelay(1);

		/* ##DCVS0 Read Out */
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  4,  4);
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  3,  0);
		dcvs_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x1e, 13, 13);
		break;

	case 2:
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0x24);
		mdelay(1);

		/* ##DCVS0 Read Out */
		dcvs_sign_out = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  4,  4);
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  3,  0);
		dcvs_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x1e, 12, 12);
		break;
	case 3:
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0x25);
		mdelay(1);

		/* ##DCVS0 Read Out */
		dcvs_sign_out = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  4,  4);
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  3,  0);
		dcvs_manual   = rtpcs_sds_read_bits(sds, 0x2e, 0x1e, 11, 11);
		break;

	case 4:
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0x2c);
		mdelay(1);

		/* ##DCVS0 Read Out */
		dcvs_sign_out = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  4,  4);
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  3,  0);
		dcvs_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x01, 15, 15);
		break;

	case 5:
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0x2d);
		mdelay(1);

		/* ##DCVS0 Read Out */
		dcvs_sign_out = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  4,  4);
		dcvs_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14,  3,  0);
		dcvs_manual   = rtpcs_sds_read_bits(sds, 0x2e, 0x02, 11, 11);
		break;

	default:
		break;
	}

	if (dcvs_sign_out)
		pr_info("%s DCVS %u Sign: -", __func__, dcvs_id);
	else
		pr_info("%s DCVS %u Sign: +", __func__, dcvs_id);

	pr_info("DCVS %u even coefficient = %u", dcvs_id, dcvs_coef_bin);
	pr_info("DCVS %u manual = %u", dcvs_id, dcvs_manual);

	dcvs_list[0] = dcvs_sign_out;
	dcvs_list[1] = dcvs_coef_bin;
}

static void rtpcs_930x_sds_rxcal_leq_manual(struct rtpcs_serdes *sds,
					    bool manual, u32 leq_gray)
{
	if (manual) {
		rtpcs_sds_write_bits(sds, 0x2e, 0x18, 15, 15, 0x1);
		rtpcs_sds_write_bits(sds, 0x2e, 0x16, 14, 10, leq_gray);
	} else {
		rtpcs_sds_write_bits(sds, 0x2e, 0x18, 15, 15, 0x0);
		mdelay(100);
	}
}

static void rtpcs_930x_sds_rxcal_leq_offset_manual(struct rtpcs_serdes *sds,
						   bool manual, u32 offset)
{
	if (manual) {
		rtpcs_sds_write_bits(sds, 0x2e, 0x17, 6, 2, offset);
	} else {
		rtpcs_sds_write_bits(sds, 0x2e, 0x17, 6, 2, offset);
		mdelay(1);
	}
}

#define GRAY_BITS 5
static u32 rtpcs_930x_sds_rxcal_gray_to_binary(u32 gray_code)
{
	int i, j, m;
	u32 g[GRAY_BITS];
	u32 c[GRAY_BITS];
	u32 leq_binary = 0;

	for (i = 0; i < GRAY_BITS; i++)
		g[i] = (gray_code & BIT(i)) >> i;

	m = GRAY_BITS - 1;

	c[m] = g[m];

	for (i = 0; i < m; i++) {
		c[i] = g[i];
		for (j  = i + 1; j < GRAY_BITS; j++)
			c[i] = c[i] ^ g[j];
	}

	for (i = 0; i < GRAY_BITS; i++)
		leq_binary += c[i] << i;

	return leq_binary;
}

static u32 rtpcs_930x_sds_rxcal_leq_read(struct rtpcs_serdes *sds)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	u32 leq_gray, leq_bin;
	bool leq_manual;

	if (sds == even_sds)
		rtpcs_sds_write(sds, 0x1f, 0x2, 0x2f);
	else
		rtpcs_sds_write(even_sds, 0x1f, 0x2, 0x31);

	/* ##Page0x2E, Reg0x15[9], REG0_RX_EN_TEST=[1] */
	rtpcs_sds_write_bits(sds, 0x2e, 0x15, 9, 9, 0x1);

	/* ##Page0x21, Reg0x06[11 6], REG0_RX_DEBUG_SEL=[0 1 x x x x] */
	rtpcs_sds_write_bits(sds, 0x21, 0x06, 11, 6, 0x10);
	mdelay(1);

	/* ##LEQ Read Out */
	leq_gray = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 7, 3);
	leq_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x18, 15, 15);
	leq_bin = rtpcs_930x_sds_rxcal_gray_to_binary(leq_gray);

	pr_info("LEQ_gray: %u, LEQ_bin: %u", leq_gray, leq_bin);
	pr_info("LEQ manual: %u", leq_manual);

	return leq_bin;
}

static void rtpcs_930x_sds_rxcal_vth_manual(struct rtpcs_serdes *sds,
					    bool manual, u32 vth_list[])
{
	if (manual) {
		rtpcs_sds_write_bits(sds, 0x2e, 0x0f, 13, 13, 0x1);
		rtpcs_sds_write_bits(sds, 0x2e, 0x13,  5,  3, vth_list[0]);
		rtpcs_sds_write_bits(sds, 0x2e, 0x13,  2,  0, vth_list[1]);
	} else {
		rtpcs_sds_write_bits(sds, 0x2e, 0x0f, 13, 13, 0x0);
		mdelay(10);
	}
}

static void rtpcs_930x_sds_rxcal_vth_get(struct rtpcs_serdes *sds,
					 u32 vth_list[])
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	u32 vth_manual;

	/* ##Page0x1F, Reg0x02[15 0], REG_DBGO_SEL=[0x002F]; */ /* Lane0 */
	/* ##Page0x1F, Reg0x02[15 0], REG_DBGO_SEL=[0x0031]; */ /* Lane1 */
	if (sds == even_sds)
		rtpcs_sds_write(sds, 0x1f, 0x2, 0x2f);
	else
		rtpcs_sds_write(even_sds, 0x1f, 0x2, 0x31);

	/* ##Page0x2E, Reg0x15[9], REG0_RX_EN_TEST=[1] */
	rtpcs_sds_write_bits(sds, 0x2e, 0x15, 9, 9, 0x1);
	/* ##Page0x21, Reg0x06[11 6], REG0_RX_DEBUG_SEL=[1 0 x x x x] */
	rtpcs_sds_write_bits(sds, 0x21, 0x06, 11, 6, 0x20);
	/* ##Page0x2F, Reg0x0C[5 0], REG0_COEF_SEL=[0 0 1 1 0 0] */
	rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0xc);

	mdelay(1);

	/* ##VthP & VthN Read Out */
	vth_list[0] = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 2, 0); /* v_thp set bin */
	vth_list[1] = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 5, 3); /* v_thn set bin */

	pr_info("vth_set_bin = %d", vth_list[0]);
	pr_info("vth_set_bin = %d", vth_list[1]);

	vth_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x0f, 13, 13);
	pr_info("Vth Maunal = %d", vth_manual);
}

static void rtpcs_930x_sds_rxcal_tap_manual(struct rtpcs_serdes *sds,
					    int tap_id, bool manual, u32 tap_list[])
{
	if (manual) {
		switch (tap_id) {
		case 0:
			/* ##REG0_LOAD_IN_INIT[0]=1; REG0_TAP0_INIT[5:0]=Tap0_Value */
			rtpcs_sds_write_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7, 0x1);
			rtpcs_sds_write_bits(sds, 0x2f, 0x03, 5, 5, tap_list[0]);
			rtpcs_sds_write_bits(sds, 0x2f, 0x03, 4, 0, tap_list[1]);
			break;
		case 1:
			rtpcs_sds_write_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7, 0x1);
			rtpcs_sds_write_bits(sds, 0x21, 0x07, 6, 6, tap_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x09, 11, 6, tap_list[1]);
			rtpcs_sds_write_bits(sds, 0x21, 0x07, 5, 5, tap_list[2]);
			rtpcs_sds_write_bits(sds, 0x2f, 0x12, 5, 0, tap_list[3]);
			break;
		case 2:
			rtpcs_sds_write_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x09, 5, 5, tap_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x09, 4, 0, tap_list[1]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x0a, 11, 11, tap_list[2]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x0a, 10, 6, tap_list[3]);
			break;
		case 3:
			rtpcs_sds_write_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7, 0x1);
			rtpcs_sds_write_bits(sds, 0x2e, 0x0a, 5, 5, tap_list[0]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x0a, 4, 0, tap_list[1]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x06, 5, 5, tap_list[2]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x06, 4, 0, tap_list[3]);
			break;
		case 4:
			rtpcs_sds_write_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7, 0x1);
			rtpcs_sds_write_bits(sds, 0x2f, 0x01, 5, 5, tap_list[0]);
			rtpcs_sds_write_bits(sds, 0x2f, 0x01, 4, 0, tap_list[1]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x06, 11, 11, tap_list[2]);
			rtpcs_sds_write_bits(sds, 0x2e, 0x06, 10, 6, tap_list[3]);
			break;
		default:
			break;
		}
	} else {
		rtpcs_sds_write_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7, 0x0);
		mdelay(10);
	}
}

static void rtpcs_930x_sds_rxcal_tap_get(struct rtpcs_serdes *sds,
					 u32 tap_id, u32 tap_list[])
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	u32 tap0_sign_out;
	u32 tap0_coef_bin;
	u32 tap_sign_out_even;
	u32 tap_coef_bin_even;
	u32 tap_sign_out_odd;
	u32 tap_coef_bin_odd;
	bool tap_manual;

	if (sds == even_sds)
		rtpcs_sds_write(sds, 0x1f, 0x2, 0x2f);
	else
		rtpcs_sds_write(even_sds, 0x1f, 0x2, 0x31);

	/* ##Page0x2E, Reg0x15[9], REG0_RX_EN_TEST=[1] */
	rtpcs_sds_write_bits(sds, 0x2e, 0x15, 9, 9, 0x1);
	/* ##Page0x21, Reg0x06[11 6], REG0_RX_DEBUG_SEL=[1 0 x x x x] */
	rtpcs_sds_write_bits(sds, 0x21, 0x06, 11, 6, 0x20);

	if (!tap_id) {
		/* ##Page0x2F, Reg0x0C[5 0], REG0_COEF_SEL=[0 0 0 0 0 1] */
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0);
		/* ##Tap1 Even Read Out */
		mdelay(1);
		tap0_sign_out = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 5, 5);
		tap0_coef_bin = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 4, 0);

		if (tap0_sign_out == 1)
			pr_info("Tap0 Sign : -");
		else
			pr_info("Tap0 Sign : +");

		pr_info("tap0_coef_bin = %d", tap0_coef_bin);

		tap_list[0] = tap0_sign_out;
		tap_list[1] = tap0_coef_bin;

		tap_manual = !!rtpcs_sds_read_bits(sds, 0x2e, 0x0f, 7, 7);
		pr_info("tap0 manual = %u", tap_manual);
	} else {
		/* ##Page0x2F, Reg0x0C[5 0], REG0_COEF_SEL=[0 0 0 0 0 1] */
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, tap_id);
		mdelay(1);
		/* ##Tap1 Even Read Out */
		tap_sign_out_even = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 5, 5);
		tap_coef_bin_even = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 4, 0);

		/* ##Page0x2F, Reg0x0C[5 0], REG0_COEF_SEL=[0 0 0 1 1 0] */
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, (tap_id + 5));
		/* ##Tap1 Odd Read Out */
		tap_sign_out_odd = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 5, 5);
		tap_coef_bin_odd = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 4, 0);

		if (tap_sign_out_even == 1)
			pr_info("Tap %u even sign: -", tap_id);
		else
			pr_info("Tap %u even sign: +", tap_id);

		pr_info("Tap %u even coefficient = %u", tap_id, tap_coef_bin_even);

		if (tap_sign_out_odd == 1)
			pr_info("Tap %u odd sign: -", tap_id);
		else
			pr_info("Tap %u odd sign: +", tap_id);

		pr_info("Tap %u odd coefficient = %u", tap_id, tap_coef_bin_odd);

		tap_list[0] = tap_sign_out_even;
		tap_list[1] = tap_coef_bin_even;
		tap_list[2] = tap_sign_out_odd;
		tap_list[3] = tap_coef_bin_odd;

		tap_manual = rtpcs_sds_read_bits(sds, 0x2e, 0x0f, tap_id + 7, tap_id + 7);
		pr_info("tap %u manual = %d", tap_id, tap_manual);
	}
}

static void rtpcs_930x_sds_do_rx_calibration_1(struct rtpcs_serdes *sds,
					       enum rtpcs_sds_mode hw_mode)
{
	/* From both rtl9300_rxCaliConf_serdes_myParam and rtl9300_rxCaliConf_phy_myParam */
	int tap0_init_val = 0x1f; /* Initial Decision Fed Equalizer 0 tap */
	int vth_min       = 0x0;

	pr_info("start_1.1.1 initial value for sds %d\n", sds->id);
	rtpcs_sds_write(sds, 6,  0, 0);

	/* FGCAL */
	rtpcs_sds_write_bits(sds, 0x2e, 0x01, 14, 14, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x1c, 10,  5, 0x20);
	rtpcs_sds_write_bits(sds, 0x2f, 0x02,  0,  0, 0x01);

	/* DCVS */
	rtpcs_sds_write_bits(sds, 0x2e, 0x1e, 14, 11, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x01, 15, 15, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x02, 11, 11, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x1c,  4,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x1d, 15, 11, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x1d, 10,  6, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x1d,  5,  1, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x02, 10,  6, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x11,  4,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2f, 0x00,  3,  0, 0x0f);
	rtpcs_sds_write_bits(sds, 0x2e, 0x04,  6,  6, 0x01);
	rtpcs_sds_write_bits(sds, 0x2e, 0x04,  7,  7, 0x01);

	/* LEQ (Long Term Equivalent signal level) */
	rtpcs_sds_write_bits(sds, 0x2e, 0x16, 14,  8, 0x00);

	/* DFE (Decision Fed Equalizer) */
	rtpcs_sds_write_bits(sds, 0x2f, 0x03,  5,  0, tap0_init_val);
	rtpcs_sds_write_bits(sds, 0x2e, 0x09, 11,  6, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x09,  5,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x0a,  5,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2f, 0x01,  5,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2f, 0x12,  5,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x0a, 11,  6, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x06,  5,  0, 0x00);
	rtpcs_sds_write_bits(sds, 0x2f, 0x01,  5,  0, 0x00);

	/* Vth */
	rtpcs_sds_write_bits(sds, 0x2e, 0x13,  5,  3, 0x07);
	rtpcs_sds_write_bits(sds, 0x2e, 0x13,  2,  0, 0x07);
	rtpcs_sds_write_bits(sds, 0x2f, 0x0b,  5,  3, vth_min);

	pr_info("end_1.1.1 --\n");

	pr_info("start_1.1.2 Load DFE init. value\n");

	rtpcs_sds_write_bits(sds, 0x2e, 0x0f, 13,  7, 0x7f);

	pr_info("end_1.1.2\n");

	pr_info("start_1.1.3 disable LEQ training,enable DFE clock\n");

	rtpcs_sds_write_bits(sds, 0x2e, 0x17,  7,  7, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x17,  6,  2, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x0c,  8,  8, 0x00);
	rtpcs_sds_write_bits(sds, 0x2e, 0x0b,  4,  4, 0x01);
	rtpcs_sds_write_bits(sds, 0x2e, 0x12, 14, 14, 0x00);
	rtpcs_sds_write_bits(sds, 0x2f, 0x02, 15, 15, 0x00);

	pr_info("end_1.1.3 --\n");

	pr_info("start_1.1.4 offset cali setting\n");

	rtpcs_sds_write_bits(sds, 0x2e, 0x0f, 15, 14, 0x03);

	pr_info("end_1.1.4\n");

	pr_info("start_1.1.5 LEQ and DFE setting\n");

	/* TODO: make this work for DAC cables of different lengths */
	/* For a 10GBit serdes wit Fibre, SDS 8 or 9 */
	if (hw_mode == RTPCS_SDS_MODE_10GBASER ||
	    hw_mode == RTPCS_SDS_MODE_1000BASEX ||
	    hw_mode == RTPCS_SDS_MODE_SGMII)
		rtpcs_sds_write_bits(sds, 0x2e, 0x16,  3,  2, 0x02);
	else
		pr_err("%s not PHY-based or SerDes, implement DAC!\n", __func__);

	/* No serdes, check for Aquantia PHYs */
	rtpcs_sds_write_bits(sds, 0x2e, 0x16,  3,  2, 0x02);

	rtpcs_sds_write_bits(sds, 0x2e, 0x0f,  6,  0, 0x5f);
	rtpcs_sds_write_bits(sds, 0x2f, 0x05,  7,  2, 0x1f);
	rtpcs_sds_write_bits(sds, 0x2e, 0x19,  9,  5, 0x1f);
	rtpcs_sds_write_bits(sds, 0x2f, 0x0b, 15,  9, 0x3c);
	rtpcs_sds_write_bits(sds, 0x2e, 0x0b,  1,  0, 0x03);

	pr_info("end_1.1.5\n");
}

static void rtpcs_930x_sds_do_rx_calibration_2_1(struct rtpcs_serdes *sds)
{
	pr_info("start_1.2.1 ForegroundOffsetCal_Manual\n");

	/* Gray config endis to 1 */
	rtpcs_sds_write_bits(sds, 0x2f, 0x02,  2,  2, 0x01);

	/* ForegroundOffsetCal_Manual(auto mode) */
	rtpcs_sds_write_bits(sds, 0x2e, 0x01, 14, 14, 0x00);

	pr_info("end_1.2.1");
}

static void rtpcs_930x_sds_do_rx_calibration_2_2(struct rtpcs_serdes *sds)
{
	/* Force Rx-Run = 0 */
	rtpcs_sds_write_bits(sds, 0x2e, 0x15, 8, 8, 0x0);

	rtpcs_930x_sds_rx_reset(sds, RTPCS_SDS_MODE_10GBASER);
}

static void rtpcs_930x_sds_do_rx_calibration_2_3(struct rtpcs_serdes *sds)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	u32 fgcal_binary, fgcal_gray;
	u32 offset_range;

	pr_info("start_1.2.3 Foreground Calibration\n");

	while (1) {
		if (sds == even_sds)
			rtpcs_sds_write(sds, 0x1f, 0x2, 0x2f);
		else
			rtpcs_sds_write(even_sds, 0x1f, 0x2, 0x31);

		/* ##Page0x2E, Reg0x15[9], REG0_RX_EN_TEST=[1] */
		rtpcs_sds_write_bits(sds, 0x2e, 0x15, 9, 9, 0x1);
		/* ##Page0x21, Reg0x06[11 6], REG0_RX_DEBUG_SEL=[1 0 x x x x] */
		rtpcs_sds_write_bits(sds, 0x21, 0x06, 11, 6, 0x20);
		/* ##Page0x2F, Reg0x0C[5 0], REG0_COEF_SEL=[0 0 1 1 1 1] */
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0xf);
		/* ##FGCAL read gray */
		fgcal_gray = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 5, 0);
		/* ##Page0x2F, Reg0x0C[5 0], REG0_COEF_SEL=[0 0 1 1 1 0] */
		rtpcs_sds_write_bits(sds, 0x2f, 0x0c, 5, 0, 0xe);
		/* ##FGCAL read binary */
		fgcal_binary = rtpcs_sds_read_bits(sds, 0x1f, 0x14, 5, 0);

		pr_info("%s: fgcal_gray: %d, fgcal_binary %d\n",
			__func__, fgcal_gray, fgcal_binary);

		offset_range = rtpcs_sds_read_bits(sds, 0x2e, 0x15, 15, 14);

		if (fgcal_binary <= 60 && fgcal_binary >= 3)
			break;

		if (offset_range == 3) {
			pr_info("%s: Foreground Calibration result marginal!", __func__);
			break;
		}

		offset_range++;
		rtpcs_sds_write_bits(sds, 0x2e, 0x15, 15, 14, offset_range);
		rtpcs_930x_sds_do_rx_calibration_2_2(sds);
	}
	pr_info("%s: end_1.2.3\n", __func__);
}

static void rtpcs_930x_sds_do_rx_calibration_2(struct rtpcs_serdes *sds)
{
	rtpcs_930x_sds_rx_reset(sds, RTPCS_SDS_MODE_10GBASER);
	rtpcs_930x_sds_do_rx_calibration_2_1(sds);
	rtpcs_930x_sds_do_rx_calibration_2_2(sds);
	rtpcs_930x_sds_do_rx_calibration_2_3(sds);
}

static void rtpcs_930x_sds_rxcal_3_1(struct rtpcs_serdes *sds,
				     enum rtpcs_sds_mode hw_mode)
{
	pr_info("start_1.3.1");

	/* ##1.3.1 */
	if (hw_mode != RTPCS_SDS_MODE_10GBASER &&
	    hw_mode != RTPCS_SDS_MODE_1000BASEX &&
	    hw_mode != RTPCS_SDS_MODE_SGMII)
		rtpcs_sds_write_bits(sds, 0x2e, 0xc, 8, 8, 0);

	rtpcs_sds_write_bits(sds, 0x2e, 0x17, 7, 7, 0x0);
	rtpcs_930x_sds_rxcal_leq_manual(sds, false, 0);

	pr_info("end_1.3.1");
}

static void rtpcs_930x_sds_rxcal_3_2(struct rtpcs_serdes *sds,
				     enum rtpcs_sds_mode hw_mode)
{
	u32 sum10 = 0, avg10, int10;
	int dac_long_cable_offset;
	bool eq_hold_enabled;
	int i;

	if (hw_mode == RTPCS_SDS_MODE_10GBASER ||
	    hw_mode == RTPCS_SDS_MODE_1000BASEX ||
	    hw_mode == RTPCS_SDS_MODE_SGMII) {
		/* rtl9300_rxCaliConf_serdes_myParam */
		dac_long_cable_offset = 3;
		eq_hold_enabled = true;
	} else {
		/* rtl9300_rxCaliConf_phy_myParam */
		dac_long_cable_offset = 0;
		eq_hold_enabled = false;
	}

	if (hw_mode != RTPCS_SDS_MODE_10GBASER)
		pr_warn("%s: LEQ only valid for 10GR!\n", __func__);

	pr_info("start_1.3.2");

	for (i = 0; i < 10; i++) {
		sum10 += rtpcs_930x_sds_rxcal_leq_read(sds);
		mdelay(10);
	}

	avg10 = (sum10 / 10) + (((sum10 % 10) >= 5) ? 1 : 0);
	int10 = sum10 / 10;

	pr_info("sum10:%u, avg10:%u, int10:%u", sum10, avg10, int10);

	if (hw_mode == RTPCS_SDS_MODE_10GBASER ||
	    hw_mode == RTPCS_SDS_MODE_1000BASEX ||
	    hw_mode == RTPCS_SDS_MODE_SGMII) {
		if (dac_long_cable_offset) {
			rtpcs_930x_sds_rxcal_leq_offset_manual(sds, 1,
							       dac_long_cable_offset);
			rtpcs_sds_write_bits(sds, 0x2e, 0x17, 7, 7,
					     eq_hold_enabled);
			if (hw_mode == RTPCS_SDS_MODE_10GBASER)
				rtpcs_930x_sds_rxcal_leq_manual(sds,
								true, avg10);
		} else {
			if (sum10 >= 5) {
				rtpcs_930x_sds_rxcal_leq_offset_manual(sds, 1, 3);
				rtpcs_sds_write_bits(sds, 0x2e, 0x17, 7, 7, 0x1);
				if (hw_mode == RTPCS_SDS_MODE_10GBASER)
					rtpcs_930x_sds_rxcal_leq_manual(sds, true, avg10);
			} else {
				rtpcs_930x_sds_rxcal_leq_offset_manual(sds, 1, 0);
				rtpcs_sds_write_bits(sds, 0x2e, 0x17, 7, 7, 0x1);
				if (hw_mode == RTPCS_SDS_MODE_10GBASER)
					rtpcs_930x_sds_rxcal_leq_manual(sds, true, avg10);
			}
		}
	}

	pr_info("Sds:%u LEQ = %u", sds->id, rtpcs_930x_sds_rxcal_leq_read(sds));

	pr_info("end_1.3.2");
}

__always_unused
static void rtpcs_930x_sds_do_rx_calibration_3(struct rtpcs_serdes *sds,
					       enum rtpcs_sds_mode hw_mode)
{
	rtpcs_930x_sds_rxcal_3_1(sds, hw_mode);

	if (hw_mode == RTPCS_SDS_MODE_10GBASER ||
	    hw_mode == RTPCS_SDS_MODE_1000BASEX ||
	    hw_mode == RTPCS_SDS_MODE_SGMII)
		rtpcs_930x_sds_rxcal_3_2(sds, hw_mode);
}

static void rtpcs_930x_sds_do_rx_calibration_4_1(struct rtpcs_serdes *sds)
{
	u32 vth_list[2] = {0, 0};
	u32 tap0_list[4] = {0, 0, 0, 0};

	pr_info("start_1.4.1");

	/* ##1.4.1 */
	rtpcs_930x_sds_rxcal_vth_manual(sds, false, vth_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 0, false, tap0_list);
	mdelay(200);

	pr_info("end_1.4.1");
}

static void rtpcs_930x_sds_do_rx_calibration_4_2(struct rtpcs_serdes *sds)
{
	u32 vth_list[2];
	u32 tap_list[4];

	pr_info("start_1.4.2");

	rtpcs_930x_sds_rxcal_vth_get(sds, vth_list);
	rtpcs_930x_sds_rxcal_vth_manual(sds, true, vth_list);

	mdelay(100);

	rtpcs_930x_sds_rxcal_tap_get(sds, 0, tap_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 0, true, tap_list);

	pr_info("end_1.4.2");
}

static void rtpcs_930x_sds_do_rx_calibration_4(struct rtpcs_serdes *sds)
{
	rtpcs_930x_sds_do_rx_calibration_4_1(sds);
	rtpcs_930x_sds_do_rx_calibration_4_2(sds);
}

static void rtpcs_930x_sds_do_rx_calibration_5_2(struct rtpcs_serdes *sds)
{
	u32 tap1_list[4] = {0};
	u32 tap2_list[4] = {0};
	u32 tap3_list[4] = {0};
	u32 tap4_list[4] = {0};

	pr_info("start_1.5.2");

	rtpcs_930x_sds_rxcal_tap_manual(sds, 1, false, tap1_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 2, false, tap2_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 3, false, tap3_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 4, false, tap4_list);

	mdelay(30);

	pr_info("end_1.5.2");
}

static void rtpcs_930x_sds_do_rx_calibration_5(struct rtpcs_serdes *sds,
					       enum rtpcs_sds_mode hw_mode)
{
	if (hw_mode == RTPCS_SDS_MODE_10GBASER) /* dfeTap1_4Enable true */
		rtpcs_930x_sds_do_rx_calibration_5_2(sds);
}

static void rtpcs_930x_sds_do_rx_calibration_dfe_disable(struct rtpcs_serdes *sds)
{
	u32 tap1_list[4] = {0};
	u32 tap2_list[4] = {0};
	u32 tap3_list[4] = {0};
	u32 tap4_list[4] = {0};

	rtpcs_930x_sds_rxcal_tap_manual(sds, 1, true, tap1_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 2, true, tap2_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 3, true, tap3_list);
	rtpcs_930x_sds_rxcal_tap_manual(sds, 4, true, tap4_list);

	mdelay(10);
}

static void rtpcs_930x_sds_do_rx_calibration(struct rtpcs_serdes *sds,
					     enum rtpcs_sds_mode hw_mode)
{
	u32 latch_sts;

	rtpcs_930x_sds_do_rx_calibration_1(sds, hw_mode);
	rtpcs_930x_sds_do_rx_calibration_2(sds);
	rtpcs_930x_sds_do_rx_calibration_4(sds);
	rtpcs_930x_sds_do_rx_calibration_5(sds, hw_mode);
	mdelay(20);

	/* Do this only for 10GR mode, SDS active in mode 0x1a */
	if (rtpcs_sds_read_bits(sds, 0x1f, 9, 11, 7) == RTPCS_930X_SDS_MODE_10GBASER) {
		pr_info("%s: SDS enabled\n", __func__);
		latch_sts = rtpcs_sds_read_bits(sds, 0x4, 1, 2, 2);
		mdelay(1);
		latch_sts = rtpcs_sds_read_bits(sds, 0x4, 1, 2, 2);
		if (latch_sts) {
			rtpcs_930x_sds_do_rx_calibration_dfe_disable(sds);
			rtpcs_930x_sds_do_rx_calibration_4(sds);
			rtpcs_930x_sds_do_rx_calibration_5(sds, hw_mode);
		}
	}
}

static int rtpcs_930x_sds_sym_err_reset(struct rtpcs_serdes *sds,
					enum rtpcs_sds_mode hw_mode)
{
	int channel, channels;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_10GBASER:
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
		/* Read twice to clear */
		rtpcs_sds_read(sds, 0x5, 0x1);
		rtpcs_sds_read(sds, 0x5, 0x1);
		return 0;

	case RTPCS_SDS_MODE_XSGMII:
	case RTPCS_SDS_MODE_QSGMII:
		channels = 4;
		break;

	default:
		channels = 1;
	}

	for (channel = 0; channel < channels; channel++) {
		if (hw_mode == RTPCS_SDS_MODE_XSGMII) {
			rtpcs_sds_xsg_write_bits(sds, 0x1, 0x18, 2, 0, channel);
			rtpcs_sds_xsg_write_bits(sds, 0x1, 0x3, 15, 8, 0x0);
			rtpcs_sds_xsg_write_bits(sds, 0x1, 0x2, 15, 0, 0x0);
		} else {
			rtpcs_sds_write_bits(sds, 0x1, 0x18, 2, 0, channel);
			rtpcs_sds_write_bits(sds, 0x1, 0x3, 15, 8, 0x0);
			rtpcs_sds_write_bits(sds, 0x1, 0x2, 15, 0, 0x0);
		}
	}

	if (channels > 1) {
		if (hw_mode == RTPCS_SDS_MODE_XSGMII) {
			rtpcs_sds_xsg_write_bits(sds, 0x1, 0x0, 15, 0, 0x0);
			rtpcs_sds_xsg_write_bits(sds, 0x1, 0x1, 15, 8, 0x0);
		} else {
			rtpcs_sds_write_bits(sds, 0x1, 0x0, 15, 0, 0x0);
			rtpcs_sds_write_bits(sds, 0x1, 0x1, 15, 8, 0x0);
		}
	}

	return 0;
}

static u32 rtpcs_930x_sds_sym_err_get(struct rtpcs_serdes *sds,
				      enum rtpcs_sds_mode hw_mode)
{
	u32 v = 0;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_QSGMII:
	case RTPCS_SDS_MODE_XSGMII:
		v = rtpcs_sds_read_bits(sds, 0x1, 0x1, 15, 8) << 16;
		v |= rtpcs_sds_read_bits(sds, 0x1, 0x0, 15, 0);
		break;

	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
		break;

	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_10GBASER:
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
		v = rtpcs_sds_read(sds, 0x5, 0x1);
		return v & 0xff;

	default:
		pr_info("%s unsupported PHY-mode\n", __func__);
	}

	return v;
}

static int rtpcs_930x_sds_check_calibration(struct rtpcs_serdes *sds,
					    enum rtpcs_sds_mode hw_mode)
{
	u32 errors1, errors2;

	rtpcs_930x_sds_sym_err_reset(sds, hw_mode);
	rtpcs_930x_sds_sym_err_reset(sds, hw_mode);

	/* Count errors during 1ms */
	errors1 = rtpcs_930x_sds_sym_err_get(sds, hw_mode);
	mdelay(1);
	errors2 = rtpcs_930x_sds_sym_err_get(sds, hw_mode);

	switch (hw_mode) {
	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_XSGMII:
		if ((errors2 - errors1 > 100) ||
		    (errors1 >= 0xffff00) || (errors2 >= 0xffff00)) {
			pr_info("%s XSGMII error rate too high\n", __func__);
			return 1;
		}
		break;
	case RTPCS_SDS_MODE_10GBASER:
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
		if (errors2 > 0) {
			pr_info("%s: 10G error rate too high\n", __func__);
			return 1;
		}
		break;
	default:
		return 1;
	}

	return 0;
}

static void rtpcs_930x_phy_enable_10g_1g(struct rtpcs_serdes *sds)
{

	u32 v;

	/* Enable 1GBit PHY */
	v = rtpcs_sds_read(sds, PHY_PAGE_2, MII_BMCR);
	pr_info("%s 1gbit phy: %08x\n", __func__, v);
	v &= ~BMCR_PDOWN;
	rtpcs_sds_write(sds, PHY_PAGE_2, MII_BMCR, v);
	pr_info("%s 1gbit phy enabled: %08x\n", __func__, v);

	/* Enable 10GBit PHY */
	v = rtpcs_sds_read(sds, PHY_PAGE_4, MII_BMCR);
	pr_info("%s 10gbit phy: %08x\n", __func__, v);
	v &= ~BMCR_PDOWN;
	rtpcs_sds_write(sds, PHY_PAGE_4, MII_BMCR, v);
	pr_info("%s 10gbit phy after: %08x\n", __func__, v);

	/* dal_longan_construct_mac_default_10gmedia_fiber */
	v = rtpcs_sds_read(sds, 0x1f, 11);
	pr_info("%s set medium: %08x\n", __func__, v);
	v |= BIT(1);
	rtpcs_sds_write(sds, 0x1f, 11, v);
	pr_info("%s set medium after: %08x\n", __func__, v);
}

static int rtpcs_930x_sds_10g_idle(struct rtpcs_serdes *sds)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	bool busy;
	int i = 0;

	do {
		if (sds == even_sds) {
			rtpcs_sds_write_bits(sds, 0x1f, 0x2, 15, 0, 53);
			busy = !!rtpcs_sds_read_bits(sds, 0x1f, 0x14, 0, 0);
		} else {
			rtpcs_sds_write_bits(even_sds, 0x1f, 0x2, 15, 0, 53);
			busy = !!rtpcs_sds_read_bits(even_sds, 0x1f, 0x14, 1, 1);
		}
		i++;
	} while (busy && i < 100);

	if (i < 100)
		return 0;

	pr_warn("%s WARNING: Waiting for RX idle timed out, SDS %d\n",
		__func__, sds->id);
	return -EIO;
}

static int rtpcs_930x_sds_set_polarity(struct rtpcs_serdes *sds,
				       bool tx_inv, bool rx_inv)
{
	u8 rx_val = rx_inv ? 1 : 0;
	u8 tx_val = tx_inv ? 1 : 0;
	u32 val;
	int ret;

	/* 10GR */
	val = (tx_val << 1) | rx_val;
	ret = rtpcs_sds_write_bits(sds, 0x6, 0x2, 14, 13, val);
	if (ret)
		return ret;

	/* 1G */
	val = (rx_val << 1) | tx_val;
	return rtpcs_sds_write_bits(sds, 0x0, 0x0, 9, 8, val);
}

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_10gr_even[] = {
	/* 1G */
	{0x00, 0x0E, 0x3053}, {0x01, 0x14, 0x0100}, {0x21, 0x03, 0x8206},
	{0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F},
	{0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009}, {0x21, 0x0E, 0x0000},
	{0x21, 0x0F, 0x0008}, {0x24, 0x00, 0x0668}, {0x24, 0x02, 0xD020},
	{0x24, 0x06, 0xC000}, {0x24, 0x0B, 0x1892}, {0x24, 0x0F, 0xFFDF},
	{0x24, 0x12, 0x03C4}, {0x24, 0x13, 0x027F}, {0x24, 0x14, 0x1311},
	{0x24, 0x16, 0x00C9}, {0x24, 0x17, 0xA100}, {0x24, 0x1A, 0x0001},
	{0x24, 0x1C, 0x0400}, {0x25, 0x01, 0x0300}, {0x25, 0x02, 0x1017},
	{0x25, 0x03, 0xFFDF}, {0x25, 0x05, 0x7F7C}, {0x25, 0x07, 0x8100},
	{0x25, 0x08, 0x0001}, {0x25, 0x09, 0xFFD4}, {0x25, 0x0A, 0x7C2F},
	{0x25, 0x0E, 0x003F}, {0x25, 0x0F, 0x0121}, {0x25, 0x10, 0x0020},
	{0x25, 0x11, 0x8840}, {0x2B, 0x13, 0x0050}, {0x2B, 0x18, 0x8E88},
	{0x2B, 0x19, 0x4902}, {0x2B, 0x1D, 0x2501}, {0x2D, 0x13, 0x0050},
	{0x2D, 0x18, 0x8E88}, {0x2D, 0x19, 0x4902}, {0x2D, 0x1D, 0x2641},
	{0x2F, 0x13, 0x0050}, {0x2F, 0x18, 0x8E88}, {0x2F, 0x19, 0x4902},
	{0x2F, 0x1D, 0x66E1},
	/* 3.125G */
	{0x28, 0x00, 0x0668}, {0x28, 0x02, 0xD020}, {0x28, 0x06, 0xC000},
	{0x28, 0x0B, 0x1892}, {0x28, 0x0F, 0xFFDF}, {0x28, 0x12, 0x01C4},
	{0x28, 0x13, 0x027F}, {0x28, 0x14, 0x1311}, {0x28, 0x16, 0x00C9},
	{0x28, 0x17, 0xA100}, {0x28, 0x1A, 0x0001}, {0x28, 0x1C, 0x0400},
	{0x29, 0x01, 0x0300}, {0x29, 0x02, 0x1017}, {0x29, 0x03, 0xFFDF},
	{0x29, 0x05, 0x7F7C}, {0x29, 0x07, 0x8100}, {0x29, 0x08, 0x0001},
	{0x29, 0x09, 0xFFD4}, {0x29, 0x0A, 0x7C2F}, {0x29, 0x0E, 0x003F},
	{0x29, 0x0F, 0x0121}, {0x29, 0x10, 0x0020}, {0x29, 0x11, 0x8840},
	/* 10G */
	{0x06, 0x0D, 0x0F00}, {0x06, 0x00, 0x0000}, {0x06, 0x01, 0xC800},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010},
	{0x21, 0x07, 0xF09F}, {0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009},
	{0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008}, {0x2E, 0x00, 0xA668},
	{0x2E, 0x02, 0xD020}, {0x2E, 0x06, 0xC000}, {0x2E, 0x0B, 0x1892},
	{0x2E, 0x0F, 0xFFDF}, {0x2E, 0x11, 0x8280}, {0x2E, 0x12, 0x0044},
	{0x2E, 0x13, 0x027F}, {0x2E, 0x14, 0x1311}, {0x2E, 0x17, 0xA100},
	{0x2E, 0x1A, 0x0001}, {0x2E, 0x1C, 0x0400}, {0x2F, 0x01, 0x0300},
	{0x2F, 0x02, 0x1217}, {0x2F, 0x03, 0xFFDF}, {0x2F, 0x05, 0x7F7C},
	{0x2F, 0x07, 0x80C4}, {0x2F, 0x08, 0x0001}, {0x2F, 0x09, 0xFFD4},
	{0x2F, 0x0A, 0x7C2F}, {0x2F, 0x0E, 0x003F}, {0x2F, 0x0F, 0x0121},
	{0x2F, 0x10, 0x0020}, {0x2F, 0x11, 0x8840}, {0x2F, 0x14, 0xE008},
	{0x2B, 0x13, 0x0050}, {0x2B, 0x18, 0x8E88}, {0x2B, 0x19, 0x4902},
	{0x2B, 0x1D, 0x2501}, {0x2D, 0x13, 0x0050}, {0x2D, 0x17, 0x4109},
	{0x2D, 0x18, 0x8E88}, {0x2D, 0x19, 0x4902}, {0x2D, 0x1C, 0x1109},
	{0x2D, 0x1D, 0x2641}, {0x2F, 0x13, 0x0050}, {0x2F, 0x18, 0x8E88},
	{0x2F, 0x19, 0x4902}, {0x2F, 0x1D, 0x76E1},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_10gr_odd[] = {
	/* 1G */
	{0x00, 0x0E, 0x3053}, {0x01, 0x14, 0x0100}, {0x21, 0x03, 0x8206},
	{0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F}, {0x21, 0x0A, 0x0003},
	{0x21, 0x0B, 0x0005}, {0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009},
	{0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008}, {0x24, 0x00, 0x0668},
	{0x24, 0x02, 0xD020}, {0x24, 0x06, 0xC000}, {0x24, 0x0B, 0x1892},
	{0x24, 0x0F, 0xFFDF}, {0x24, 0x12, 0x03C4}, {0x24, 0x13, 0x027F},
	{0x24, 0x14, 0x1311}, {0x24, 0x16, 0x00C9}, {0x24, 0x17, 0xA100},
	{0x24, 0x1A, 0x0001}, {0x24, 0x1C, 0x0400}, {0x25, 0x00, 0x820F},
	{0x25, 0x01, 0x0300}, {0x25, 0x02, 0x1017}, {0x25, 0x03, 0xFFDF},
	{0x25, 0x05, 0x7F7C}, {0x25, 0x07, 0x8100}, {0x25, 0x08, 0x0001},
	{0x25, 0x09, 0xFFD4}, {0x25, 0x0A, 0x7C2F}, {0x25, 0x0E, 0x003F},
	{0x25, 0x0F, 0x0121}, {0x25, 0x10, 0x0020}, {0x25, 0x11, 0x8840},
	{0x2B, 0x13, 0x3D87}, {0x2B, 0x14, 0x3108}, {0x2D, 0x13, 0x3C87},
	{0x2D, 0x14, 0x1808},
	/* 3.125G */
	{0x28, 0x00, 0x0668}, {0x28, 0x02, 0xD020}, {0x28, 0x06, 0xC000},
	{0x28, 0x0B, 0x1892}, {0x28, 0x0F, 0xFFDF}, {0x28, 0x12, 0x01C4},
	{0x28, 0x13, 0x027F}, {0x28, 0x14, 0x1311}, {0x28, 0x16, 0x00C9},
	{0x28, 0x17, 0xA100}, {0x28, 0x1A, 0x0001}, {0x28, 0x1C, 0x0400},
	{0x29, 0x00, 0x820F}, {0x29, 0x01, 0x0300}, {0x29, 0x02, 0x1017},
	{0x29, 0x03, 0xFFDF}, {0x29, 0x05, 0x7F7C}, {0x29, 0x07, 0x8100},
	{0x29, 0x08, 0x0001}, {0x29, 0x0A, 0x7C2F}, {0x29, 0x0E, 0x003F},
	{0x29, 0x0F, 0x0121}, {0x29, 0x10, 0x0020}, {0x29, 0x11, 0x8840},
	/* 10G */
	{0x06, 0x0D, 0x0F00}, {0x06, 0x00, 0x0000}, {0x06, 0x01, 0xC800},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010},
	{0x21, 0x07, 0xF09F}, {0x21, 0x0A, 0x0003}, {0x21, 0x0B, 0x0005},
	{0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009}, {0x21, 0x0E, 0x0000},
	{0x21, 0x0F, 0x0008}, {0x2E, 0x00, 0xA668}, {0x2E, 0x02, 0xD020},
	{0x2E, 0x06, 0xC000}, {0x2E, 0x0B, 0x1892}, {0x2E, 0x0F, 0xFFDF},
	{0x2E, 0x11, 0x8280}, {0x2E, 0x12, 0x0044}, {0x2E, 0x13, 0x027F},
	{0x2E, 0x14, 0x1311}, {0x2E, 0x17, 0xA100}, {0x2E, 0x1A, 0x0001},
	{0x2E, 0x1C, 0x0400}, {0x2F, 0x00, 0x820F}, {0x2F, 0x01, 0x0300},
	{0x2F, 0x02, 0x1217}, {0x2F, 0x03, 0xFFDF}, {0x2F, 0x05, 0x7F7C},
	{0x2F, 0x07, 0x80C4}, {0x2F, 0x08, 0x0001}, {0x2F, 0x09, 0xFFD4},
	{0x2F, 0x0A, 0x7C2F}, {0x2F, 0x0E, 0x003F}, {0x2F, 0x0F, 0x0121},
	{0x2F, 0x10, 0x0020}, {0x2F, 0x11, 0x8840}, {0x2B, 0x13, 0x3D87},
	{0x2B, 0x14, 0x3108}, {0x2D, 0x13, 0x3C87}, {0x2D, 0x14, 0x1808},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_10g_2500bx_even[] = {
	{0x00, 0x0E, 0x3053}, {0x01, 0x14, 0x0100},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F},
	{0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009}, {0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008},
	{0x24, 0x00, 0x0668}, {0x24, 0x02, 0xD020}, {0x24, 0x06, 0xC000}, {0x24, 0x0B, 0x1892},
	{0x24, 0x0F, 0xFFDF}, {0x24, 0x12, 0x03C4}, {0x24, 0x13, 0x027F}, {0x24, 0x14, 0x1311},
	{0x24, 0x16, 0x00C9}, {0x24, 0x17, 0xA100}, {0x24, 0x1A, 0x0001}, {0x24, 0x1C, 0x0400},
	{0x25, 0x01, 0x0300}, {0x25, 0x02, 0x1017}, {0x25, 0x03, 0xFFDF}, {0x25, 0x05, 0x7F7C},
	{0x25, 0x07, 0x8100}, {0x25, 0x08, 0x0001}, {0x25, 0x09, 0xFFD4}, {0x25, 0x0A, 0x7C2F},
	{0x25, 0x0E, 0x003F}, {0x25, 0x0F, 0x0121}, {0x25, 0x10, 0x0020}, {0x25, 0x11, 0x8840},
	{0x28, 0x00, 0x0668}, {0x28, 0x02, 0xD020}, {0x28, 0x06, 0xC000}, {0x28, 0x0B, 0x1892},
	{0x28, 0x0F, 0xFFDF}, {0x28, 0x12, 0x01C4}, {0x28, 0x13, 0x027F}, {0x28, 0x14, 0x1311},
	{0x28, 0x16, 0x00C9}, {0x28, 0x17, 0xA100}, {0x28, 0x1A, 0x0001}, {0x28, 0x1C, 0x0400},
	{0x29, 0x01, 0x0300}, {0x29, 0x02, 0x1017}, {0x29, 0x03, 0xFFDF}, {0x29, 0x05, 0x7F7C},
	{0x29, 0x07, 0x8100}, {0x29, 0x08, 0x0001}, {0x29, 0x09, 0xFFD4}, {0x29, 0x0A, 0x7C2F},
	{0x29, 0x0E, 0x003F}, {0x29, 0x0F, 0x0121}, {0x29, 0x10, 0x0020}, {0x29, 0x11, 0x8840},
	{0x2B, 0x13, 0x0050}, {0x2B, 0x18, 0x8E88}, {0x2B, 0x19, 0x4902}, {0x2B, 0x1D, 0x2501},
	{0x2D, 0x13, 0x0050}, {0x2D, 0x18, 0x8E88}, {0x2D, 0x17, 0x4109}, {0x2D, 0x19, 0x4902},
	{0x2D, 0x1C, 0x1109}, {0x2D, 0x1D, 0x2641},
	{0x2F, 0x13, 0x0050}, {0x2F, 0x18, 0x8E88}, {0x2F, 0x19, 0x4902}, {0x2F, 0x1D, 0x66E1},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_10g_2500bx_odd[] = {
	{0x00, 0x0E, 0x3053}, {0x01, 0x14, 0x0100},
	{0x21, 0x03, 0x8206}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F}, {0x21, 0x0A, 0x0003},
	{0x21, 0x0B, 0x0005}, {0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009}, {0x21, 0x0E, 0x0000},
	{0x21, 0x0F, 0x0008},
	{0x24, 0x00, 0x0668}, {0x24, 0x02, 0xD020}, {0x24, 0x06, 0xC000}, {0x24, 0x0B, 0x1892},
	{0x24, 0x0F, 0xFFDF}, {0x24, 0x12, 0x03C4}, {0x24, 0x13, 0x027F}, {0x24, 0x14, 0x1311},
	{0x24, 0x16, 0x00C9}, {0x24, 0x17, 0xA100}, {0x24, 0x1A, 0x0001}, {0x24, 0x1C, 0x0400},
	{0x25, 0x00, 0x820F}, {0x25, 0x01, 0x0300}, {0x25, 0x02, 0x1017}, {0x25, 0x03, 0xFFDF},
	{0x25, 0x05, 0x7F7C}, {0x25, 0x07, 0x8100}, {0x25, 0x08, 0x0001}, {0x25, 0x09, 0xFFD4},
	{0x25, 0x0A, 0x7C2F}, {0x25, 0x0E, 0x003F}, {0x25, 0x0F, 0x0121}, {0x25, 0x10, 0x0020},
	{0x25, 0x11, 0x8840},
	{0x28, 0x00, 0x0668}, {0x28, 0x02, 0xD020}, {0x28, 0x06, 0xC000}, {0x28, 0x0B, 0x1892},
	{0x28, 0x0F, 0xFFDF}, {0x28, 0x12, 0x01C4}, {0x28, 0x13, 0x027F}, {0x28, 0x14, 0x1311},
	{0x28, 0x16, 0x00C9}, {0x28, 0x17, 0xA100}, {0x28, 0x1A, 0x0001}, {0x28, 0x1C, 0x0400},
	{0x29, 0x00, 0x820F}, {0x29, 0x01, 0x0300}, {0x29, 0x02, 0x1017}, {0x29, 0x03, 0xFFDF},
	{0x29, 0x05, 0x7F7C}, {0x29, 0x07, 0x8100}, {0x29, 0x08, 0x0001}, {0x29, 0x0A, 0x7C2F},
	{0x29, 0x0E, 0x003F}, {0x29, 0x0F, 0x0121}, {0x29, 0x10, 0x0020}, {0x29, 0x11, 0x8840},
	{0x2B, 0x13, 0x3D87}, {0x2B, 0x14, 0x3108},
	{0x2D, 0x13, 0x3C87}, {0x2D, 0x14, 0x1808},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_xsgmii_even[] =
{
	{0x00, 0x0E, 0x3053},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F},
	{0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009}, {0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008},
	{0x2E, 0x00, 0xA668}, {0x2E, 0x02, 0xD020}, {0x2E, 0x06, 0xC000}, {0x2E, 0x0B, 0x1892},
	{0x2E, 0x0F, 0xFFDF}, {0x2E, 0x11, 0x8280}, {0x2E, 0x12, 0x0484}, {0x2E, 0x13, 0x027F},
	{0x2E, 0x14, 0x1311}, {0x2E, 0x17, 0xA100}, {0x2E, 0x1A, 0x0001}, {0x2E, 0x1C, 0x0400},
	{0x2F, 0x01, 0x0300}, {0x2F, 0x02, 0x1017}, {0x2F, 0x03, 0xFFDF}, {0x2F, 0x05, 0x7F7C},
	{0x2F, 0x07, 0x8104}, {0x2F, 0x08, 0x0001}, {0x2F, 0x09, 0xFFD4}, {0x2F, 0x0A, 0x7C2F},
	{0x2F, 0x0E, 0x003F}, {0x2F, 0x0F, 0x0121}, {0x2F, 0x10, 0x0020}, {0x2F, 0x11, 0x8840},
	{0x2B, 0x13, 0x0050}, {0x2B, 0x18, 0x8E88}, {0x2B, 0x19, 0x4902}, {0x2B, 0x1D, 0x2501},
	{0x2D, 0x13, 0x0050}, {0x2D, 0x18, 0x8E88}, {0x2D, 0x19, 0x4902}, {0x2D, 0x1D, 0x2641},
	{0x2F, 0x13, 0x0050}, {0x2F, 0x18, 0x8E88}, {0x2F, 0x19, 0x4902}, {0x2F, 0x1D, 0x66E1},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_xsgmii_odd[] =
{
	{0x00, 0x0E, 0x3053},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F},
	{0x21, 0x0A, 0x0003}, {0x21, 0x0B, 0x0005}, {0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009},
	{0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008},
	{0x2E, 0x00, 0xA668}, {0x2E, 0x02, 0xD020}, {0x2E, 0x06, 0xC000}, {0x2E, 0x0B, 0x1892},
	{0x2E, 0x0F, 0xFFDF}, {0x2E, 0x11, 0x8280}, {0x2E, 0x12, 0x0484}, {0x2E, 0x13, 0x027F},
	{0x2E, 0x14, 0x1311}, {0x2E, 0x17, 0xA100}, {0x2E, 0x1A, 0x0001}, {0x2E, 0x1C, 0x0400},
	{0x2F, 0x00, 0x820F}, {0x2F, 0x01, 0x0300}, {0x2F, 0x02, 0x1017}, {0x2F, 0x03, 0xFFDF},
	{0x2F, 0x05, 0x7F7C}, {0x2F, 0x07, 0x8104}, {0x2F, 0x08, 0x0001}, {0x2F, 0x09, 0xFFD4},
	{0x2F, 0x0A, 0x7C2F}, {0x2F, 0x0E, 0x003F}, {0x2F, 0x0F, 0x0121}, {0x2F, 0x10, 0x0020},
	{0x2F, 0x11, 0x8840},
	{0x2B, 0x13, 0x3D87}, {0x2B, 0x14, 0x3108},
	{0x2D, 0x13, 0x3C87}, {0x2D, 0x14, 0x1808},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_usxgmii_sx_even[] =
{
	{0x06, 0x00, 0x0000}, {0x06, 0x0D, 0x0F00}, {0x06, 0x0E, 0x055A}, {0x07, 0x10, 0x6003},
	{0x06, 0x1D, 0x0600}, {0x06, 0x13, 0x68C1}, {0x06, 0x14, 0xF021}, {0x07, 0x06, 0x1401},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F},
	{0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009}, {0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008},
	{0x2E, 0x00, 0xA668}, {0x2E, 0x01, 0x2088}, {0x2E, 0x02, 0xD020}, {0x2E, 0x06, 0xC000},
	{0x2E, 0x0B, 0x1892}, {0x2E, 0x0F, 0xFFDF}, {0x2E, 0x11, 0x8280}, {0x2E, 0x12, 0x0484},
	{0x2E, 0x13, 0x027F}, {0x2E, 0x14, 0x1311}, {0x2E, 0x17, 0xA100}, {0x2E, 0x1A, 0x0001},
	{0x2E, 0x1C, 0x0400}, {0x2F, 0x01, 0x0300}, {0x2F, 0x02, 0x1017}, {0x2F, 0x03, 0xFFDF},
	{0x2F, 0x05, 0x7F7C}, {0x2F, 0x07, 0x8104}, {0x2F, 0x08, 0x0001}, {0x2F, 0x09, 0xFFD4},
	{0x2F, 0x0A, 0x7C2F}, {0x2F, 0x0E, 0x003F}, {0x2F, 0x0F, 0x0121}, {0x2F, 0x10, 0x0020},
	{0x2F, 0x11, 0x8840},
	{0x2B, 0x13, 0x0050}, {0x2B, 0x18, 0x8E88}, {0x2B, 0x19, 0x4902}, {0x2B, 0x1D, 0x2501},
	{0x2D, 0x13, 0x0050}, {0x2D, 0x18, 0x8E88}, {0x2D, 0x19, 0x4902}, {0x2D, 0x1D, 0x2641},
	{0x2F, 0x13, 0x0050}, {0x2F, 0x18, 0x8E88}, {0x2F, 0x19, 0x4902}, {0x2F, 0x1D, 0x66E1},
	/*enable eee*/
	{0x06, 0x03, 0xc45c},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_usxgmii_sx_odd[] =
{
	{0x06, 0x00, 0x0000}, {0x06, 0x0D, 0x0F00}, {0x06, 0x0E, 0x055A}, {0x07, 0x10, 0x6003},
	{0x06, 0x1D, 0x0600}, {0x06, 0x13, 0x68C1}, {0x06, 0x14, 0xF021}, {0x07, 0x06, 0x1401},
	{0x21, 0x03, 0x8206}, {0x21, 0x05, 0x40B0}, {0x21, 0x06, 0x0010}, {0x21, 0x07, 0xF09F},
	{0x21, 0x0A, 0x0003}, {0x21, 0x0B, 0x0005}, {0x21, 0x0C, 0x0007}, {0x21, 0x0D, 0x6009},
	{0x21, 0x0E, 0x0000}, {0x21, 0x0F, 0x0008},
	{0x2E, 0x00, 0xA668}, {0x2E, 0x02, 0xD020}, {0x2E, 0x06, 0xC000}, {0x2E, 0x0B, 0x1892},
	{0x2E, 0x0F, 0xFFDF}, {0x2E, 0x11, 0x8280}, {0x2E, 0x12, 0x0484}, {0x2E, 0x13, 0x027F},
	{0x2E, 0x14, 0x1311}, {0x2E, 0x17, 0xA100}, {0x2E, 0x1A, 0x0001}, {0x2E, 0x1C, 0x0400},
	{0x2F, 0x00, 0x820F}, {0x2F, 0x01, 0x0300}, {0x2F, 0x02, 0x1017}, {0x2F, 0x03, 0xFFDF},
	{0x2F, 0x05, 0x7F7C}, {0x2F, 0x07, 0x8104}, {0x2F, 0x08, 0x0001}, {0x2F, 0x09, 0xFFD4},
	{0x2F, 0x0A, 0x7C2F}, {0x2F, 0x0E, 0x003F}, {0x2F, 0x0F, 0x0121}, {0x2F, 0x10, 0x0020},
	{0x2F, 0x11, 0x8840},
	{0x2B, 0x13, 0x3D87},{0x2B, 0x14, 0x3108},
	{0x2D, 0x13, 0x3C87},{0x2D, 0x14, 0x1808},
	/*enable eee*/
	{0x06, 0x03, 0xc45c},
};

static const struct rtpcs_sds_config rtpcs_930x_sds_cfg_5g_qsgmii[] =
{
	{0x00, 0x0E, 0x3053},
	{0x21, 0x00, 0x3C91},{0x21, 0x02, 0xB602},{0x21, 0x07, 0xFA66},{0x21, 0x0A, 0xDF40},
	{0x2A, 0x02, 0x35A1},{0x2A, 0x03, 0x6960},
};

static void rtpcs_930x_sds_usxgmii_config(struct rtpcs_serdes *sds, int nway_en,
					  u32 opcode, u32 am_period,
					  u32 all_am_markers, u32 an_table,
					  u32 sync_bit)
{
	rtpcs_sds_write_bits(sds, 0x7, 0x11, 0, 0, nway_en);
	rtpcs_sds_write_bits(sds, 0x7, 0x11, 1, 1, nway_en);
	rtpcs_sds_write_bits(sds, 0x7, 0x11, 2, 2, nway_en);
	rtpcs_sds_write_bits(sds, 0x7, 0x11, 3, 3, nway_en);
	rtpcs_sds_write_bits(sds, 0x6, 0x12, 15, 0, am_period);
	rtpcs_sds_write_bits(sds, 0x6, 0x13, 7,  0, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x13, 15, 8, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x14, 7,  0, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x14, 15, 8, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x15, 7,  0, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x15, 15, 8, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x16, 7,  0, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x16, 15, 8, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x17, 7,  0, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x17, 15, 8, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x18, 7,  0, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x6, 0x18, 15, 8, all_am_markers);
	rtpcs_sds_write_bits(sds, 0x7, 0x10, 7, 0, opcode);
	rtpcs_sds_write_bits(sds, 0x6, 0xe, 10, 10, an_table);
	rtpcs_sds_write_bits(sds, 0x6, 0x1d, 11, 10, sync_bit);
}

static void rtpcs_930x_sds_patch(struct rtpcs_serdes *sds,
				 enum rtpcs_sds_mode hw_mode)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	const struct rtpcs_sds_config *config;
	bool is_even_sds;
	size_t count;

	is_even_sds = (sds == even_sds);

	switch (hw_mode) {
	case RTPCS_SDS_MODE_1000BASEX:
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_10GBASER:
		if (is_even_sds) {
			config = rtpcs_930x_sds_cfg_10gr_even;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_10gr_even);
		} else {
			config = rtpcs_930x_sds_cfg_10gr_odd;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_10gr_odd);
		}
		break;

	case RTPCS_SDS_MODE_2500BASEX:
		if (is_even_sds) {
			config = rtpcs_930x_sds_cfg_10g_2500bx_even;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_10g_2500bx_even);
		} else {
			config = rtpcs_930x_sds_cfg_10g_2500bx_odd;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_10g_2500bx_odd);
		}
		break;

	case RTPCS_SDS_MODE_QSGMII:
		/* only QSGMII on 5G SerDes (0 + 1) for now */
		config = rtpcs_930x_sds_cfg_5g_qsgmii;
		count = ARRAY_SIZE(rtpcs_930x_sds_cfg_5g_qsgmii);
		break;

	case RTPCS_SDS_MODE_XSGMII:
		if (is_even_sds) {
			config = rtpcs_930x_sds_cfg_xsgmii_even;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_xsgmii_even);
		} else {
			config = rtpcs_930x_sds_cfg_xsgmii_odd;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_xsgmii_odd);
		}

		/* this needs XSG write */
		for (size_t i = 0; i < count; i++)
			rtpcs_sds_xsg_write(sds, config[i].page, config[i].reg, config[i].data);

		return;

	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
		if (is_even_sds) {
			config = rtpcs_930x_sds_cfg_usxgmii_sx_even;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_usxgmii_sx_even);
		} else {
			config = rtpcs_930x_sds_cfg_usxgmii_sx_odd;
			count = ARRAY_SIZE(rtpcs_930x_sds_cfg_usxgmii_sx_odd);
		}
		break;

	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	default:
		return;
	}

	for (size_t i = 0; i < count; ++i)
		rtpcs_sds_write(sds, config[i].page, config[i].reg, config[i].data);

	if (hw_mode == RTPCS_SDS_MODE_USXGMII_10GQXGMII) {
		/* Default configuration */
		rtpcs_930x_sds_usxgmii_config(sds, 1, 0xaa, 0x5078, 0, 1, 0x1);
	}
}

__always_unused
static int rtpcs_930x_sds_cmu_band_get(struct rtpcs_serdes *sds)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	struct rtpcs_serdes *odd_sds = rtpcs_sds_get_odd(sds);
	u32 page;
	u32 en;
	u32 cmu_band;

/*	page = rtl9300_sds_cmu_page_get(sds); */
	page = 0x25; /* 10GR and 1000BX */

	rtpcs_sds_write_bits(even_sds, page, 0x1c, 15, 15, 1);
	rtpcs_sds_write_bits(odd_sds, page, 0x1c, 15, 15, 1);

	en = rtpcs_sds_read_bits(even_sds, page, 27, 1, 1);
	if (!en) { /* Auto mode */
		rtpcs_sds_write(even_sds, 0x1f, 0x02, 31);

		cmu_band = rtpcs_sds_read_bits(even_sds, 0x1f, 0x15, 5, 1);
	} else {
		cmu_band = rtpcs_sds_read_bits(even_sds, page, 30, 4, 0);
	}

	return cmu_band;
}

static int rtpcs_930x_setup_serdes(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	int calib_tries = 0, ret;

	/* Turn Off Serdes */
	ret = rtpcs_930x_sds_set_mode(sds, RTPCS_SDS_MODE_OFF);
	if (ret < 0)
		return ret;

	/* Apply serdes patches */
	rtpcs_930x_sds_patch(sds, hw_mode);

	/* Maybe use dal_longan_sds_init */

	/* dal_longan_construct_serdesConfig_init */ /* Serdes Construct */
	rtpcs_930x_phy_enable_10g_1g(sds);

	/* ----> dal_longan_sds_mode_set */
	pr_info("%s: Configuring RTL9300 SERDES %d\n", __func__, sds->id);

	/* Set SDS polarity */
	rtpcs_930x_sds_set_polarity(sds, sds->tx_pol_inv, sds->rx_pol_inv);

	/* Enable SDS in desired mode */
	ret = rtpcs_930x_sds_set_mode(sds, hw_mode);
	if (ret < 0)
		return ret;

	sds->hw_mode = hw_mode;

	/* Enable Fiber RX */
	rtpcs_sds_write_bits(sds, 0x20, 2, 12, 12, 0);

	if (hw_mode == RTPCS_SDS_MODE_QSGMII)
		goto skip_cali;

	/* Calibrate SerDes receiver in loopback mode */
	rtpcs_930x_sds_10g_idle(sds);
	do {
		rtpcs_930x_sds_do_rx_calibration(sds, hw_mode);
		calib_tries++;
		mdelay(50);
	} while (rtpcs_930x_sds_check_calibration(sds, hw_mode) && calib_tries < 3);
	if (calib_tries >= 3)
		pr_warn("%s: SerDes RX calibration failed\n", __func__);

skip_cali:
	/* Leave loopback mode */
	rtpcs_930x_sds_tx_config(sds, hw_mode);

	return 0;
}

/* RTL931X */

/*
 * The SerDes MDIO driver maps page regions to different background SerDes.
 * 0x00 - 0x3f	analog SDS
 * 0x40 - 0x7f	digital SDS 1
 * 0x80 - 0xbf	digital SDS 2
 *
 * An XSG write operates on digital SDS 1 and digital SDS 2. Map that to the
 * page ranges accordingly.
 */
static int rtpcs_931x_sds_op_xsg_write(struct rtpcs_serdes *sds, int page, int regnum,
                                       int bithigh, int bitlow, u16 value)
{
        int ret;

        ret = __rtpcs_sds_write_raw(sds->ctrl, sds->id, page + 0x40, regnum, bithigh, bitlow,
				    value);
        if (ret)
                return ret;

        return __rtpcs_sds_write_raw(sds->ctrl, sds->id, page + 0x80, regnum, bithigh, bitlow,
				     value);
}

__maybe_unused
static int rtpcs_931x_sds_fiber_get_symerr(struct rtpcs_serdes *sds,
					   enum rtpcs_sds_mode hw_mode)
{
	int symerr, val, val2;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_10GBASER:
		symerr = rtpcs_sds_read_bits(sds, 0x5, 0x1, 7, 0);
		break;
	case RTPCS_SDS_MODE_1000BASEX:
		rtpcs_sds_write_bits(sds, 0x41, 0x18, 2, 0, 0x0);

		val = rtpcs_sds_read_bits(sds, 0x41, 0x3, 15, 8);
		val2 = rtpcs_sds_read_bits(sds, 0x41, 0x2, 15, 0);
		symerr = (val << 16) | val2;
		break;
	default:
		symerr = -EINVAL;
	}

	return symerr;
}

static void rtpcs_931x_sds_clear_symerr(struct rtpcs_serdes *sds,
					enum rtpcs_sds_mode hw_mode)
{
	switch (hw_mode) {
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_HISGMII:
	case RTPCS_SDS_MODE_XSGMII:
		for (int i = 0; i < 4; ++i) {
			rtpcs_sds_write_bits(sds, 0x41, 0x18, 2, 0, i);
			rtpcs_sds_write_bits(sds, 0x41, 0x3, 15, 8, 0x0);
			rtpcs_sds_write_bits(sds, 0x41, 0x2, 15, 0, 0x0);
		}

		for (int i = 0; i < 4; ++i) {
			rtpcs_sds_write_bits(sds, 0x81, 0x18, 2, 0, i);
			rtpcs_sds_write_bits(sds, 0x81, 0x3, 15, 8, 0x0);
			rtpcs_sds_write_bits(sds, 0x81, 0x2, 15, 0, 0x0);
		}

		rtpcs_sds_write_bits(sds, 0x41, 0x0, 15, 0, 0x0);
		rtpcs_sds_write_bits(sds, 0x41, 0x1, 15, 8, 0x0);
		rtpcs_sds_write_bits(sds, 0x81, 0x0, 15, 0, 0x0);
		rtpcs_sds_write_bits(sds, 0x81, 0x1, 15, 8, 0x0);
		break;
	case RTPCS_SDS_MODE_1000BASEX:
		rtpcs_sds_write_bits(sds, 0x41, 0x18, 2, 0, 0x0);
		rtpcs_sds_write_bits(sds, 0x41, 0x3, 15, 8, 0x0);
		rtpcs_sds_write_bits(sds, 0x41, 0x2, 15, 0, 0x0);
		break;
	case RTPCS_SDS_MODE_10GBASER:
		/* to be verified: clear on read? */
		rtpcs_sds_read_bits(sds, 0x5, 0x1, 7, 0);
		break;
	case RTPCS_SDS_MODE_OFF:
	default:
		break;
	}
}

/**
 * rtpcs_931x_sds_reset_leq_dfe() - Reset LEQ + DFE to a baseline.
 *
 * @sds: Reference to SerDes instance
 *
 * Reset both LEQ and DFE in the RX path to baseline configuration. I.e.
 * sets LEQ and DFE to manual mode and sets certain values (mostly 0) for
 * LEQ and DFE coefficients/parameters.
 *
 * LEQ and DFE can run in two modes:
 * * manual: specific values are set and used
 * * auto: both adapt their parameters automatically
 *
 */
static int rtpcs_931x_sds_reset_leq_dfe(struct rtpcs_serdes *sds)
{
	rtpcs_sds_write_bits(sds, 0x2e, 0xd, 6, 0, 0x0);	/* [6:2] LEQ gain */
	rtpcs_sds_write_bits(sds, 0x2e, 0xd, 7, 7, 0x1);	/* LEQ manual 1=true,0=false */

	rtpcs_sds_write_bits(sds, 0x2e, 0x1c, 5, 0, 0x1e);	/* TAP0 */
	rtpcs_sds_write_bits(sds, 0x2e, 0x1d, 11, 0, 0x0);	/* TAP1 [11:6] ODD | [5:0] EVEN */
	rtpcs_sds_write_bits(sds, 0x2e, 0x1f, 11, 0, 0x0);	/* TAP2 [11:6] ODD | [5:0] EVEN */
	rtpcs_sds_write_bits(sds, 0x2f, 0x0, 11, 0, 0x0);	/* TAP3 [11:6] ODD | [5:0] EVEN */
	rtpcs_sds_write_bits(sds, 0x2f, 0x1, 11, 0, 0x0);	/* TAP4 [11:6] ODD | [5:0] EVEN */

	rtpcs_sds_write_bits(sds, 0x2e, 0xf, 12, 6, 0x7f);	/* set manual mode */
	rtpcs_sds_write(sds, 0x2f, 0x12, 0xaaa);		/* [11:8] VTHN | [7:4] VTHP */

	return 0;
}

static int rtpcs_931x_sds_power(struct rtpcs_serdes *sds, bool power_on)
{
	u32 en_val = power_on ? 0 : BIT(sds->id);

	return regmap_write_bits(sds->ctrl->map,
				 RTL931X_PS_SERDES_OFF_MODE_CTRL_ADDR,
				 BIT(sds->id), en_val);
}

/*
 * rtpcs_931x_sds_set_mac_mode
 *
 * Set the SerDes mode in the MAC's registers.
 */
static int rtpcs_931x_sds_set_mac_mode(struct rtpcs_serdes *sds,
				       enum rtpcs_sds_mode hw_mode)
{
	u32 mode_val;
	int shift = ((sds->id & 0x3) << 3);

	switch (hw_mode) {
	case RTPCS_SDS_MODE_OFF:
		mode_val = 0x1f;
		break;
	case RTPCS_SDS_MODE_QSGMII:
		mode_val = 0x6;
		break;
	case RTPCS_SDS_MODE_HISGMII:
		mode_val = 0x12;
		break;
	case RTPCS_SDS_MODE_XSGMII:
		mode_val = 0x10;
		break;
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_2_5GSXGMII:
		mode_val = 0xd;
		break;
	case RTPCS_SDS_MODE_SGMII:
		mode_val = 0x2;
		break;
	default:
		return -EINVAL;
	}

	mode_val |= BIT(7); /* force mode bit */
	return regmap_write_bits(sds->ctrl->map,
				 RTL931X_SERDES_MODE_CTRL + 4 * (sds->id >> 2),
				 0xff << shift, mode_val << shift);
}

/*
 * rtpcs_931x_sds_set_ip_mode
 *
 * Set the SerDes mode in the SerDes IP block's registers.
 */
static int rtpcs_931x_sds_set_ip_mode(struct rtpcs_serdes *sds,
				      enum rtpcs_sds_mode hw_mode)
{
	u32 mode_val;

	/* clear symbol error count before changing mode */
	rtpcs_931x_sds_clear_symerr(sds, hw_mode);
	rtpcs_931x_sds_set_mac_mode(sds, RTPCS_SDS_MODE_OFF);

	switch (hw_mode) {
	case RTPCS_SDS_MODE_OFF:
		mode_val = 0x3f;
		break;

	case RTPCS_SDS_MODE_SGMII:
		mode_val = 0x5;
		break;

	case RTPCS_SDS_MODE_1000BASEX:
		/* serdes mode FIBER1G */
		mode_val = 0x9;
		break;

	case RTPCS_SDS_MODE_2500BASEX:
		/* available SDK code doesn't have this value. based on brute-forcing
		 * the SerDes mode register field until the link is working
		 */
		mode_val = 0x2d;
		break;

	case RTPCS_SDS_MODE_10GBASER:
		mode_val = 0x35;
		break;
/*      case MII_10GR1000BX_AUTO:
                mode_val = 0x39;
                break; */

	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_2_5GSXGMII:
		mode_val = 0x1b;
		break;
	case RTPCS_SDS_MODE_HISGMII:
		mode_val = 0x25;
		break;
	default:
		return -ENOTSUPP;
	}

	pr_info("%s writing analog SerDes Mode value %02x\n", __func__, mode_val);
	return rtpcs_sds_write_bits(sds, 0x1f, 0x9, 11, 6, mode_val);
}

static int rtpcs_931x_sds_set_mode(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	if (hw_mode == RTPCS_SDS_MODE_XSGMII)
		return rtpcs_931x_sds_set_mac_mode(sds, hw_mode);
	else
		return rtpcs_931x_sds_set_ip_mode(sds, hw_mode);
}

static void rtpcs_931x_sds_reset(struct rtpcs_serdes *sds)
{
	struct rtpcs_ctrl *ctrl = sds->ctrl;
	u32 sds_id = sds->id;
	u32 v, o_mode;
	int shift = ((sds_id & 0x3) << 3);

	/* TODO: We need to lock this! */

	rtpcs_931x_sds_power(sds, false);

	regmap_read(ctrl->map, RTL931X_SERDES_MODE_CTRL + 4 * (sds_id >> 2), &o_mode);
	v = BIT(7) | 0x1F;
	regmap_write_bits(ctrl->map, RTL931X_SERDES_MODE_CTRL + 4 * (sds_id >> 2),
			  0xff << shift, v << shift);
	regmap_write(ctrl->map, RTL931X_SERDES_MODE_CTRL + 4 * (sds_id >> 2), o_mode);

	rtpcs_931x_sds_power(sds, true);
}

static void rtpcs_931x_sds_rx_reset(struct rtpcs_serdes *sds)
{
	if (sds->id < 2)
		return;

	rtpcs_sds_write(sds, 0x2e, 0x12, 0x2740);
	rtpcs_sds_write(sds, 0x2f, 0x0, 0x0);		/* [11:6] DFE_TAP3_ODD | [5:0] DFE_TAP3_EVEN */
	rtpcs_sds_write(sds, 0x2f, 0x2, 0x2010);
	rtpcs_sds_write(sds, 0x20, 0x0, 0xc10);

	rtpcs_sds_write(sds, 0x2e, 0x12, 0x27c0);
	rtpcs_sds_write(sds, 0x2f, 0x0, 0xc000);	/* [11:6] DFE_TAP3_ODD | [5:0] DFE_TAP3_EVEN */
	rtpcs_sds_write(sds, 0x2f, 0x2, 0x6010);
	rtpcs_sds_write(sds, 0x20, 0x0, 0xc30);

	mdelay(50);
}

static int rtpcs_931x_sds_cmu_page_get(enum rtpcs_sds_mode hw_mode)
{
	switch (hw_mode) {
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_1000BASEX:
		return 0x24;			/* ANA_1G */
	case RTPCS_SDS_MODE_2500BASEX:
		return 0x28;			/* ANA_3G */
	case RTPCS_SDS_MODE_QSGMII:
		return 0x2a;			/* ANA_5G/6G */
	//	return 0x34;
	case RTPCS_SDS_MODE_XSGMII:
	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_2_5GSXGMII:
	case RTPCS_SDS_MODE_10GBASER:
		return 0x2e;			/* ANA_10G */
	default:
		return -ENOTSUPP;
	}
}

static int rtpcs_931x_sds_get_pll_select(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type *pll)
{
	int cmu_page, pll_sel;

	cmu_page = rtpcs_931x_sds_cmu_page_get(sds->hw_mode);
	if (cmu_page < 0)
		return cmu_page;

	pll_sel = rtpcs_sds_read_bits(sds, cmu_page, 0x7, 15, 15);
	if (pll_sel < 0)
		return pll_sel;

	*pll = (enum rtpcs_sds_pll_type)pll_sel;
	return 0;
}

static int rtpcs_931x_sds_set_pll_select(struct rtpcs_serdes *sds, enum rtpcs_sds_mode hw_mode,
					 enum rtpcs_sds_pll_type pll)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int cmu_page, ret, val;
	int frc_lc_mode_bit;

	cmu_page = rtpcs_931x_sds_cmu_page_get(hw_mode);
	if (cmu_page < 0)
		return cmu_page;

	/*
	 * bits [5:4] (even) / [7:6] (odd) are used by RTL930x as selector. The selector
	 * for RTL931x SerDes is in the CMU page of each SerDes, depending on the hardware
	 * mode.
	 *
	 * Here, the SDK calls them 'frc_lc_mode' and 'frc_lc_mode_val'. However, they don't
	 * seem to have any effect and thus their purpose is unknown. So just set them as
	 * the SDK does.
	 */
	val = (pll == RTPCS_SDS_PLL_TYPE_LC) ? 0x3 : 0x1;
	frc_lc_mode_bit = (sds == even_sds) ? 4 : 6;
	ret = rtpcs_sds_write_bits(even_sds, 0x20, 0x12, frc_lc_mode_bit + 1,
				   frc_lc_mode_bit, val);
	if (ret < 0)
		return ret;

	return rtpcs_sds_write_bits(sds, cmu_page, 0x7, 15, 15, pll);
}

static int rtpcs_931x_sds_reconfigure_to_pll(struct rtpcs_serdes *sds, enum rtpcs_sds_pll_type pll)
{
	enum rtpcs_sds_pll_type tmp_pll;
	enum rtpcs_sds_pll_speed speed;
	enum rtpcs_sds_mode tmp_mode;
	int ret;

	/* assume we always reconfigure to the other PLL */
	tmp_pll = (pll == RTPCS_SDS_PLL_TYPE_LC) ? RTPCS_SDS_PLL_TYPE_RING : RTPCS_SDS_PLL_TYPE_LC;

	ret = rtpcs_93xx_sds_get_pll_config(sds, tmp_pll, &speed);
	if (ret < 0)
		return ret;

	tmp_mode = sds->hw_mode;

	/* turn off SerDes for reconfiguration */
	ret = rtpcs_931x_sds_power(sds, false);
	if (ret < 0)
		return ret;

	ret = rtpcs_931x_sds_set_mode(sds, RTPCS_SDS_MODE_OFF);
	if (ret < 0)
		return ret;

	/* reconfigure to other PLL */
	ret = rtpcs_93xx_sds_set_pll_config(sds, pll, speed);
	if (ret < 0)
		return ret;

	ret = rtpcs_931x_sds_set_pll_select(sds, sds->hw_mode, pll);
	if (ret < 0)
		return ret;

	/* turn on SerDes again */
	ret = rtpcs_931x_sds_set_mode(sds, tmp_mode);
	if (ret < 0)
		return ret;

	return rtpcs_931x_sds_power(sds, true);
}

static int rtpcs_931x_sds_cmu_band_set(struct rtpcs_serdes *sds,
				       bool enable, u32 band,
				       enum rtpcs_sds_mode hw_mode)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int page = rtpcs_931x_sds_cmu_page_get(hw_mode);
	int en_val;

	if (page < 0)
		return -EINVAL;

	page += 1;
	en_val = enable ? 0 : 1;

	rtpcs_sds_write_bits(even_sds, page, 0x7, 13, 13, en_val);
	rtpcs_sds_write_bits(even_sds, page, 0x7, 11, 11, en_val);
	rtpcs_sds_write_bits(even_sds, page, 0x7, 4, 0, band);

	rtpcs_931x_sds_reset(even_sds);

	return 0;
}

__maybe_unused
static int rtpcs_931x_sds_cmu_band_get(struct rtpcs_serdes *sds,
				       enum rtpcs_sds_mode hw_mode)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	int page = rtpcs_931x_sds_cmu_page_get(hw_mode);

	if (page < 0)
		return -EINVAL;

	page += 1;
	rtpcs_sds_write(even_sds, 0x1f, 0x02, 73);
	rtpcs_sds_write_bits(even_sds, page, 0x5, 15, 15, 0x1);

	return rtpcs_sds_read_bits(even_sds, 0x1f, 0x15, 8, 3);
}

__always_unused
static int rtpcs_931x_sds_link_sts_get(struct rtpcs_serdes *sds)
{
	u32 sts, sts1, latch_sts, latch_sts1;

	switch (sds->hw_mode) {
	case RTPCS_SDS_MODE_XSGMII:
		sts = rtpcs_sds_read_bits(sds, 0x41, 29, 8, 0);
		sts1 = rtpcs_sds_read_bits(sds, 0x81, 29, 8, 0);
		latch_sts = rtpcs_sds_read_bits(sds, 0x41, 30, 8, 0);
		latch_sts1 = rtpcs_sds_read_bits(sds, 0x81, 30, 8, 0);
		break;

	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_HISGMII:
	case RTPCS_SDS_MODE_2500BASEX:
		sts = rtpcs_sds_read_bits(sds, 0x41, 29, 8, 0);
		latch_sts = rtpcs_sds_read_bits(sds, 0x41, 30, 8, 0);
		break;

	default:
		sts = rtpcs_sds_read_bits(sds, 0x5, 0, 12, 12);
		latch_sts = rtpcs_sds_read_bits(sds, 0x4, 1, 2, 2);
		latch_sts1 = rtpcs_sds_read_bits(sds, 0x42, 1, 2, 2);
		sts1 = rtpcs_sds_read_bits(sds, 0x42, 1, 2, 2);
	}

	pr_info("%s: serdes %d sts %d, sts1 %d, latch_sts %d, latch_sts1 %d\n", __func__,
		sds->id, sts, sts1, latch_sts, latch_sts1);

	return sts1;
}

static int rtpcs_931x_sds_set_polarity(struct rtpcs_serdes *sds,
				       bool tx_inv, bool rx_inv)
{
	u8 rx_val = rx_inv ? 1 : 0;
	u8 tx_val = tx_inv ? 1 : 0;
	u32 val;
	int ret;

	/* 10gr_*_inv */
	val = (tx_val << 1) | rx_val;
	ret = rtpcs_sds_write_bits(sds, 0x6, 0x2, 14, 13, val);
	if (ret)
		return ret;

	/* xsg_*_inv */
	val = (rx_val << 1) | tx_val;
	ret = rtpcs_sds_write_bits(sds, 0x40, 0x0, 9, 8, val);
	if (ret)
		return ret;

	return rtpcs_sds_write_bits(sds, 0x80, 0x0, 9, 8, val);
}

static int rtpcs_931x_sds_set_port_media(struct rtpcs_serdes *sds,
					 enum rtpcs_port_media port_media)
{
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	bool is_dac, is_10g;

	/*
	 * SDK identifies this as some kind of gating. It's enabled
	 * here and later deactivated for non-10G.
	 * (from DMS1250 SDK)
	 */
	rtpcs_sds_write_bits(sds, 0x5f, 0x1, 0, 0, 0x1);

	/* media none behavior */
	rtpcs_sds_write(sds, 0x2e, 0x12, 0x2740);
	rtpcs_sds_write(sds, 0x2f, 0x0, 0x0);		/* [11:6] DFE_TAP3_ODD | [5:0] DFE_TAP3_EVEN */
	rtpcs_sds_write(sds, 0x2f, 0x2, 0x2010);
	rtpcs_sds_write(sds, 0x20, 0x0, 0xcd1);		/* from 930x: [7:6] POWER_DOWN OF ?? */
	rtpcs_sds_write_bits(sds, 0x2e, 0xf, 5, 0, 0x4);

	rtpcs_sds_write_bits(sds, 0x2a, 0x12, 7, 6, 0x1);
	/* TODO: can we drop this in favor of turning off SerDes ealier? */
	rtpcs_931x_sds_set_mode(sds, RTPCS_SDS_MODE_OFF);

	if (port_media == RTPCS_PORT_MEDIA_NONE)
		return 0;

	rtpcs_sds_write(sds, 0x21, 0x19, 0xf0f0); /* from XS1930-10 SDK */
	rtpcs_sds_write(even_sds, 0x2e, 0x8, 0x0294);	/* [10:7] impedance */

	/* from _phy_rtl9310_sds_init, DMS1250 SDK */
	rtpcs_sds_write_bits(sds, 0x2e, 0xe, 13, 11, 0x0);
	rtpcs_931x_sds_rx_reset(sds);
	rtpcs_931x_sds_reset_leq_dfe(sds);

	is_dac = (port_media == RTPCS_PORT_MEDIA_DAC_50CM ||
		  port_media == RTPCS_PORT_MEDIA_DAC_100CM ||
		  port_media == RTPCS_PORT_MEDIA_DAC_300CM ||
		  port_media == RTPCS_PORT_MEDIA_DAC_500CM);
	is_10g = is_dac || port_media == RTPCS_PORT_MEDIA_FIBER_10G;

	if (port_media != RTPCS_PORT_MEDIA_FIBER_100M) {
		rtpcs_sds_write_bits(sds, 0x20, 0x0, 11, 10, 0x0);
		rtpcs_sds_write_bits(sds, 0x2a, 0x7, 15, 15, is_dac ? 0x1 : 0x0);
		rtpcs_sds_write_bits(sds, 0x20, 0x0, 11, 10, 0x3);
	}

	switch (port_media) {
	case RTPCS_PORT_MEDIA_DAC_50CM:
	case RTPCS_PORT_MEDIA_DAC_100CM:
		rtpcs_sds_write_bits(sds, 0x2e, 0x1, 15, 0, 0x1340);
		rtpcs_sds_write(sds, 0x21, 0x19, 0xf0a5); /* from XS1930-10 SDK */
		rtpcs_sds_write(even_sds, 0x2e, 0x8, 0x02a0); /* [10:7] impedance */
		break;

	case RTPCS_PORT_MEDIA_DAC_300CM:
	case RTPCS_PORT_MEDIA_DAC_500CM:
		rtpcs_sds_write_bits(sds, 0x2e, 0x1, 15, 0, 0x5200);
		rtpcs_sds_write(sds, 0x21, 0x19, 0xf0a5); /* from XS1930-10 SDK */
		rtpcs_sds_write(even_sds, 0x2e, 0x8, 0x02a0); /* [10:7] impedance */
		break;

	case RTPCS_PORT_MEDIA_FIBER_10G:
		/*
		 * TODO: this would need to be saved during early init, before
		 * actually changing any SerDes settings. Then restored here.
		 * see phy_rtl9310_init in SDK
		 */
		// rtpcs_sds_write(sds, 0x2e, 0x1, phy_rtl9310_10g_tx[unit][sds]);
		rtpcs_sds_write_bits(sds, 0x2e, 0xf, 5, 0, 0x2); /* from DMS1250 SDK */
		break;
	default:
		break;
	}

	/* CFG_LINKDW_SEL? (same semantics as 930x) */
	rtpcs_sds_write_bits(sds, 0x6, 0xd, 6, 6, is_dac ? 0x0 : 0x1);

	if (is_10g) {
		rtpcs_sds_write(sds, 0x2e, 0x12, 0x27c0);
		rtpcs_sds_write(sds, 0x2f, 0x0, 0xc000);	/* [11:6] DFE_TAP3_ODD | [5:0] DFE_TAP3_EVEN */
		rtpcs_sds_write(sds, 0x2f, 0x2, 0x6010);
	}

	/* FIXME: is this redundant with the writes below? */
	rtpcs_sds_write(sds, 0x20, 0x0, 0xc30);			/* from 930x: [7:6] POWER_DOWN OF ?? */
	rtpcs_sds_write_bits(sds, 0x20, 0x0, 9, 0, 0x30);
	rtpcs_sds_write_bits(sds, 0x2a, 0x12, 7, 6, 0x3);

	rtpcs_sds_write_bits(sds, 0x20, 0x0, 11, 10, 0x1);
	rtpcs_sds_write_bits(sds, 0x20, 0x0, 11, 10, 0x3);

	/* clear pending SerDes RX idle interrupt flag */
	regmap_write_bits(sds->ctrl->map, RTPCS_931X_ISR_SERDES_RXIDLE,
			  BIT(sds->id - 2), BIT(sds->id - 2));

	/* Gating as mentioned above, deactivated here for non-10G */
	if (!is_10g)
		rtpcs_sds_write_bits(sds, 0x5f, 0x1, 0, 0, 0x0);

	return 0;
}

static int rtpcs_931x_sds_config_fiber_1g(struct rtpcs_serdes *sds)
{
	rtpcs_sds_write_bits(sds, 0x43, 0x12, 15, 14, 0x0);

	rtpcs_sds_write_bits(sds, 0x42, 0x0, 6, 6, 0x1);
	rtpcs_sds_write_bits(sds, 0x42, 0x0, 13, 13, 0x0);

	return 0;
}

static int rtpcs_931x_sds_config_hw_mode(struct rtpcs_serdes *sds,
					 enum rtpcs_sds_mode hw_mode)
{
	switch (hw_mode) {
	case RTPCS_SDS_MODE_OFF:
		break;

	case RTPCS_SDS_MODE_1000BASEX:
		rtpcs_931x_sds_config_fiber_1g(sds);
		break;

	case RTPCS_SDS_MODE_2500BASEX:
		rtpcs_sds_write_bits(sds, 0x41, 0x14, 8, 8, 1);
		break;

	case RTPCS_SDS_MODE_10GBASER: /* 10GR1000BX_AUTO */
		/* configure 10GR fiber mode=1 */
		rtpcs_sds_write_bits(sds, 0x1f, 0xb, 1, 1, 1);

		rtpcs_931x_sds_config_fiber_1g(sds);

		/* init auto */
		rtpcs_sds_write_bits(sds, 0x1f, 13, 15, 0, 0x109e);
		rtpcs_sds_write_bits(sds, 0x1f, 0x6, 14, 10, 0x8);
		rtpcs_sds_write_bits(sds, 0x1f, 0x7, 10, 4, 0x7f);
		break;

	case RTPCS_SDS_MODE_SGMII:
		rtpcs_sds_write_bits(sds, 0x24, 0x9, 15, 15, 0);

		/* TODO: where does this come from? SDK doesn't have this. */
		rtpcs_931x_sds_cmu_band_set(sds, true, 62, RTPCS_SDS_MODE_SGMII);
		break;

	case RTPCS_SDS_MODE_XSGMII:
		rtpcs_sds_write_bits(sds, 0x40, 0xE, 12, 12, 1);
		rtpcs_sds_write_bits(sds, 0x80, 0xE, 12, 12, 1);
		break;

	case RTPCS_SDS_MODE_USXGMII_10GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_10GQXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GSXGMII:
	case RTPCS_SDS_MODE_USXGMII_5GDXGMII:
	case RTPCS_SDS_MODE_USXGMII_2_5GSXGMII:
		u32 op_code = 0x6003;

		rtpcs_931x_sds_reset_leq_dfe(sds);
		rtpcs_931x_sds_rx_reset(sds);

		rtpcs_sds_write(sds, 0x7, 0x10, op_code);
		rtpcs_sds_write(sds, 0x6, 0x1d, 0x0480);
		rtpcs_sds_write(sds, 0x6, 0xe, 0x0400);
		break;

	case RTPCS_SDS_MODE_QSGMII:
	default:
		return -ENOTSUPP;
	}

	return 0;
}

static int rtpcs_931x_setup_serdes(struct rtpcs_serdes *sds,
				   enum rtpcs_sds_mode hw_mode)
{
	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,
	};
	struct rtpcs_serdes *even_sds = rtpcs_sds_get_even(sds);
	struct rtpcs_ctrl *ctrl = sds->ctrl;
	u32 sds_id = sds->id;
	u32 val;
	int ret;

	/*
	 * TODO: USXGMII is currently the swiss army knife to declare 10G
	 * multi port PHYs. Real devices use other modes instead. Especially
	 *
	 * - RTL8224 is driven in 10G_QXGMII
	 * - RTL8218D/E are driven in (Realtek proprietary) XSGMII (10G SGMII)
	 *
	 * For now, disable "USXGMII" modes we cannot configure properly. Only
	 * USXGMII_10GSXGMII is configured properly for now.
	 */
	if (hw_mode == RTPCS_SDS_MODE_USXGMII_10GDXGMII ||
	    hw_mode == RTPCS_SDS_MODE_USXGMII_10GQXGMII ||
	    hw_mode == RTPCS_SDS_MODE_XSGMII)
		return 0;

	val = rtpcs_sds_read_bits(sds, 0x1F, 0x9, 11, 6);

	pr_info("%s: fibermode %08X stored mode 0x%x", __func__,
		rtpcs_sds_read(sds, 0x1f, 0x9), val);
	pr_info("%s: SGMII mode %08X in 0x24 0x9", __func__,
		rtpcs_sds_read(sds, 0x24, 0x9));
	pr_info("%s: CMU mode %08X stored even SDS %d", __func__,
		rtpcs_sds_read(even_sds, 0x20, 0x12), even_sds->id);
	pr_info("%s: serdes_mode_ctrl %08X", __func__,  RTL931X_SERDES_MODE_CTRL + 4 * (sds_id >> 2));
	pr_info("%s CMU page 0x24 0x7 %08x\n", __func__, rtpcs_sds_read(sds, 0x24, 0x7));
	pr_info("%s CMU page 0x26 0x7 %08x\n", __func__, rtpcs_sds_read(sds, 0x26, 0x7));
	pr_info("%s CMU page 0x28 0x7 %08x\n", __func__, rtpcs_sds_read(sds, 0x28, 0x7));
	pr_info("%s XSG page 0x0 0xe %08x\n", __func__, rtpcs_sds_read(sds, 0x40, 0xe));
	pr_info("%s XSG2 page 0x0 0xe %08x\n", __func__, rtpcs_sds_read(sds, 0x80, 0xe));
	pr_info("%s: 2.5gbit %08X", __func__, rtpcs_sds_read(sds, 0x41, 0x14));

	rtpcs_931x_sds_power(sds, false);

	ret = rtpcs_931x_sds_config_hw_mode(sds, hw_mode);
	if (ret < 0)
		return ret;

	ret = rtpcs_93xx_sds_config_cmu(sds, hw_mode);
	if (ret < 0)
		return ret;

	switch (hw_mode) {
	case RTPCS_SDS_MODE_OFF:
		ret = rtpcs_931x_sds_set_port_media(sds, RTPCS_PORT_MEDIA_NONE);
		break;
	case RTPCS_SDS_MODE_2500BASEX:
		ret = rtpcs_931x_sds_set_port_media(sds, RTPCS_PORT_MEDIA_FIBER_2_5G);
		break;
	case RTPCS_SDS_MODE_10GBASER:
		ret = rtpcs_931x_sds_set_port_media(sds, RTPCS_PORT_MEDIA_FIBER_10G);
		break;
	case RTPCS_SDS_MODE_SGMII:
	case RTPCS_SDS_MODE_1000BASEX:
		ret = rtpcs_931x_sds_set_port_media(sds, RTPCS_PORT_MEDIA_FIBER_1G);
		break;
	default:
		break;
	}

	if (sds_id >= 2) {
		if (ctrl->chip_version == RTPCS_CHIP_V2)
			/* consider 9311 etc. RTL9313_CHIP_ID == HWP_CHIP_ID(unit)) */
			rtpcs_sds_write(sds, 0x2E, 0x1, board_sds_tx2[sds_id - 2]);
		else
			rtpcs_sds_write(sds, 0x2E, 0x1, board_sds_tx[sds_id - 2]);
	}

	rtpcs_931x_sds_set_polarity(sds, sds->tx_pol_inv, sds->rx_pol_inv);

	rtpcs_931x_sds_power(sds, true);

	ret = rtpcs_931x_sds_set_mode(sds, hw_mode);
	if (ret < 0)
		return ret;

	sds->hw_mode = hw_mode;

	return 0;
}

/**
 * rtpcs_931x_init_mac_groups - Initialize MAC groups
 *
 * RTL931x organizes MACs into 12 groups (one per SerDes) that must be explicitly
 * enabled before link establishment. Without initialization, link may fail or
 * packets may be corrupted, especially in USXGMII/XSGMII modes.
 *
 * Simply enable all MACs by writing 0xffffffff to all group registers. Unused
 * MACs and reserved bits are harmless, avoiding complex per-SerDes logic.
 *
 * This lives in the PCS driver since groups are tied to SerDes, and the DSA
 * driver has no SerDes awareness.
 */
static int rtpcs_931x_init_mac_groups(struct rtpcs_ctrl *ctrl)
{
	static const u32 mac_group_regs[] = {
		RTPCS_931X_MAC_GROUP0_1_CTRL,
		RTPCS_931X_MAC_GROUP2_3_CTRL,
		RTPCS_931X_MAC_GROUP4_CTRL,
		RTPCS_931X_MAC_GROUP5_CTRL,
		RTPCS_931X_MAC_GROUP6_7_CTRL,
		RTPCS_931X_MAC_GROUP8_11_CTRL,
	};
	int ret;

	for (int i = 0; i < ARRAY_SIZE(mac_group_regs); i++) {
		ret = regmap_write(ctrl->map, mac_group_regs[i], 0xffffffff);
		if (ret)
			return ret;
	}

	return 0;
}

static int rtpcs_931x_init(struct rtpcs_ctrl *ctrl)
{
	int ret;

	ret = rtpcs_931x_init_mac_groups(ctrl);
	if (ret < 0)
		return ret;

	return rtpcs_93xx_init_serdes_common(ctrl);
}

/* 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);
	struct rtpcs_ctrl *ctrl = link->ctrl;
	int port = link->port;
	int linkup, speed;

	state->link = 0;
	state->speed = SPEED_UNKNOWN;
	state->duplex = DUPLEX_UNKNOWN;
	state->pause &= ~(MLO_PAUSE_RX | MLO_PAUSE_TX);

	/* Read MAC side link twice */
	for (int i = 0; i < 2; i++)
		linkup = rtpcs_regmap_read_bits(ctrl, ctrl->cfg->mac_link_sts, port, port);

	if (!linkup)
		return;

	state->link = 1;
	state->duplex = rtpcs_regmap_read_bits(ctrl, ctrl->cfg->mac_link_dup_sts, port, port);

	speed = rtpcs_regmap_read_bits(ctrl, ctrl->cfg->mac_link_spd_sts,
				       ctrl->cfg->mac_link_spd_bits * (port + 1) - 1,
				       ctrl->cfg->mac_link_spd_bits * port);
	switch (speed) {
	case RTPCS_SPEED_10:
		state->speed = SPEED_10;
		break;
	case RTPCS_SPEED_100:
		state->speed = SPEED_100;
		break;
	case RTPCS_SPEED_1000:
		state->speed = SPEED_1000;
		break;
	case RTPCS_SPEED_10000:
	case RTPCS_SPEED_10000_LEGACY:
		/*
		 * The legacy mode is ok so far with minor inconsistencies. On RTL838x this flag
		 * is either 500M or 2G. It might be that MAC_GLITE_STS register tells more. On
		 * RTL839x this is either 500M or 10G. More info might be in MAC_LINK_500M_STS.
		 * Without support for the 500M modes simply resolve to 10G.
		 */
		state->speed = SPEED_10000;
		break;
	case RTPCS_SPEED_2500:
		state->speed = SPEED_2500;
		break;
	case RTPCS_SPEED_5000:
		state->speed = SPEED_5000;
		break;
	default:
		dev_err(ctrl->dev, "unknown speed %d\n", speed);
	}

	if (rtpcs_regmap_read_bits(ctrl, ctrl->cfg->mac_rx_pause_sts, port, port))
		state->pause |= MLO_PAUSE_RX;
	if (rtpcs_regmap_read_bits(ctrl, ctrl->cfg->mac_tx_pause_sts, port, port))
		state->pause |= MLO_PAUSE_TX;
}

static void rtpcs_pcs_an_restart(struct phylink_pcs *pcs)
{
	struct rtpcs_link *link = rtpcs_phylink_pcs_to_link(pcs);
	struct rtpcs_ctrl *ctrl = link->ctrl;
	struct rtpcs_serdes *sds = link->sds;

	mutex_lock(&ctrl->lock);
	sds->ops->restart_autoneg(sds);
	mutex_unlock(&ctrl->lock);
}

static int rtpcs_pcs_config(struct phylink_pcs *pcs, unsigned int neg_mode,
			    phy_interface_t interface, const unsigned long *advertising,
			    bool permit_pause_to_mac)
{
	struct rtpcs_link *link = rtpcs_phylink_pcs_to_link(pcs);
	struct rtpcs_ctrl *ctrl = link->ctrl;
	struct rtpcs_serdes *sds = link->sds;
	enum rtpcs_sds_mode hw_mode;
	int ret;

	ret = rtpcs_sds_determine_hw_mode(sds, interface, &hw_mode);
	if (ret < 0) {
		dev_err(ctrl->dev, "SerDes %u doesn't support %s mode\n", sds->id,
			phy_modes(interface));
		return -ENOTSUPP;
	}

	mutex_lock(&ctrl->lock);

	if (sds->hw_mode != hw_mode) {
		dev_info(ctrl->dev, "configure SerDes %u for mode %s\n", sds->id,
			 phy_modes(interface));

		ret = ctrl->cfg->setup_serdes(sds, hw_mode);
		if (ret < 0)
			goto out;

		sds->first_start = false;
	} else {
		dev_dbg(ctrl->dev, "SerDes %u already in mode %s, no change\n",
			 sds->id, phy_modes(interface));
	}

	ret = sds->ops->set_autoneg(sds, neg_mode, advertising);

out:
	mutex_unlock(&ctrl->lock);
	return ret;
}

struct phylink_pcs *rtpcs_create(struct device *dev, struct device_node *np, int port);
struct phylink_pcs *rtpcs_create(struct device *dev, struct device_node *np, int port)
{
	struct platform_device *pdev;
	struct device_node *pcs_np;
	struct rtpcs_serdes *sds;
	struct rtpcs_ctrl *ctrl;
	struct rtpcs_link *link;
	u32 sds_id;

	if (!np || !of_device_is_available(np))
		return ERR_PTR(-ENODEV);

	pcs_np = of_get_parent(np);
	if (!pcs_np)
		return ERR_PTR(-ENODEV);

	if (!of_device_is_available(pcs_np)) {
		of_node_put(pcs_np);
		return ERR_PTR(-ENODEV);
	}

	pdev = of_find_device_by_node(pcs_np);
	of_node_put(pcs_np);
	if (!pdev)
		return ERR_PTR(-EPROBE_DEFER);

	ctrl = platform_get_drvdata(pdev);
	if (!ctrl) {
		put_device(&pdev->dev);
		return ERR_PTR(-EPROBE_DEFER);
	}

	if (port < 0 || port > ctrl->cfg->cpu_port)
		return ERR_PTR(-EINVAL);

	if (of_property_read_u32(np, "reg", &sds_id))
		return ERR_PTR(-EINVAL);
	if (sds_id >= ctrl->cfg->serdes_count)
		return ERR_PTR(-EINVAL);

	sds = &ctrl->serdes[sds_id];
	if (rtpcs_sds_read(sds, 0, 0) < 0)
		return ERR_PTR(-EINVAL);
	if (sds->num_of_links >= RTPCS_MAX_LINKS_PER_SDS)
		return ERR_PTR(-ERANGE);

	link = kzalloc(sizeof(*link), GFP_KERNEL);
	if (!link) {
		put_device(&pdev->dev);
		return ERR_PTR(-ENOMEM);
	}

	device_link_add(dev, ctrl->dev, DL_FLAG_AUTOREMOVE_CONSUMER);

	sds->num_of_links++;
	link->ctrl = ctrl;
	link->port = port;
	link->sds = sds;
	link->pcs.ops = ctrl->cfg->pcs_ops;
	link->pcs.neg_mode = true;

	ctrl->link[port] = link;

	dev_dbg(ctrl->dev, "phylink_pcs created, port %d, sds %d\n", port, sds_id);

	return &link->pcs;
}
EXPORT_SYMBOL(rtpcs_create);

static struct mii_bus *rtpcs_probe_serdes_bus(struct rtpcs_ctrl *ctrl)
{
	struct device_node *np;
	struct mii_bus *bus;

	np = of_find_compatible_node(NULL, NULL, "realtek,otto-serdes-mdio");
	if (!np) {
		dev_err(ctrl->dev, "SerDes mdio bus not found in DT");
		return ERR_PTR(-ENODEV);
	}

	bus = of_mdio_find_bus(np);
	of_node_put(np);
	if (!bus) {
		dev_warn(ctrl->dev, "SerDes mdio bus not (yet) active");
		return ERR_PTR(-EPROBE_DEFER);
	}

	if (!of_device_is_available(np)) {
		dev_err(ctrl->dev, "SerDes mdio bus not usable");
		return ERR_PTR(-ENODEV);
	}

	return bus;
}

static int rtpcs_probe(struct platform_device *pdev)
{
	struct device_node *np = pdev->dev.of_node;
	struct device *dev = &pdev->dev;
	struct device_node *child;
	struct rtpcs_serdes *sds;
	struct rtpcs_ctrl *ctrl;
	u32 sds_id;
	int i, ret;

	ctrl = devm_kzalloc(dev, sizeof(*ctrl), GFP_KERNEL);
	if (!ctrl)
		return -ENOMEM;

	mutex_init(&ctrl->lock);

	ctrl->dev = dev;
	ctrl->cfg = (const struct rtpcs_config *)device_get_match_data(ctrl->dev);
	ctrl->map = syscon_node_to_regmap(np->parent);
	if (IS_ERR(ctrl->map))
		return PTR_ERR(ctrl->map);

	ctrl->bus = rtpcs_probe_serdes_bus(ctrl);
	if (IS_ERR(ctrl->bus))
		return PTR_ERR(ctrl->bus);

	for (i = 0; i < ctrl->cfg->serdes_count; i++) {
		sds = &ctrl->serdes[i];

		sds->ctrl = ctrl;
		sds->first_start = true;
		sds->id = i;
		sds->ops = ctrl->cfg->sds_ops;
		sds->regs = ctrl->cfg->sds_regs;
	}

	for_each_child_of_node(dev->of_node, child) {
		ret = of_property_read_u32(child, "reg", &sds_id);
		if (ret)
			return ret;
		if (sds_id >= ctrl->cfg->serdes_count)
			return -EINVAL;

		sds = &ctrl->serdes[sds_id];
		sds->rx_pol_inv = of_property_read_bool(child, "realtek,pnswap-rx");
		sds->tx_pol_inv = of_property_read_bool(child, "realtek,pnswap-tx");
	}

	if (ctrl->cfg->init_serdes_common) {
		ret = ctrl->cfg->init_serdes_common(ctrl);
		if (ret)
			return ret;
	}

	/*
	 * rtpcs_create() relies on that fact that data is attached to the platform device to
	 * determine if the driver is ready. Do this after everything is initialized properly.
	 */
	platform_set_drvdata(pdev, ctrl);

	dev_info(dev, "Realtek PCS driver initialized\n");

	return 0;
}

static const struct phylink_pcs_ops rtpcs_838x_pcs_ops = {
	.pcs_an_restart		= rtpcs_pcs_an_restart,
	.pcs_config		= rtpcs_pcs_config,
	.pcs_get_state		= rtpcs_pcs_get_state,
};

static const struct rtpcs_serdes_ops rtpcs_838x_sds_ops = {
	.read			= rtpcs_generic_sds_op_read,
	.write			= rtpcs_generic_sds_op_write,
	.set_autoneg		= rtpcs_generic_sds_set_autoneg,
	.restart_autoneg	= rtpcs_generic_sds_restart_autoneg,
};

static const struct rtpcs_sds_regs rtpcs_838x_sds_regs = {
	.an_enable	= { .page = 0x2, .reg = MII_BMCR, .msb = 12, .lsb = 12 },
	.an_restart	= { .page = 0x2, .reg = MII_BMCR, .msb = 9, .lsb = 9 },
	.an_advertise	= { .page = 0x2, .reg = MII_ADVERTISE, .msb = 15, .lsb = 0 },
};

static const struct rtpcs_config rtpcs_838x_cfg = {
	.cpu_port		= RTPCS_838X_CPU_PORT,
	.mac_link_dup_sts	= RTPCS_838X_MAC_LINK_DUP_STS,
	.mac_link_spd_sts	= RTPCS_838X_MAC_LINK_SPD_STS,
	.mac_link_spd_bits	= RTPCS_83XX_MAC_LINK_SPD_BITS,
	.mac_link_sts		= RTPCS_838X_MAC_LINK_STS,
	.mac_rx_pause_sts	= RTPCS_838X_MAC_RX_PAUSE_STS,
	.mac_tx_pause_sts	= RTPCS_838X_MAC_TX_PAUSE_STS,
	.serdes_count		= RTPCS_838X_SERDES_CNT,
	.pcs_ops		= &rtpcs_838x_pcs_ops,
	.sds_ops		= &rtpcs_838x_sds_ops,
	.sds_regs		= &rtpcs_838x_sds_regs,
	.init_serdes_common	= rtpcs_838x_init_serdes_common,
	.setup_serdes		= rtpcs_838x_setup_serdes,
};

static const struct phylink_pcs_ops rtpcs_839x_pcs_ops = {
	.pcs_an_restart		= rtpcs_pcs_an_restart,
	.pcs_config		= rtpcs_pcs_config,
	.pcs_get_state		= rtpcs_pcs_get_state,
};

static const struct rtpcs_serdes_ops rtpcs_839x_sds_ops = {
	.read			= rtpcs_generic_sds_op_read,
	.write			= rtpcs_generic_sds_op_write,
	.set_autoneg		= rtpcs_generic_sds_set_autoneg,
	.restart_autoneg	= rtpcs_generic_sds_restart_autoneg,
};

static const struct rtpcs_sds_regs rtpcs_839x_sds_regs = {
	.an_enable	= { .page = 0x2, .reg = MII_BMCR, .msb = 12, .lsb = 12 },
	.an_restart	= { .page = 0x2, .reg = MII_BMCR, .msb = 9, .lsb = 9 },
	.an_advertise	= { .page = 0x2, .reg = MII_ADVERTISE, .msb = 15, .lsb = 0 },
};

static const struct rtpcs_config rtpcs_839x_cfg = {
	.cpu_port		= RTPCS_839X_CPU_PORT,
	.mac_link_dup_sts	= RTPCS_839X_MAC_LINK_DUP_STS,
	.mac_link_spd_sts	= RTPCS_839X_MAC_LINK_SPD_STS,
	.mac_link_spd_bits	= RTPCS_83XX_MAC_LINK_SPD_BITS,
	.mac_link_sts		= RTPCS_839X_MAC_LINK_STS,
	.mac_rx_pause_sts	= RTPCS_839X_MAC_RX_PAUSE_STS,
	.mac_tx_pause_sts	= RTPCS_839X_MAC_TX_PAUSE_STS,
	.serdes_count		= RTPCS_839X_SERDES_CNT,
	.pcs_ops		= &rtpcs_839x_pcs_ops,
	.sds_ops		= &rtpcs_839x_sds_ops,
	.sds_regs		= &rtpcs_839x_sds_regs,
	.init_serdes_common	= rtpcs_839x_init_serdes_common,
	.setup_serdes		= rtpcs_839x_setup_serdes,
};

static const struct phylink_pcs_ops rtpcs_930x_pcs_ops = {
	.pcs_an_restart		= rtpcs_pcs_an_restart,
	.pcs_config		= rtpcs_pcs_config,
	.pcs_get_state		= rtpcs_pcs_get_state,
};

static const struct rtpcs_serdes_ops rtpcs_930x_sds_ops = {
	.read			= rtpcs_930x_sds_op_read,
	.write			= rtpcs_930x_sds_op_write,
	.xsg_write		= rtpcs_930x_sds_op_xsg_write,
	.set_autoneg		= rtpcs_93xx_sds_set_autoneg,
	.restart_autoneg	= rtpcs_generic_sds_restart_autoneg,
	.get_pll_select		= rtpcs_930x_sds_get_pll_select,
	.set_pll_select		= rtpcs_930x_sds_set_pll_select,
	.reset_cmu		= rtpcs_930x_sds_reset_cmu,
	.reconfigure_to_pll	= rtpcs_930x_sds_reconfigure_to_pll,
};

static const struct rtpcs_sds_regs rtpcs_930x_sds_regs = {
	.an_enable	= { .page = 0x2, .reg = MII_BMCR, .msb = 12, .lsb = 12 },
	.an_restart	= { .page = 0x2, .reg = MII_BMCR, .msb = 9, .lsb = 9 },
	.an_advertise	= { .page = 0x2, .reg = MII_ADVERTISE, .msb = 15, .lsb = 0 },
};

static const struct rtpcs_config rtpcs_930x_cfg = {
	.cpu_port		= RTPCS_930X_CPU_PORT,
	.mac_link_dup_sts	= RTPCS_930X_MAC_LINK_DUP_STS,
	.mac_link_spd_sts	= RTPCS_930X_MAC_LINK_SPD_STS,
	.mac_link_spd_bits	= RTPCS_93XX_MAC_LINK_SPD_BITS,
	.mac_link_sts		= RTPCS_930X_MAC_LINK_STS,
	.mac_rx_pause_sts	= RTPCS_930X_MAC_RX_PAUSE_STS,
	.mac_tx_pause_sts	= RTPCS_930X_MAC_TX_PAUSE_STS,
	.serdes_count		= RTPCS_930X_SERDES_CNT,
	.pcs_ops		= &rtpcs_930x_pcs_ops,
	.sds_ops		= &rtpcs_930x_sds_ops,
	.sds_regs		= &rtpcs_930x_sds_regs,
	.init_serdes_common	= rtpcs_93xx_init_serdes_common,
	.setup_serdes		= rtpcs_930x_setup_serdes,
};

static const struct phylink_pcs_ops rtpcs_931x_pcs_ops = {
	.pcs_an_restart		= rtpcs_pcs_an_restart,
	.pcs_config		= rtpcs_pcs_config,
	.pcs_get_state		= rtpcs_pcs_get_state,
};

static const struct rtpcs_serdes_ops rtpcs_931x_sds_ops = {
	.read			= rtpcs_generic_sds_op_read,
	.write			= rtpcs_generic_sds_op_write,
	.xsg_write		= rtpcs_931x_sds_op_xsg_write,
	.set_autoneg		= rtpcs_93xx_sds_set_autoneg,
	.restart_autoneg	= rtpcs_generic_sds_restart_autoneg,
	.get_pll_select		= rtpcs_931x_sds_get_pll_select,
	.set_pll_select		= rtpcs_931x_sds_set_pll_select,
	.reconfigure_to_pll	= rtpcs_931x_sds_reconfigure_to_pll,
};

static const struct rtpcs_sds_regs rtpcs_931x_sds_regs = {
	.an_enable	= { .page = 0x42, .reg = MII_BMCR, .msb = 12, .lsb = 12 },
	.an_restart	= { .page = 0x42, .reg = MII_BMCR, .msb = 9, .lsb = 9 },
	.an_advertise	= { .page = 0x42, .reg = MII_ADVERTISE, .msb = 15, .lsb = 0 },
};

static const struct rtpcs_config rtpcs_931x_cfg = {
	.cpu_port		= RTPCS_931X_CPU_PORT,
	.mac_link_dup_sts	= RTPCS_931X_MAC_LINK_DUP_STS,
	.mac_link_spd_sts	= RTPCS_931X_MAC_LINK_SPD_STS,
	.mac_link_spd_bits	= RTPCS_93XX_MAC_LINK_SPD_BITS,
	.mac_link_sts		= RTPCS_931X_MAC_LINK_STS,
	.mac_rx_pause_sts	= RTPCS_931X_MAC_RX_PAUSE_STS,
	.mac_tx_pause_sts	= RTPCS_931X_MAC_TX_PAUSE_STS,
	.serdes_count		= RTPCS_931X_SERDES_CNT,
	.pcs_ops		= &rtpcs_931x_pcs_ops,
	.sds_ops		= &rtpcs_931x_sds_ops,
	.sds_regs		= &rtpcs_931x_sds_regs,
	.init_serdes_common	= rtpcs_931x_init,
	.setup_serdes		= rtpcs_931x_setup_serdes,
};

static const struct of_device_id rtpcs_of_match[] = {
	{
		.compatible = "realtek,rtl8380-pcs",
		.data = &rtpcs_838x_cfg,
	},
	{
		.compatible = "realtek,rtl8392-pcs",
		.data = &rtpcs_839x_cfg,
	},
	{
		.compatible = "realtek,rtl9301-pcs",
		.data = &rtpcs_930x_cfg,
	},
	{
		.compatible = "realtek,rtl9311-pcs",
		.data = &rtpcs_931x_cfg,
	},
	{ /* sentinel */ }
};
MODULE_DEVICE_TABLE(of, rtpcs_of_match);

static struct platform_driver rtpcs_driver = {
	.driver = {
		.name = "realtek-otto-pcs",
		.of_match_table = rtpcs_of_match
	},
	.probe = rtpcs_probe,
};
module_platform_driver(rtpcs_driver);

MODULE_AUTHOR("Markus Stockhausen <markus.stockhausen@gmx.de>");
MODULE_AUTHOR("Jonas Jelonek <jelonek.jonas@gmail.com>");
MODULE_DESCRIPTION("Realtek Otto SerDes PCS driver");
MODULE_LICENSE("GPL v2");
