diff --git a/drivers/net/mdio/mdio-realtek-rtl9300.c b/drivers/net/mdio/mdio-realtek-rtl9300.c index a925640290cb..d6027062e452 100644 --- a/drivers/net/mdio/mdio-realtek-rtl9300.c +++ b/drivers/net/mdio/mdio-realtek-rtl9300.c @@ -179,6 +179,9 @@ #define PHY_CTRL_MMD_DEVAD GENMASK(20, 16) #define PHY_CTRL_MMD_REG GENMASK(15, 0) +#define RTL_VENDOR_ID 0x001cc800 +#define RTL_PAGE_SELECT 31 + #define MAP_ADDRS_PER_REG 6 #define MAP_BITS_PER_ADDR 5 #define MAP_BITS_PER_BUS 2 @@ -204,6 +207,7 @@ struct otto_emdio_priv { struct regmap *regmap; struct mutex lock; /* protect HW access */ DECLARE_BITMAP(valid_ports, MAX_PORTS); + u16 page[MAX_PORTS]; u8 smi_bus[MAX_PORTS]; u8 smi_addr[MAX_PORTS]; bool smi_bus_is_c45[MAX_SMI_BUSSES]; @@ -355,7 +359,7 @@ static int otto_emdio_9300_read_c22(struct mii_bus *bus, int port, int regnum, u struct otto_emdio_cmd_regs cmd_data = { .c22_data = FIELD_PREP(RTL9300_PHY_CTRL_REG_ADDR, regnum) | FIELD_PREP(RTL9300_PHY_CTRL_PARK_PAGE, 0x1f) | - FIELD_PREP(RTL9300_PHY_CTRL_MAIN_PAGE, RAW_PAGE(priv)), + FIELD_PREP(RTL9300_PHY_CTRL_MAIN_PAGE, priv->page[port]), .io_data = FIELD_PREP(RTL9300_PHY_CTRL_INDATA, port), }; @@ -369,7 +373,7 @@ static int otto_emdio_9300_write_c22(struct mii_bus *bus, int port, int regnum, struct otto_emdio_cmd_regs cmd_data = { .c22_data = FIELD_PREP(RTL9300_PHY_CTRL_REG_ADDR, regnum) | FIELD_PREP(RTL9300_PHY_CTRL_PARK_PAGE, 0x1f) | - FIELD_PREP(RTL9300_PHY_CTRL_MAIN_PAGE, RAW_PAGE(priv)), + FIELD_PREP(RTL9300_PHY_CTRL_MAIN_PAGE, priv->page[port]), .io_data = FIELD_PREP(RTL9300_PHY_CTRL_INDATA, value), .port_mask_low = BIT(port), }; @@ -409,7 +413,7 @@ static int otto_emdio_9310_read_c22(struct mii_bus *bus, int port, int regnum, u struct otto_emdio_cmd_regs cmd_data = { .broadcast = FIELD_PREP(RTL9310_BC_PORT_ID, port), .c22_data = FIELD_PREP(RTL9310_PHY_CTRL_REG_ADDR, regnum) | - FIELD_PREP(RTL9310_PHY_CTRL_MAIN_PAGE, RAW_PAGE(priv)), + FIELD_PREP(RTL9310_PHY_CTRL_MAIN_PAGE, priv->page[port]), }; return otto_emdio_read_cmd(bus, RTL9310_PHY_CTRL_TYPE_C22, &cmd_data, @@ -421,7 +425,7 @@ static int otto_emdio_9310_write_c22(struct mii_bus *bus, int port, int regnum, struct otto_emdio_priv *priv = otto_emdio_bus_to_priv(bus); struct otto_emdio_cmd_regs cmd_data = { .c22_data = FIELD_PREP(RTL9310_PHY_CTRL_REG_ADDR, regnum) | - FIELD_PREP(RTL9310_PHY_CTRL_MAIN_PAGE, RAW_PAGE(priv)), + FIELD_PREP(RTL9310_PHY_CTRL_MAIN_PAGE, priv->page[port]), .io_data = FIELD_PREP(RTL9310_PHY_CTRL_INDATA, value), .port_mask_high = (u32)(BIT_ULL(port) >> 32), .port_mask_low = (u32)(BIT_ULL(port)), @@ -473,8 +477,12 @@ static int otto_emdio_read_c22(struct mii_bus *bus, int phy_id, int regnum) if (port < 0) return port; - scoped_guard(mutex, &priv->lock) + scoped_guard(mutex, &priv->lock) { + if (regnum == RTL_PAGE_SELECT) + return priv->page[port]; + ret = priv->info->read_c22(bus, port, regnum, &value); + } return ret ? ret : value; } @@ -495,8 +503,17 @@ static int otto_emdio_write_c22(struct mii_bus *bus, int phy_id, int regnum, if (port < 0) return port; - scoped_guard(mutex, &priv->lock) + scoped_guard(mutex, &priv->lock) { + if (regnum == RTL_PAGE_SELECT) { + if (value >= RAW_PAGE(priv)) + return -EINVAL; + + priv->page[port] = value; + return 0; + } + ret = priv->info->write_c22(bus, port, regnum, value); + } return ret; } @@ -607,8 +624,9 @@ static int otto_emdio_9310_setup_controller(struct otto_emdio_priv *priv) static int otto_emdio_notify_phy_attach(struct phy_device *phydev) { - struct otto_emdio_priv *priv = otto_emdio_bus_to_priv(phydev->mdio.bus); int port = otto_emdio_phy_to_port(phydev->mdio.bus, phydev->mdio.addr); + struct otto_emdio_chan *chan = phydev->mdio.bus->priv; + struct otto_emdio_priv *priv = chan->priv; if (port < 0) { /* All subsequent bus operations will fail */ @@ -616,6 +634,16 @@ static int otto_emdio_notify_phy_attach(struct phy_device *phydev) return port; } + /* "sync" page in case of previously failed attachment */ + scoped_guard(mutex, &priv->lock) + priv->page[port] = 0; + + if (!priv->smi_bus_is_c45[chan->mdio_bus] && + !phy_id_compare_vendor(phydev->phy_id, RTL_VENDOR_ID)) { + phydev_err(phydev, "Only Realtek PHYs allowed on C22 bus\n"); + return -EOPNOTSUPP; + } + return otto_emdio_set_port_polling(priv, port, true); } @@ -631,6 +659,10 @@ static void otto_emdio_notify_phy_detach(struct phy_device *phydev) if (port < 0) return; + /* "sync" page for next attachment */ + scoped_guard(mutex, &priv->lock) + priv->page[port] = 0; + if (otto_emdio_set_port_polling(priv, port, false)) dev_err(bus->parent, "failed to disable polling for port %d\n", port); }