diff --git a/drivers/net/pcs/pcs-lynx.c b/drivers/net/pcs/pcs-lynx.c index da4f99059eef..6d94f92a3332 100644 --- a/drivers/net/pcs/pcs-lynx.c +++ b/drivers/net/pcs/pcs-lynx.c @@ -20,6 +20,12 @@ #define IF_MODE_SPEED_MSK GENMASK(3, 2) #define IF_MODE_HALF_DUPLEX BIT(4) +/* USXGMII replicator link timer step is 3.2 ns (312.5M XGMII columns per sec) + * for single port mode. For quad port mode, it is 1/4 of that. + */ +#define LINK_TIMER_VAL_USXGMII(ns) ((u32)((ns) * 10 / 32)) +#define LINK_TIMER_VAL_10G_QXGMII(ns) ((u32)((ns) * 10 / 128)) + struct lynx_pcs { struct phylink_pcs pcs; struct mdio_device *mdio; @@ -158,6 +164,9 @@ static int lynx_pcs_config_usxgmii(struct mdio_device *pcs, { struct mii_bus *bus = pcs->bus; int addr = pcs->addr; + int link_timer_ns; + u32 link_timer; + int ret; if (neg_mode != PHYLINK_PCS_NEG_INBAND_ENABLED) { dev_err(&pcs->dev, "%s only supports in-band AN for now\n", @@ -166,10 +175,46 @@ static int lynx_pcs_config_usxgmii(struct mdio_device *pcs, } /* Configure device ability for the USXGMII Replicator */ - return mdiobus_c45_write(bus, addr, MDIO_MMD_VEND2, MII_ADVERTISE, - MDIO_USXGMII_10G | MDIO_USXGMII_LINK | - MDIO_USXGMII_FULL_DUPLEX | - ADVERTISE_SGMII | ADVERTISE_LPACK); + ret = mdiobus_c45_write(bus, addr, MDIO_MMD_VEND2, MII_ADVERTISE, + MDIO_USXGMII_10G | MDIO_USXGMII_LINK | + MDIO_USXGMII_FULL_DUPLEX | + ADVERTISE_SGMII | ADVERTISE_LPACK); + if (ret < 0) { + dev_err(&pcs->dev, "could not set USXGMII replicator config\n"); + return ret; + } + + link_timer_ns = phylink_get_link_timer_ns(interface); + if (link_timer_ns > 0) { + if (interface == PHY_INTERFACE_MODE_10G_QXGMII) + link_timer = LINK_TIMER_VAL_10G_QXGMII(link_timer_ns); + else + link_timer = LINK_TIMER_VAL_USXGMII(link_timer_ns); + + ret = mdiobus_c45_write(bus, addr, MDIO_MMD_VEND2, + LINK_TIMER_LO, link_timer & 0xffff); + if (ret < 0) { + dev_err(&pcs->dev, "could not set USXGMII Link Timer 1\n"); + return ret; + } + + ret = mdiobus_c45_write(bus, addr, MDIO_MMD_VEND2, + LINK_TIMER_HI, (link_timer >> 16) & 0x1f); + if (ret < 0) { + dev_err(&pcs->dev, "could not set USXGMII Link Timer 2\n"); + return ret; + } + } + + /* Configure autonegotiation */ + ret = mdiobus_c45_write(bus, addr, MDIO_MMD_VEND2, MII_BMCR, + BMCR_RESET | BMCR_ANENABLE | BMCR_ANRESTART); + if (ret < 0) { + dev_err(&pcs->dev, "could not set USXGMII replicator control config\n"); + return ret; + } + + return ret; } static int lynx_pcs_config(struct phylink_pcs *pcs, unsigned int neg_mode,