target/linux/layerscape/patches-5.4/701-net-0386-net-dsa-felix-Add-PCS-operations-for-PHYLINK.patch

   1 From 78d77114e35a491232c50e054f43c980bbf9d434 Mon Sep 17 00:00:00 2001
   2 From: Vladimir Oltean <vladimir.oltean@nxp.com>
   3 Date: Mon, 6 Jan 2020 03:34:17 +0200
   4 Subject: [PATCH] net: dsa: felix: Add PCS operations for PHYLINK
   5
   6 Layerscape SoCs traditionally expose the SerDes configuration/status for
   7 Ethernet protocols (PCS for SGMII/USXGMII/10GBase-R etc etc) in a register
   8 format that is compatible with clause 22 or clause 45 (depending on
   9 SerDes protocol). Each MAC has its own internal MDIO bus on which there
  10 is one or more of these PCS's, responding to commands at a configurable
  11 PHY address. The per-port internal MDIO bus (which is just for PCSs) is
  12 totally separate and has nothing to do with the dedicated external MDIO
  13 controller (which is just for PHYs), but the register map for the MDIO
  14 controller is the same.
  15
  16 The VSC9959 (Felix) switch instantiated in the LS1028A is integrated
  17 in hardware with the ENETC PCS of its DSA master, and reuses its MDIO
  18 controller driver, so Felix has been made to depend on it in Kconfig.
  19
  20  +------------------------------------------------------------------------+
  21  |                   +--------+ GMII (typically disabled via RCW)         |
  22  | ENETC PCI         |  ENETC |--------------------------+                |
  23  | Root Complex      | port 3 |-----------------------+  |                |
  24  | Integrated        +--------+                       |  |                |
  25  | Endpoint                                           |  |                |
  26  |                   +--------+ 2.5G GMII             |  |                |
  27  |                   |  ENETC |--------------+        |  |                |
  28  |                   | port 2 |-----------+  |        |  |                |
  29  |                   +--------+           |  |        |  |                |
  30  |                                     +--------+  +--------+             |
  31  |                                     |  Felix |  |  Felix |             |
  32  |                                     | port 4 |  | port 5 |             |
  33  |                                     +--------+  +--------+             |
  34  |                                                                        |
  35  | +--------+  +--------+  +--------+  +--------+  +--------+  +--------+ |
  36  | |  ENETC |  |  ENETC |  |  Felix |  |  Felix |  |  Felix |  |  Felix | |
  37  | | port 0 |  | port 1 |  | port 0 |  | port 1 |  | port 2 |  | port 3 | |
  38  +------------------------------------------------------------------------+
  39  |    ||||  SerDes |          ||||        ||||        ||||        ||||    |
  40  | +--------+block |       +--------------------------------------------+ |
  41  | |  ENETC |      |       |       ENETC port 2 internal MDIO bus       | |
  42  | | port 0 |      |       |  PCS         PCS          PCS        PCS   | |
  43  | |   PCS  |      |       |   0           1            2          3    | |
  44  +-----------------|------------------------------------------------------+
  45         v          v           v           v            v          v
  46      SGMII/      RGMII    QSGMII/QSXGMII/4xSGMII/4x1000Base-X/4x2500Base-X
  47     USXGMII/   (bypasses
  48   1000Base-X/   SerDes)
  49   2500Base-X
  50
  51 In the LS1028A SoC described above, the VSC9959 Felix switch is PF5 of
  52 the ENETC root complex, and has 2 BARs:
  53 - BAR 4: the switch's effective registers
  54 - BAR 0: the MDIO controller register map lended from ENETC port 2
  55          (PF2), for accessing its associated PCS's.
  56
  57 This explanation is necessary because the patch does some renaming
  58 "pci_bar" -> "switch_pci_bar" for clarity, which would otherwise appear
  59 a bit obtuse.
  60
  61 The fact that the internal MDIO bus is "borrowed" is relevant because
  62 the register map is found in PF5 (the switch) but it triggers an access
  63 fault if PF2 (the ENETC DSA master) is not enabled. This is not treated
  64 in any way (and I don't think it can be treated).
  65
  66 All of this is so SoC-specific, that it was contained as much as
  67 possible in the platform-integration file felix_vsc9959.c.
  68
  69 We need to parse and pre-validate the device tree because of 2 reasons:
  70 - The PHY mode (SerDes protocol) cannot change at runtime due to SoC
  71   design.
  72 - There is a circular dependency in that we need to know what clause the
  73   PCS speaks in order to find it on the internal MDIO bus. But the
  74   clause of the PCS depends on what phy-mode it is configured for.
  75
  76 The goal of this patch is to make steps towards removing the bootloader
  77 dependency for SGMII PCS pre-configuration, as well as to add support
  78 for monitoring the in-band SGMII AN between the PCS and the system-side
  79 link partner (PHY or other MAC).
  80
  81 In practice the bootloader dependency is not completely removed. U-Boot
  82 pre-programs the PHY address at which each PCS can be found on the
  83 internal MDIO bus (MDEV_PORT). This is needed because the PCS of each
  84 port has the same out-of-reset PHY address of zero. The SerDes register
  85 for changing MDEV_PORT is pretty deep in the SoC (outside the addresses
  86 of the ENETC PCI BARs) and therefore inaccessible to us from here.
  87
  88 Felix VSC9959 and Ocelot VSC7514 are integrated very differently in
  89 their respective SoCs, and for that reason Felix does not use the Ocelot
  90 core library for PHYLINK. On one hand we don't want to impose the
  91 fixed phy-mode limitation to Ocelot, and on the other hand Felix doesn't
  92 need to force the MAC link speed the way Ocelot does, since the MAC is
  93 connected to the PCS through a fixed GMII, and the PCS is the one who
  94 does the rate adaptation at lower link speeds, which the MAC does not
  95 even need to know about. In fact changing the GMII speed for Felix
  96 irrecoverably breaks transmission through that port until a reset.
  97
  98 The pair with ENETC port 3 and Felix port 5 is optional and doesn't
  99 support tagging. When we enable it, swp5 is a regular slave port, albeit
 100 an internal one. The trouble is that it doesn't work, and that is
 101 because the DSA PHYLIB adaptation layer doesn't treat fixed-link slave
 102 ports. So that is yet another reason for wanting to convert Felix to the
 103 native PHYLINK API.
 104
 105 Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
 106
 107 Conflicts:
 108         drivers/net/dsa/ocelot/felix.c
 109
 110 with the upstream API change for of_get_phy_mode() introduced in
 111 0c65b2b90d13 ("net: of_get_phy_mode: Change API to solve int/unit
 112 warnings") and merged in v5.4-rc5.
 113 ---
 114  drivers/net/dsa/ocelot/Kconfig         |   2 +
 115  drivers/net/dsa/ocelot/felix.c         | 267 +++++++++++++++++-
 116  drivers/net/dsa/ocelot/felix.h         |  16 +-
 117  drivers/net/dsa/ocelot/felix_vsc9959.c | 501 ++++++++++++++++++++++++++++++++-
 118  4 files changed, 769 insertions(+), 17 deletions(-)
 119
 120 --- a/drivers/net/dsa/ocelot/Kconfig
 121 +++ b/drivers/net/dsa/ocelot/Kconfig
 122 @@ -3,8 +3,10 @@ config NET_DSA_MSCC_FELIX
 123         tristate "Ocelot / Felix Ethernet switch support"
 124         depends on NET_DSA && PCI
 125         depends on NET_VENDOR_MICROSEMI
 126 +       depends on NET_VENDOR_FREESCALE
 127         select MSCC_OCELOT_SWITCH
 128         select NET_DSA_TAG_OCELOT
 129 +       select FSL_ENETC_MDIO
 130         help
 131           This driver supports the VSC9959 network switch, which is a member of
 132           the Vitesse / Microsemi / Microchip Ocelot family of switching cores.
 133 --- a/drivers/net/dsa/ocelot/felix.c
 134 +++ b/drivers/net/dsa/ocelot/felix.c
 135 @@ -2,9 +2,14 @@
 136  /* Copyright 2019 NXP Semiconductors
 137   */
 138  #include <uapi/linux/if_bridge.h>
 139 +#include <soc/mscc/ocelot_qsys.h>
 140 +#include <soc/mscc/ocelot_sys.h>
 141 +#include <soc/mscc/ocelot_dev.h>
 142 +#include <soc/mscc/ocelot_ana.h>
 143  #include <soc/mscc/ocelot.h>
 144  #include <linux/packing.h>
 145  #include <linux/module.h>
 146 +#include <linux/of_net.h>
 147  #include <linux/pci.h>
 148  #include <linux/of.h>
 149  #include <net/dsa.h>
 150 @@ -58,14 +63,6 @@ static int felix_set_ageing_time(struct
 151         return 0;
 152  }
 153
 154 -static void felix_adjust_link(struct dsa_switch *ds, int port,
 155 -                             struct phy_device *phydev)
 156 -{
 157 -       struct ocelot *ocelot = ds->priv;
 158 -
 159 -       ocelot_adjust_link(ocelot, port, phydev);
 160 -}
 161 -
 162  static int felix_fdb_dump(struct dsa_switch *ds, int port,
 163                           dsa_fdb_dump_cb_t *cb, void *data)
 164  {
 165 @@ -202,6 +199,138 @@ static void felix_port_disable(struct ds
 166         return ocelot_port_disable(ocelot, port);
 167  }
 168
 169 +static void felix_phylink_validate(struct dsa_switch *ds, int port,
 170 +                                  unsigned long *supported,
 171 +                                  struct phylink_link_state *state)
 172 +{
 173 +       struct ocelot *ocelot = ds->priv;
 174 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 175 +       __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
 176 +
 177 +       if (state->interface != PHY_INTERFACE_MODE_NA &&
 178 +           state->interface != ocelot_port->phy_mode) {
 179 +               bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
 180 +               return;
 181 +       }
 182 +
 183 +       /* No half-duplex. */
 184 +       phylink_set_port_modes(mask);
 185 +       phylink_set(mask, Autoneg);
 186 +       phylink_set(mask, Pause);
 187 +       phylink_set(mask, Asym_Pause);
 188 +       if (state->interface != PHY_INTERFACE_MODE_2500BASEX) {
 189 +               phylink_set(mask, 10baseT_Full);
 190 +               phylink_set(mask, 100baseT_Full);
 191 +               phylink_set(mask, 1000baseT_Full);
 192 +       }
 193 +       /* The internal ports that run at 2.5G are overclocked GMII */
 194 +       if (state->interface == PHY_INTERFACE_MODE_GMII ||
 195 +           state->interface == PHY_INTERFACE_MODE_2500BASEX ||
 196 +           state->interface == PHY_INTERFACE_MODE_USXGMII) {
 197 +               phylink_set(mask, 2500baseT_Full);
 198 +               phylink_set(mask, 2500baseX_Full);
 199 +       }
 200 +
 201 +       bitmap_and(supported, supported, mask,
 202 +                  __ETHTOOL_LINK_MODE_MASK_NBITS);
 203 +       bitmap_and(state->advertising, state->advertising, mask,
 204 +                  __ETHTOOL_LINK_MODE_MASK_NBITS);
 205 +}
 206 +
 207 +static int felix_phylink_mac_pcs_get_state(struct dsa_switch *ds, int port,
 208 +                                          struct phylink_link_state *state)
 209 +{
 210 +       struct ocelot *ocelot = ds->priv;
 211 +       struct felix *felix = ocelot_to_felix(ocelot);
 212 +
 213 +       if (felix->info->pcs_link_state)
 214 +               felix->info->pcs_link_state(ocelot, port, state);
 215 +
 216 +       return 0;
 217 +}
 218 +
 219 +static void felix_phylink_mac_config(struct dsa_switch *ds, int port,
 220 +                                    unsigned int link_an_mode,
 221 +                                    const struct phylink_link_state *state)
 222 +{
 223 +       struct ocelot *ocelot = ds->priv;
 224 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 225 +       struct felix *felix = ocelot_to_felix(ocelot);
 226 +       u32 mac_fc_cfg;
 227 +
 228 +       /* Take port out of reset by clearing the MAC_TX_RST, MAC_RX_RST and
 229 +        * PORT_RST bits in CLOCK_CFG
 230 +        */
 231 +       ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(state->speed),
 232 +                          DEV_CLOCK_CFG);
 233 +
 234 +       /* Flow control. Link speed is only used here to evaluate the time
 235 +        * specification in incoming pause frames.
 236 +        */
 237 +       mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(state->speed);
 238 +       if (state->pause & MLO_PAUSE_RX)
 239 +               mac_fc_cfg |= SYS_MAC_FC_CFG_RX_FC_ENA;
 240 +       if (state->pause & MLO_PAUSE_TX)
 241 +               mac_fc_cfg |= SYS_MAC_FC_CFG_TX_FC_ENA |
 242 +                             SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 243 +                             SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 244 +                             SYS_MAC_FC_CFG_ZERO_PAUSE_ENA;
 245 +       ocelot_write_rix(ocelot, mac_fc_cfg, SYS_MAC_FC_CFG, port);
 246 +
 247 +       ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
 248 +
 249 +       if (felix->info->pcs_init)
 250 +               felix->info->pcs_init(ocelot, port, link_an_mode, state);
 251 +}
 252 +
 253 +static void felix_phylink_mac_an_restart(struct dsa_switch *ds, int port)
 254 +{
 255 +       struct ocelot *ocelot = ds->priv;
 256 +       struct felix *felix = ocelot_to_felix(ocelot);
 257 +
 258 +       if (felix->info->pcs_an_restart)
 259 +               felix->info->pcs_an_restart(ocelot, port);
 260 +}
 261 +
 262 +static void felix_phylink_mac_link_down(struct dsa_switch *ds, int port,
 263 +                                       unsigned int link_an_mode,
 264 +                                       phy_interface_t interface)
 265 +{
 266 +       struct ocelot *ocelot = ds->priv;
 267 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 268 +
 269 +       ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
 270 +       ocelot_rmw_rix(ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
 271 +                      QSYS_SWITCH_PORT_MODE, port);
 272 +}
 273 +
 274 +static void felix_phylink_mac_link_up(struct dsa_switch *ds, int port,
 275 +                                     unsigned int link_an_mode,
 276 +                                     phy_interface_t interface,
 277 +                                     struct phy_device *phydev)
 278 +{
 279 +       struct ocelot *ocelot = ds->priv;
 280 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 281 +
 282 +       /* Enable MAC module */
 283 +       ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
 284 +                          DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 285 +
 286 +       /* Enable receiving frames on the port, and activate auto-learning of
 287 +        * MAC addresses.
 288 +        */
 289 +       ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
 290 +                        ANA_PORT_PORT_CFG_RECV_ENA |
 291 +                        ANA_PORT_PORT_CFG_PORTID_VAL(port),
 292 +                        ANA_PORT_PORT_CFG, port);
 293 +
 294 +       /* Core: Enable port for frame transfer */
 295 +       ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
 296 +                        QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
 297 +                        QSYS_SWITCH_PORT_MODE_PORT_ENA,
 298 +                        QSYS_SWITCH_PORT_MODE, port);
 299 +}
 300 +
 301  static void felix_get_strings(struct dsa_switch *ds, int port,
 302                               u32 stringset, u8 *data)
 303  {
 304 @@ -232,10 +361,78 @@ static int felix_get_ts_info(struct dsa_
 305         return ocelot_get_ts_info(ocelot, port, info);
 306  }
 307
 308 +static int felix_parse_ports_node(struct felix *felix,
 309 +                                 struct device_node *ports_node,
 310 +                                 phy_interface_t *port_phy_modes)
 311 +{
 312 +       struct ocelot *ocelot = &felix->ocelot;
 313 +       struct device *dev = felix->ocelot.dev;
 314 +       struct device_node *child;
 315 +
 316 +       for_each_child_of_node(ports_node, child) {
 317 +               phy_interface_t phy_mode;
 318 +               u32 port;
 319 +               int err;
 320 +
 321 +               /* Get switch port number from DT */
 322 +               if (of_property_read_u32(child, "reg", &port) < 0) {
 323 +                       dev_err(dev, "Port number not defined in device tree "
 324 +                               "(property \"reg\")\n");
 325 +                       of_node_put(child);
 326 +                       return -ENODEV;
 327 +               }
 328 +
 329 +               /* Get PHY mode from DT */
 330 +               err = of_get_phy_mode(child);
 331 +               if (err < 0) {
 332 +                       dev_err(dev, "Failed to read phy-mode or "
 333 +                               "phy-interface-type property for port %d\n",
 334 +                               port);
 335 +                       of_node_put(child);
 336 +                       return -ENODEV;
 337 +               }
 338 +
 339 +               phy_mode = err;
 340 +
 341 +               err = felix->info->prevalidate_phy_mode(ocelot, port, phy_mode);
 342 +               if (err < 0) {
 343 +                       dev_err(dev, "Unsupported PHY mode %s on port %d\n",
 344 +                               phy_modes(phy_mode), port);
 345 +                       return err;
 346 +               }
 347 +
 348 +               port_phy_modes[port] = phy_mode;
 349 +       }
 350 +
 351 +       return 0;
 352 +}
 353 +
 354 +static int felix_parse_dt(struct felix *felix, phy_interface_t *port_phy_modes)
 355 +{
 356 +       struct device *dev = felix->ocelot.dev;
 357 +       struct device_node *switch_node;
 358 +       struct device_node *ports_node;
 359 +       int err;
 360 +
 361 +       switch_node = dev->of_node;
 362 +
 363 +       ports_node = of_get_child_by_name(switch_node, "ports");
 364 +       if (!ports_node) {
 365 +               dev_err(dev, "Incorrect bindings: absent \"ports\" node\n");
 366 +               return -ENODEV;
 367 +       }
 368 +
 369 +       err = felix_parse_ports_node(felix, ports_node, port_phy_modes);
 370 +       of_node_put(ports_node);
 371 +
 372 +       return err;
 373 +}
 374 +
 375  static int felix_init_structs(struct felix *felix, int num_phys_ports)
 376  {
 377         struct ocelot *ocelot = &felix->ocelot;
 378 -       resource_size_t base;
 379 +       phy_interface_t *port_phy_modes;
 380 +       resource_size_t switch_base;
 381         int port, i, err;
 382
 383         ocelot->num_phys_ports = num_phys_ports;
 384 @@ -250,7 +447,19 @@ static int felix_init_structs(struct fel
 385         ocelot->shared_queue_sz = felix->info->shared_queue_sz;
 386         ocelot->ops             = felix->info->ops;
 387
 388 -       base = pci_resource_start(felix->pdev, felix->info->pci_bar);
 389 +       port_phy_modes = kcalloc(num_phys_ports, sizeof(phy_interface_t),
 390 +                                GFP_KERNEL);
 391 +       if (!port_phy_modes)
 392 +               return -ENOMEM;
 393 +
 394 +       err = felix_parse_dt(felix, port_phy_modes);
 395 +       if (err) {
 396 +               kfree(port_phy_modes);
 397 +               return err;
 398 +       }
 399 +
 400 +       switch_base = pci_resource_start(felix->pdev,
 401 +                                        felix->info->switch_pci_bar);
 402
 403         for (i = 0; i < TARGET_MAX; i++) {
 404                 struct regmap *target;
 405 @@ -261,13 +470,14 @@ static int felix_init_structs(struct fel
 406
 407                 res = &felix->info->target_io_res[i];
 408                 res->flags = IORESOURCE_MEM;
 409 -               res->start += base;
 410 -               res->end += base;
 411 +               res->start += switch_base;
 412 +               res->end += switch_base;
 413
 414                 target = ocelot_regmap_init(ocelot, res);
 415                 if (IS_ERR(target)) {
 416                         dev_err(ocelot->dev,
 417                                 "Failed to map device memory space\n");
 418 +                       kfree(port_phy_modes);
 419                         return PTR_ERR(target);
 420                 }
 421
 422 @@ -277,6 +487,7 @@ static int felix_init_structs(struct fel
 423         err = ocelot_regfields_init(ocelot, felix->info->regfields);
 424         if (err) {
 425                 dev_err(ocelot->dev, "failed to init reg fields map\n");
 426 +               kfree(port_phy_modes);
 427                 return err;
 428         }
 429
 430 @@ -291,26 +502,37 @@ static int felix_init_structs(struct fel
 431                 if (!ocelot_port) {
 432                         dev_err(ocelot->dev,
 433                                 "failed to allocate port memory\n");
 434 +                       kfree(port_phy_modes);
 435                         return -ENOMEM;
 436                 }
 437
 438                 res = &felix->info->port_io_res[port];
 439                 res->flags = IORESOURCE_MEM;
 440 -               res->start += base;
 441 -               res->end += base;
 442 +               res->start += switch_base;
 443 +               res->end += switch_base;
 444
 445                 port_regs = devm_ioremap_resource(ocelot->dev, res);
 446                 if (IS_ERR(port_regs)) {
 447                         dev_err(ocelot->dev,
 448                                 "failed to map registers for port %d\n", port);
 449 +                       kfree(port_phy_modes);
 450                         return PTR_ERR(port_regs);
 451                 }
 452
 453 +               ocelot_port->phy_mode = port_phy_modes[port];
 454                 ocelot_port->ocelot = ocelot;
 455                 ocelot_port->regs = port_regs;
 456                 ocelot->ports[port] = ocelot_port;
 457         }
 458
 459 +       kfree(port_phy_modes);
 460 +
 461 +       if (felix->info->mdio_bus_alloc) {
 462 +               err = felix->info->mdio_bus_alloc(ocelot);
 463 +               if (err < 0)
 464 +                       return err;
 465 +       }
 466 +
 467         return 0;
 468  }
 469
 470 @@ -340,12 +562,22 @@ static int felix_setup(struct dsa_switch
 471                                             OCELOT_TAG_PREFIX_LONG);
 472         }
 473
 474 +       /* It looks like the MAC/PCS interrupt register - PM0_IEVENT (0x8040)
 475 +        * isn't instantiated for the Felix PF.
 476 +        * In-band AN may take a few ms to complete, so we need to poll.
 477 +        */
 478 +       ds->pcs_poll = true;
 479 +
 480         return 0;
 481  }
 482
 483  static void felix_teardown(struct dsa_switch *ds)
 484  {
 485         struct ocelot *ocelot = ds->priv;
 486 +       struct felix *felix = ocelot_to_felix(ocelot);
 487 +
 488 +       if (felix->info->mdio_bus_free)
 489 +               felix->info->mdio_bus_free(ocelot);
 490
 491         /* stop workqueue thread */
 492         ocelot_deinit(ocelot);
 493 @@ -416,7 +648,12 @@ static const struct dsa_switch_ops felix
 494         .get_ethtool_stats      = felix_get_ethtool_stats,
 495         .get_sset_count         = felix_get_sset_count,
 496         .get_ts_info            = felix_get_ts_info,
 497 -       .adjust_link            = felix_adjust_link,
 498 +       .phylink_validate       = felix_phylink_validate,
 499 +       .phylink_mac_link_state = felix_phylink_mac_pcs_get_state,
 500 +       .phylink_mac_config     = felix_phylink_mac_config,
 501 +       .phylink_mac_an_restart = felix_phylink_mac_an_restart,
 502 +       .phylink_mac_link_down  = felix_phylink_mac_link_down,
 503 +       .phylink_mac_link_up    = felix_phylink_mac_link_up,
 504         .port_enable            = felix_port_enable,
 505         .port_disable           = felix_port_disable,
 506         .port_fdb_dump          = felix_fdb_dump,
 507 --- a/drivers/net/dsa/ocelot/felix.h
 508 +++ b/drivers/net/dsa/ocelot/felix.h
 509 @@ -10,6 +10,7 @@
 510  struct felix_info {
 511         struct resource                 *target_io_res;
 512         struct resource                 *port_io_res;
 513 +       struct resource                 *imdio_res;
 514         const struct reg_field          *regfields;
 515         const u32 *const                *map;
 516         const struct ocelot_ops         *ops;
 517 @@ -17,7 +18,18 @@ struct felix_info {
 518         const struct ocelot_stat_layout *stats_layout;
 519         unsigned int                    num_stats;
 520         int                             num_ports;
 521 -       int                             pci_bar;
 522 +       int                             switch_pci_bar;
 523 +       int                             imdio_pci_bar;
 524 +       int     (*mdio_bus_alloc)(struct ocelot *ocelot);
 525 +       void    (*mdio_bus_free)(struct ocelot *ocelot);
 526 +       void    (*pcs_init)(struct ocelot *ocelot, int port,
 527 +                           unsigned int link_an_mode,
 528 +                           const struct phylink_link_state *state);
 529 +       void    (*pcs_an_restart)(struct ocelot *ocelot, int port);
 530 +       void    (*pcs_link_state)(struct ocelot *ocelot, int port,
 531 +                                 struct phylink_link_state *state);
 532 +       int     (*prevalidate_phy_mode)(struct ocelot *ocelot, int port,
 533 +                                       phy_interface_t phy_mode);
 534  };
 535
 536  extern struct felix_info               felix_info_vsc9959;
 537 @@ -32,6 +44,8 @@ struct felix {
 538         struct pci_dev                  *pdev;
 539         struct felix_info               *info;
 540         struct ocelot                   ocelot;
 541 +       struct mii_bus                  *imdio;
 542 +       struct phy_device               **pcs;
 543  };
 544
 545  #endif
 546 --- a/drivers/net/dsa/ocelot/felix_vsc9959.c
 547 +++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
 548 @@ -2,12 +2,33 @@
 549  /* Copyright 2017 Microsemi Corporation
 550   * Copyright 2018-2019 NXP Semiconductors
 551   */
 552 +#include <linux/fsl/enetc_mdio.h>
 553  #include <soc/mscc/ocelot_sys.h>
 554  #include <soc/mscc/ocelot.h>
 555  #include <linux/iopoll.h>
 556  #include <linux/pci.h>
 557  #include "felix.h"
 558
 559 +/* TODO: should find a better place for these */
 560 +#define USXGMII_BMCR_RESET             BIT(15)
 561 +#define USXGMII_BMCR_AN_EN             BIT(12)
 562 +#define USXGMII_BMCR_RST_AN            BIT(9)
 563 +#define USXGMII_BMSR_LNKS(status)      (((status) & GENMASK(2, 2)) >> 2)
 564 +#define USXGMII_BMSR_AN_CMPL(status)   (((status) & GENMASK(5, 5)) >> 5)
 565 +#define USXGMII_ADVERTISE_LNKS(x)      (((x) << 15) & BIT(15))
 566 +#define USXGMII_ADVERTISE_FDX          BIT(12)
 567 +#define USXGMII_ADVERTISE_SPEED(x)     (((x) << 9) & GENMASK(11, 9))
 568 +#define USXGMII_LPA_LNKS(lpa)          ((lpa) >> 15)
 569 +#define USXGMII_LPA_DUPLEX(lpa)                (((lpa) & GENMASK(12, 12)) >> 12)
 570 +#define USXGMII_LPA_SPEED(lpa)         (((lpa) & GENMASK(11, 9)) >> 9)
 571 +
 572 +enum usxgmii_speed {
 573 +       USXGMII_SPEED_10        = 0,
 574 +       USXGMII_SPEED_100       = 1,
 575 +       USXGMII_SPEED_1000      = 2,
 576 +       USXGMII_SPEED_2500      = 4,
 577 +};
 578 +
 579  static const u32 vsc9959_ana_regmap[] = {
 580         REG(ANA_ADVLEARN,                       0x0089a0),
 581         REG(ANA_VLANMASK,                       0x0089a4),
 582 @@ -390,6 +411,15 @@ static struct resource vsc9959_port_io_r
 583         },
 584  };
 585
 586 +/* Port MAC 0 Internal MDIO bus through which the SerDes acting as an
 587 + * SGMII/QSGMII MAC PCS can be found.
 588 + */
 589 +static struct resource vsc9959_imdio_res = {
 590 +       .start          = 0x8030,
 591 +       .end            = 0x8040,
 592 +       .name           = "imdio",
 593 +};
 594 +
 595  static const struct reg_field vsc9959_regfields[] = {
 596         [ANA_ADVLEARN_VLAN_CHK] = REG_FIELD(ANA_ADVLEARN, 6, 6),
 597         [ANA_ADVLEARN_LEARN_MIRROR] = REG_FIELD(ANA_ADVLEARN, 0, 5),
 598 @@ -569,13 +599,475 @@ static int vsc9959_reset(struct ocelot *
 599         return 0;
 600  }
 601
 602 +static void vsc9959_pcs_an_restart_sgmii(struct phy_device *pcs)
 603 +{
 604 +       phy_set_bits(pcs, MII_BMCR, BMCR_ANRESTART);
 605 +}
 606 +
 607 +static void vsc9959_pcs_an_restart_usxgmii(struct phy_device *pcs)
 608 +{
 609 +       phy_write_mmd(pcs, MDIO_MMD_VEND2, MII_BMCR,
 610 +                     USXGMII_BMCR_RESET |
 611 +                     USXGMII_BMCR_AN_EN |
 612 +                     USXGMII_BMCR_RST_AN);
 613 +}
 614 +
 615 +static void vsc9959_pcs_an_restart(struct ocelot *ocelot, int port)
 616 +{
 617 +       struct felix *felix = ocelot_to_felix(ocelot);
 618 +       struct phy_device *pcs = felix->pcs[port];
 619 +
 620 +       if (!pcs)
 621 +               return;
 622 +
 623 +       switch (pcs->interface) {
 624 +       case PHY_INTERFACE_MODE_SGMII:
 625 +       case PHY_INTERFACE_MODE_QSGMII:
 626 +               vsc9959_pcs_an_restart_sgmii(pcs);
 627 +               break;
 628 +       case PHY_INTERFACE_MODE_USXGMII:
 629 +               vsc9959_pcs_an_restart_usxgmii(pcs);
 630 +               break;
 631 +       default:
 632 +               dev_err(ocelot->dev, "Invalid PCS interface type %s\n",
 633 +                       phy_modes(pcs->interface));
 634 +               break;
 635 +       }
 636 +}
 637 +
 638 +/* We enable SGMII AN only when the PHY has managed = "in-band-status" in the
 639 + * device tree. If we are in MLO_AN_PHY mode, we program directly state->speed
 640 + * into the PCS, which is retrieved out-of-band over MDIO. This also has the
 641 + * benefit of working with SGMII fixed-links, like downstream switches, where
 642 + * both link partners attempt to operate as AN slaves and therefore AN never
 643 + * completes.  But it also has the disadvantage that some PHY chips don't pass
 644 + * traffic if SGMII AN is enabled but not completed (acknowledged by us), so
 645 + * setting MLO_AN_INBAND is actually required for those.
 646 + */
 647 +static void vsc9959_pcs_init_sgmii(struct phy_device *pcs,
 648 +                                  unsigned int link_an_mode,
 649 +                                  const struct phylink_link_state *state)
 650 +{
 651 +       if (link_an_mode == MLO_AN_INBAND) {
 652 +               /* SGMII spec requires tx_config_Reg[15:0] to be exactly 0x4001
 653 +                * for the MAC PCS in order to acknowledge the AN.
 654 +                */
 655 +               phy_write(pcs, MII_ADVERTISE, ADVERTISE_SGMII |
 656 +                                             ADVERTISE_LPACK);
 657 +
 658 +               phy_write(pcs, ENETC_PCS_IF_MODE,
 659 +                         ENETC_PCS_IF_MODE_SGMII_EN |
 660 +                         ENETC_PCS_IF_MODE_USE_SGMII_AN);
 661 +
 662 +               /* Adjust link timer for SGMII */
 663 +               phy_write(pcs, ENETC_PCS_LINK_TIMER1,
 664 +                         ENETC_PCS_LINK_TIMER1_VAL);
 665 +               phy_write(pcs, ENETC_PCS_LINK_TIMER2,
 666 +                         ENETC_PCS_LINK_TIMER2_VAL);
 667 +
 668 +               phy_write(pcs, MII_BMCR, BMCR_ANRESTART | BMCR_ANENABLE);
 669 +       } else {
 670 +               int speed;
 671 +
 672 +               if (state->duplex == DUPLEX_HALF) {
 673 +                       phydev_err(pcs, "Half duplex not supported\n");
 674 +                       return;
 675 +               }
 676 +               switch (state->speed) {
 677 +               case SPEED_1000:
 678 +                       speed = ENETC_PCS_SPEED_1000;
 679 +                       break;
 680 +               case SPEED_100:
 681 +                       speed = ENETC_PCS_SPEED_100;
 682 +                       break;
 683 +               case SPEED_10:
 684 +                       speed = ENETC_PCS_SPEED_10;
 685 +                       break;
 686 +               case SPEED_UNKNOWN:
 687 +                       /* Silently don't do anything */
 688 +                       return;
 689 +               default:
 690 +                       phydev_err(pcs, "Invalid PCS speed %d\n", state->speed);
 691 +                       return;
 692 +               }
 693 +
 694 +               phy_write(pcs, ENETC_PCS_IF_MODE,
 695 +                         ENETC_PCS_IF_MODE_SGMII_EN |
 696 +                         ENETC_PCS_IF_MODE_SGMII_SPEED(speed));
 697 +
 698 +               /* Yes, not a mistake: speed is given by IF_MODE. */
 699 +               phy_write(pcs, MII_BMCR, BMCR_RESET |
 700 +                                        BMCR_SPEED1000 |
 701 +                                        BMCR_FULLDPLX);
 702 +       }
 703 +}
 704 +
 705 +/* 2500Base-X is SerDes protocol 7 on Felix and 6 on ENETC. It is a SerDes lane
 706 + * clocked at 3.125 GHz which encodes symbols with 8b/10b and does not have
 707 + * auto-negotiation of any link parameters. Electrically it is compatible with
 708 + * a single lane of XAUI.
 709 + * The hardware reference manual wants to call this mode SGMII, but it isn't
 710 + * really, since the fundamental features of SGMII:
 711 + * - Downgrading the link speed by duplicating symbols
 712 + * - Auto-negotiation
 713 + * are not there.
 714 + * The speed is configured at 1000 in the IF_MODE and BMCR MDIO registers
 715 + * because the clock frequency is actually given by a PLL configured in the
 716 + * Reset Configuration Word (RCW).
 717 + * Since there is no difference between fixed speed SGMII w/o AN and 802.3z w/o
 718 + * AN, we call this PHY interface type 2500Base-X. In case a PHY negotiates a
 719 + * lower link speed on line side, the system-side interface remains fixed at
 720 + * 2500 Mbps and we do rate adaptation through pause frames.
 721 + */
 722 +static void vsc9959_pcs_init_2500basex(struct phy_device *pcs,
 723 +                                      unsigned int link_an_mode,
 724 +                                      const struct phylink_link_state *state)
 725 +{
 726 +       if (link_an_mode == MLO_AN_INBAND) {
 727 +               phydev_err(pcs, "AN not supported on 3.125GHz SerDes lane\n");
 728 +               return;
 729 +       }
 730 +
 731 +       phy_write(pcs, ENETC_PCS_IF_MODE,
 732 +                 ENETC_PCS_IF_MODE_SGMII_EN |
 733 +                 ENETC_PCS_IF_MODE_SGMII_SPEED(ENETC_PCS_SPEED_2500));
 734 +
 735 +       phy_write(pcs, MII_BMCR, BMCR_SPEED1000 |
 736 +                                BMCR_FULLDPLX |
 737 +                                BMCR_RESET);
 738 +}
 739 +
 740 +static void vsc9959_pcs_init_usxgmii(struct phy_device *pcs,
 741 +                                    unsigned int link_an_mode,
 742 +                                    const struct phylink_link_state *state)
 743 +{
 744 +       if (link_an_mode != MLO_AN_INBAND) {
 745 +               phydev_err(pcs, "USXGMII only supports in-band AN for now\n");
 746 +               return;
 747 +       }
 748 +
 749 +       /* Configure device ability for the USXGMII Replicator */
 750 +       phy_write_mmd(pcs, MDIO_MMD_VEND2, MII_ADVERTISE,
 751 +                     USXGMII_ADVERTISE_SPEED(USXGMII_SPEED_2500) |
 752 +                     USXGMII_ADVERTISE_LNKS(1) |
 753 +                     ADVERTISE_SGMII |
 754 +                     ADVERTISE_LPACK |
 755 +                     USXGMII_ADVERTISE_FDX);
 756 +}
 757 +
 758 +static void vsc9959_pcs_init(struct ocelot *ocelot, int port,
 759 +                            unsigned int link_an_mode,
 760 +                            const struct phylink_link_state *state)
 761 +{
 762 +       struct felix *felix = ocelot_to_felix(ocelot);
 763 +       struct phy_device *pcs = felix->pcs[port];
 764 +
 765 +       if (!pcs)
 766 +               return;
 767 +
 768 +       /* The PCS does not implement the BMSR register fully, so capability
 769 +        * detection via genphy_read_abilities does not work. Since we can get
 770 +        * the PHY config word from the LPA register though, there is still
 771 +        * value in using the generic phy_resolve_aneg_linkmode function. So
 772 +        * populate the supported and advertising link modes manually here.
 773 +        */
 774 +       linkmode_set_bit_array(phy_basic_ports_array,
 775 +                              ARRAY_SIZE(phy_basic_ports_array),
 776 +                              pcs->supported);
 777 +       linkmode_set_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT, pcs->supported);
 778 +       linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT, pcs->supported);
 779 +       linkmode_set_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT, pcs->supported);
 780 +       if (pcs->interface == PHY_INTERFACE_MODE_2500BASEX ||
 781 +           pcs->interface == PHY_INTERFACE_MODE_USXGMII)
 782 +               linkmode_set_bit(ETHTOOL_LINK_MODE_2500baseX_Full_BIT,
 783 +                                pcs->supported);
 784 +       if (pcs->interface != PHY_INTERFACE_MODE_2500BASEX)
 785 +               linkmode_set_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
 786 +                                pcs->supported);
 787 +       phy_advertise_supported(pcs);
 788 +
 789 +       switch (pcs->interface) {
 790 +       case PHY_INTERFACE_MODE_SGMII:
 791 +       case PHY_INTERFACE_MODE_QSGMII:
 792 +               vsc9959_pcs_init_sgmii(pcs, link_an_mode, state);
 793 +               break;
 794 +       case PHY_INTERFACE_MODE_2500BASEX:
 795 +               vsc9959_pcs_init_2500basex(pcs, link_an_mode, state);
 796 +               break;
 797 +       case PHY_INTERFACE_MODE_USXGMII:
 798 +               vsc9959_pcs_init_usxgmii(pcs, link_an_mode, state);
 799 +               break;
 800 +       default:
 801 +               dev_err(ocelot->dev, "Unsupported link mode %s\n",
 802 +                       phy_modes(pcs->interface));
 803 +       }
 804 +}
 805 +
 806 +static void vsc9959_pcs_link_state_resolve(struct phy_device *pcs,
 807 +                                          struct phylink_link_state *state)
 808 +{
 809 +       state->an_complete = pcs->autoneg_complete;
 810 +       state->an_enabled = pcs->autoneg;
 811 +       state->link = pcs->link;
 812 +       state->duplex = pcs->duplex;
 813 +       state->speed = pcs->speed;
 814 +       /* SGMII AN does not negotiate flow control, but that's ok,
 815 +        * since phylink already knows that, and does:
 816 +        *      link_state.pause |= pl->phy_state.pause;
 817 +        */
 818 +       state->pause = MLO_PAUSE_NONE;
 819 +
 820 +       phydev_dbg(pcs,
 821 +                  "mode=%s/%s/%s adv=%*pb lpa=%*pb link=%u an_enabled=%u an_complete=%u\n",
 822 +                  phy_modes(pcs->interface),
 823 +                  phy_speed_to_str(pcs->speed),
 824 +                  phy_duplex_to_str(pcs->duplex),
 825 +                  __ETHTOOL_LINK_MODE_MASK_NBITS, pcs->advertising,
 826 +                  __ETHTOOL_LINK_MODE_MASK_NBITS, pcs->lp_advertising,
 827 +                  pcs->link, pcs->autoneg, pcs->autoneg_complete);
 828 +}
 829 +
 830 +static void vsc9959_pcs_link_state_sgmii(struct phy_device *pcs,
 831 +                                        struct phylink_link_state *state)
 832 +{
 833 +       int err;
 834 +
 835 +       err = genphy_update_link(pcs);
 836 +       if (err < 0)
 837 +               return;
 838 +
 839 +       if (pcs->autoneg_complete) {
 840 +               u16 lpa = phy_read(pcs, MII_LPA);
 841 +
 842 +               mii_lpa_to_linkmode_lpa_sgmii(pcs->lp_advertising, lpa);
 843 +
 844 +               phy_resolve_aneg_linkmode(pcs);
 845 +       }
 846 +}
 847 +
 848 +static void vsc9959_pcs_link_state_2500basex(struct phy_device *pcs,
 849 +                                            struct phylink_link_state *state)
 850 +{
 851 +       int err;
 852 +
 853 +       err = genphy_update_link(pcs);
 854 +       if (err < 0)
 855 +               return;
 856 +
 857 +       pcs->speed = SPEED_2500;
 858 +       pcs->asym_pause = true;
 859 +       pcs->pause = true;
 860 +}
 861 +
 862 +static void vsc9959_pcs_link_state_usxgmii(struct phy_device *pcs,
 863 +                                          struct phylink_link_state *state)
 864 +{
 865 +       int status, lpa;
 866 +
 867 +       status = phy_read_mmd(pcs, MDIO_MMD_VEND2, MII_BMSR);
 868 +       if (status < 0)
 869 +               return;
 870 +
 871 +       pcs->autoneg = true;
 872 +       pcs->autoneg_complete = USXGMII_BMSR_AN_CMPL(status);
 873 +       pcs->link = USXGMII_BMSR_LNKS(status);
 874 +
 875 +       if (!pcs->link || !pcs->autoneg_complete)
 876 +               return;
 877 +
 878 +       lpa = phy_read_mmd(pcs, MDIO_MMD_VEND2, MII_LPA);
 879 +       if (lpa < 0)
 880 +               return;
 881 +
 882 +       switch (USXGMII_LPA_SPEED(lpa)) {
 883 +       case USXGMII_SPEED_10:
 884 +               pcs->speed = SPEED_10;
 885 +               break;
 886 +       case USXGMII_SPEED_100:
 887 +               pcs->speed = SPEED_100;
 888 +               break;
 889 +       case USXGMII_SPEED_1000:
 890 +               pcs->speed = SPEED_1000;
 891 +               break;
 892 +       case USXGMII_SPEED_2500:
 893 +               pcs->speed = SPEED_2500;
 894 +               break;
 895 +       default:
 896 +               break;
 897 +       }
 898 +
 899 +       pcs->link = USXGMII_LPA_LNKS(lpa);
 900 +       if (USXGMII_LPA_DUPLEX(lpa))
 901 +               pcs->duplex = DUPLEX_FULL;
 902 +       else
 903 +               pcs->duplex = DUPLEX_HALF;
 904 +}
 905 +
 906 +static void vsc9959_pcs_link_state(struct ocelot *ocelot, int port,
 907 +                                  struct phylink_link_state *state)
 908 +{
 909 +       struct felix *felix = ocelot_to_felix(ocelot);
 910 +       struct phy_device *pcs = felix->pcs[port];
 911 +
 912 +       if (!pcs)
 913 +               return;
 914 +
 915 +       pcs->speed = SPEED_UNKNOWN;
 916 +       pcs->duplex = DUPLEX_UNKNOWN;
 917 +       pcs->pause = 0;
 918 +       pcs->asym_pause = 0;
 919 +
 920 +       switch (pcs->interface) {
 921 +       case PHY_INTERFACE_MODE_SGMII:
 922 +       case PHY_INTERFACE_MODE_QSGMII:
 923 +               vsc9959_pcs_link_state_sgmii(pcs, state);
 924 +               break;
 925 +       case PHY_INTERFACE_MODE_2500BASEX:
 926 +               vsc9959_pcs_link_state_2500basex(pcs, state);
 927 +               break;
 928 +       case PHY_INTERFACE_MODE_USXGMII:
 929 +               vsc9959_pcs_link_state_usxgmii(pcs, state);
 930 +               break;
 931 +       default:
 932 +               return;
 933 +       }
 934 +
 935 +       vsc9959_pcs_link_state_resolve(pcs, state);
 936 +}
 937 +
 938 +static int vsc9959_prevalidate_phy_mode(struct ocelot *ocelot, int port,
 939 +                                       phy_interface_t phy_mode)
 940 +{
 941 +       switch (phy_mode) {
 942 +       case PHY_INTERFACE_MODE_GMII:
 943 +               /* Only supported on internal to-CPU ports */
 944 +               if (port != 4 && port != 5)
 945 +                       return -ENOTSUPP;
 946 +               return 0;
 947 +       case PHY_INTERFACE_MODE_SGMII:
 948 +       case PHY_INTERFACE_MODE_QSGMII:
 949 +       case PHY_INTERFACE_MODE_USXGMII:
 950 +       case PHY_INTERFACE_MODE_2500BASEX:
 951 +               /* Not supported on internal to-CPU ports */
 952 +               if (port == 4 || port == 5)
 953 +                       return -ENOTSUPP;
 954 +               return 0;
 955 +       default:
 956 +               return -ENOTSUPP;
 957 +       }
 958 +}
 959 +
 960  static const struct ocelot_ops vsc9959_ops = {
 961         .reset                  = vsc9959_reset,
 962  };
 963
 964 +static int vsc9959_mdio_bus_alloc(struct ocelot *ocelot)
 965 +{
 966 +       struct felix *felix = ocelot_to_felix(ocelot);
 967 +       struct enetc_mdio_priv *mdio_priv;
 968 +       struct device *dev = ocelot->dev;
 969 +       resource_size_t imdio_base;
 970 +       void __iomem *imdio_regs;
 971 +       struct resource *res;
 972 +       struct enetc_hw *hw;
 973 +       struct mii_bus *bus;
 974 +       int port;
 975 +       int rc;
 976 +
 977 +       felix->pcs = devm_kcalloc(dev, felix->info->num_ports,
 978 +                                 sizeof(struct phy_device *),
 979 +                                 GFP_KERNEL);
 980 +       if (!felix->pcs) {
 981 +               dev_err(dev, "failed to allocate array for PCS PHYs\n");
 982 +               return -ENOMEM;
 983 +       }
 984 +
 985 +       imdio_base = pci_resource_start(felix->pdev,
 986 +                                       felix->info->imdio_pci_bar);
 987 +
 988 +       res = felix->info->imdio_res;
 989 +       res->flags = IORESOURCE_MEM;
 990 +       res->start += imdio_base;
 991 +       res->end += imdio_base;
 992 +
 993 +       imdio_regs = devm_ioremap_resource(dev, res);
 994 +       if (IS_ERR(imdio_regs)) {
 995 +               dev_err(dev, "failed to map internal MDIO registers\n");
 996 +               return PTR_ERR(imdio_regs);
 997 +       }
 998 +
 999 +       hw = enetc_hw_alloc(dev, imdio_regs);
1000 +       if (IS_ERR(hw)) {
1001 +               dev_err(dev, "failed to allocate ENETC HW structure\n");
1002 +               return PTR_ERR(hw);
1003 +       }
1004 +
1005 +       bus = devm_mdiobus_alloc_size(dev, sizeof(*mdio_priv));
1006 +       if (!bus)
1007 +               return -ENOMEM;
1008 +
1009 +       bus->name = "VSC9959 internal MDIO bus";
1010 +       bus->read = enetc_mdio_read;
1011 +       bus->write = enetc_mdio_write;
1012 +       bus->parent = dev;
1013 +       mdio_priv = bus->priv;
1014 +       mdio_priv->hw = hw;
1015 +       /* This gets added to imdio_regs, which already maps addresses
1016 +        * starting with the proper offset.
1017 +        */
1018 +       mdio_priv->mdio_base = 0;
1019 +       snprintf(bus->id, MII_BUS_ID_SIZE, "%s-imdio", dev_name(dev));
1020 +
1021 +       /* Needed in order to initialize the bus mutex lock */
1022 +       rc = mdiobus_register(bus);
1023 +       if (rc < 0) {
1024 +               dev_err(dev, "failed to register MDIO bus\n");
1025 +               return rc;
1026 +       }
1027 +
1028 +       felix->imdio = bus;
1029 +
1030 +       for (port = 0; port < felix->info->num_ports; port++) {
1031 +               struct ocelot_port *ocelot_port = ocelot->ports[port];
1032 +               struct phy_device *pcs;
1033 +               bool is_c45 = false;
1034 +
1035 +               if (ocelot_port->phy_mode == PHY_INTERFACE_MODE_USXGMII)
1036 +                       is_c45 = true;
1037 +
1038 +               pcs = get_phy_device(felix->imdio, port, is_c45);
1039 +               if (IS_ERR(pcs))
1040 +                       continue;
1041 +
1042 +               pcs->interface = ocelot_port->phy_mode;
1043 +               felix->pcs[port] = pcs;
1044 +
1045 +               dev_info(dev, "Found PCS at internal MDIO address %d\n", port);
1046 +       }
1047 +
1048 +       return 0;
1049 +}
1050 +
1051 +static void vsc9959_mdio_bus_free(struct ocelot *ocelot)
1052 +{
1053 +       struct felix *felix = ocelot_to_felix(ocelot);
1054 +       int port;
1055 +
1056 +       for (port = 0; port < ocelot->num_phys_ports; port++) {
1057 +               struct phy_device *pcs = felix->pcs[port];
1058 +
1059 +               if (!pcs)
1060 +                       continue;
1061 +
1062 +               put_device(&pcs->mdio.dev);
1063 +       }
1064 +       mdiobus_unregister(felix->imdio);
1065 +}
1066 +
1067  struct felix_info felix_info_vsc9959 = {
1068         .target_io_res          = vsc9959_target_io_res,
1069         .port_io_res            = vsc9959_port_io_res,
1070 +       .imdio_res              = &vsc9959_imdio_res,
1071         .regfields              = vsc9959_regfields,
1072         .map                    = vsc9959_regmap,
1073         .ops                    = &vsc9959_ops,
1074 @@ -583,5 +1075,12 @@ struct felix_info felix_info_vsc9959 = {
1075         .num_stats              = ARRAY_SIZE(vsc9959_stats_layout),
1076         .shared_queue_sz        = 128 * 1024,
1077         .num_ports              = 6,
1078 -       .pci_bar                = 4,
1079 +       .switch_pci_bar         = 4,
1080 +       .imdio_pci_bar          = 0,
1081 +       .mdio_bus_alloc         = vsc9959_mdio_bus_alloc,
1082 +       .mdio_bus_free          = vsc9959_mdio_bus_free,
1083 +       .pcs_init               = vsc9959_pcs_init,
1084 +       .pcs_an_restart         = vsc9959_pcs_an_restart,
1085 +       .pcs_link_state         = vsc9959_pcs_link_state,
1086 +       .prevalidate_phy_mode   = vsc9959_prevalidate_phy_mode,
1087  };