drivers/net/mscc_eswitch/servalt_switch.c

   1 // SPDX-License-Identifier: (GPL-2.0+ OR MIT)
   2 /*
   3  * Copyright (c) 2019 Microsemi Corporation
   4  */
   5
   6 #include <common.h>
   7 #include <config.h>
   8 #include <dm.h>
   9 #include <dm/of_access.h>
  10 #include <dm/of_addr.h>
  11 #include <fdt_support.h>
  12 #include <linux/io.h>
  13 #include <linux/ioport.h>
  14 #include <miiphy.h>
  15 #include <net.h>
  16 #include <wait_bit.h>
  17
  18 #include "mscc_xfer.h"
  19
  20 #define GCB_MIIM_MII_STATUS             0x0
  21 #define         GCB_MIIM_STAT_BUSY              BIT(3)
  22 #define GCB_MIIM_MII_CMD                0x8
  23 #define         GCB_MIIM_MII_CMD_OPR_WRITE      BIT(1)
  24 #define         GCB_MIIM_MII_CMD_OPR_READ       BIT(2)
  25 #define         GCB_MIIM_MII_CMD_WRDATA(x)      ((x) << 4)
  26 #define         GCB_MIIM_MII_CMD_REGAD(x)       ((x) << 20)
  27 #define         GCB_MIIM_MII_CMD_PHYAD(x)       ((x) << 25)
  28 #define         GCB_MIIM_MII_CMD_VLD            BIT(31)
  29 #define GCB_MIIM_DATA                   0xC
  30 #define         GCB_MIIM_DATA_ERROR             (0x3 << 16)
  31
  32 #define PHY_CFG                         0x0
  33 #define PHY_CFG_ENA                             0x3
  34 #define PHY_CFG_COMMON_RST                      BIT(2)
  35 #define PHY_CFG_RST                             (0x3 << 3)
  36 #define PHY_STAT                        0x4
  37 #define PHY_STAT_SUPERVISOR_COMPLETE            BIT(0)
  38
  39 #define ANA_AC_RAM_CTRL_RAM_INIT                0x14fdc
  40 #define ANA_AC_STAT_GLOBAL_CFG_PORT_RESET       0x15474
  41
  42 #define ANA_CL_PORT_VLAN_CFG(x)                 (0xa018 + 0xc8 * (x))
  43 #define         ANA_CL_PORT_VLAN_CFG_AWARE_ENA                  BIT(19)
  44 #define         ANA_CL_PORT_VLAN_CFG_POP_CNT(x)                 ((x) << 17)
  45
  46 #define ANA_L2_COMMON_FWD_CFG                   0x18498
  47 #define         ANA_L2_COMMON_FWD_CFG_CPU_DMAC_COPY_ENA BIT(6)
  48
  49 #define ASM_CFG_STAT_CFG                        0xb08
  50 #define ASM_CFG_PORT(x)                         (0xb74 + 0x4 * (x))
  51 #define         ASM_CFG_PORT_NO_PREAMBLE_ENA            BIT(8)
  52 #define         ASM_CFG_PORT_INJ_FORMAT_CFG(x)          ((x) << 1)
  53 #define ASM_RAM_CTRL_RAM_INIT                   0xbfc
  54
  55 #define DEV_DEV_CFG_DEV_RST_CTRL        0x0
  56 #define         DEV_DEV_CFG_DEV_RST_CTRL_SPEED_SEL(x)   ((x) << 20)
  57 #define DEV_MAC_CFG_MAC_ENA             0x24
  58 #define         DEV_MAC_CFG_MAC_ENA_RX_ENA              BIT(4)
  59 #define         DEV_MAC_CFG_MAC_ENA_TX_ENA              BIT(0)
  60 #define DEV_MAC_CFG_MAC_IFG             0x3c
  61 #define         DEV_MAC_CFG_MAC_IFG_TX_IFG(x)           ((x) << 8)
  62 #define         DEV_MAC_CFG_MAC_IFG_RX_IFG2(x)          ((x) << 4)
  63 #define         DEV_MAC_CFG_MAC_IFG_RX_IFG1(x)          (x)
  64 #define DEV_PCS1G_CFG_PCS1G_CFG         0x48
  65 #define         DEV_PCS1G_CFG_PCS1G_CFG_PCS_ENA         BIT(0)
  66 #define DEV_PCS1G_CFG_PCS1G_MODE        0x4c
  67 #define DEV_PCS1G_CFG_PCS1G_SD          0x50
  68 #define DEV_PCS1G_CFG_PCS1G_ANEG        0x54
  69 #define         DEV_PCS1G_CFG_PCS1G_ANEG_ADV_ABILITY(x) ((x) << 16)
  70
  71 #define LRN_COMMON_ACCESS_CTRL                  0x0
  72 #define         LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT    BIT(0)
  73 #define LRN_COMMON_MAC_ACCESS_CFG0              0x4
  74 #define LRN_COMMON_MAC_ACCESS_CFG1              0x8
  75 #define LRN_COMMON_MAC_ACCESS_CFG2              0xc
  76 #define         LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_ADDR(x)    (x)
  77 #define         LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_TYPE(x)    ((x) << 12)
  78 #define         LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_VLD        BIT(15)
  79 #define         LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_LOCKED     BIT(16)
  80 #define         LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_COPY   BIT(23)
  81 #define         LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_QU(x)  ((x) << 24)
  82
  83 #define QFWD_SYSTEM_SWITCH_PORT_MODE(x)         (0x4400 + 0x4 * (x))
  84 #define         QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA           BIT(17)
  85
  86 #define QS_XTR_GRP_CFG(x)               (4 * (x))
  87 #define QS_INJ_GRP_CFG(x)               (0x24 + (x) * 4)
  88
  89 #define QSYS_SYSTEM_RESET_CFG                   0x1048
  90 #define QSYS_CALCFG_CAL_AUTO                    0x1134
  91 #define QSYS_CALCFG_CAL_CTRL                    0x113c
  92 #define         QSYS_CALCFG_CAL_CTRL_CAL_MODE(x)                ((x) << 11)
  93 #define QSYS_RAM_CTRL_RAM_INIT                  0x1140
  94
  95 #define REW_RAM_CTRL_RAM_INIT                   0xFFF4
  96
  97 #define MAC_VID                 0
  98 #define CPU_PORT                11
  99 #define IFH_LEN                 7
 100 #define ETH_ALEN                6
 101 #define PGID_BROADCAST          50
 102 #define PGID_UNICAST            51
 103
 104 static const char * const regs_names[] = {
 105         "port0", "port1",
 106         "ana_ac", "ana_cl", "ana_l2", "asm", "lrn", "qfwd", "qs", "qsys", "rew",
 107 };
 108
 109 #define REGS_NAMES_COUNT ARRAY_SIZE(regs_names) + 1
 110 #define MAX_PORT 2
 111
 112 enum servalt_ctrl_regs {
 113         ANA_AC = MAX_PORT,
 114         ANA_CL,
 115         ANA_L2,
 116         ASM,
 117         LRN,
 118         QFWD,
 119         QS,
 120         QSYS,
 121         REW,
 122 };
 123
 124 #define SERVALT_MIIM_BUS_COUNT 2
 125
 126 struct servalt_phy_port_t {
 127         size_t phy_addr;
 128         struct mii_dev *bus;
 129 };
 130
 131 struct servalt_private {
 132         void __iomem *regs[REGS_NAMES_COUNT];
 133         struct mii_dev *bus[SERVALT_MIIM_BUS_COUNT];
 134         struct servalt_phy_port_t ports[MAX_PORT];
 135 };
 136
 137 struct mscc_miim_dev {
 138         void __iomem *regs;
 139         phys_addr_t miim_base;
 140         unsigned long miim_size;
 141         struct mii_dev *bus;
 142 };
 143
 144 static const unsigned long servalt_regs_qs[] = {
 145         [MSCC_QS_XTR_RD] = 0x8,
 146         [MSCC_QS_XTR_FLUSH] = 0x18,
 147         [MSCC_QS_XTR_DATA_PRESENT] = 0x1c,
 148         [MSCC_QS_INJ_WR] = 0x2c,
 149         [MSCC_QS_INJ_CTRL] = 0x34,
 150 };
 151
 152 static struct mscc_miim_dev miim[SERVALT_MIIM_BUS_COUNT];
 153 static int miim_count = -1;
 154
 155 static int mscc_miim_wait_ready(struct mscc_miim_dev *miim)
 156 {
 157         return wait_for_bit_le32(miim->regs + GCB_MIIM_MII_STATUS,
 158                                  GCB_MIIM_STAT_BUSY, false, 250, false);
 159 }
 160
 161 static int mscc_miim_read(struct mii_dev *bus, int addr, int devad, int reg)
 162 {
 163         struct mscc_miim_dev *miim = (struct mscc_miim_dev *)bus->priv;
 164         u32 val;
 165         int ret;
 166
 167         ret = mscc_miim_wait_ready(miim);
 168         if (ret)
 169                 goto out;
 170
 171         writel(GCB_MIIM_MII_CMD_VLD | GCB_MIIM_MII_CMD_PHYAD(addr) |
 172                GCB_MIIM_MII_CMD_REGAD(reg) | GCB_MIIM_MII_CMD_OPR_READ,
 173                miim->regs + GCB_MIIM_MII_CMD);
 174
 175         ret = mscc_miim_wait_ready(miim);
 176         if (ret)
 177                 goto out;
 178
 179         val = readl(miim->regs + GCB_MIIM_DATA);
 180         if (val & GCB_MIIM_DATA_ERROR) {
 181                 ret = -EIO;
 182                 goto out;
 183         }
 184
 185         ret = val & 0xFFFF;
 186 out:
 187         return ret;
 188 }
 189
 190 static int mscc_miim_write(struct mii_dev *bus, int addr, int devad, int reg,
 191                            u16 val)
 192 {
 193         struct mscc_miim_dev *miim = (struct mscc_miim_dev *)bus->priv;
 194         int ret;
 195
 196         ret = mscc_miim_wait_ready(miim);
 197         if (ret < 0)
 198                 goto out;
 199
 200         writel(GCB_MIIM_MII_CMD_VLD | GCB_MIIM_MII_CMD_PHYAD(addr) |
 201                GCB_MIIM_MII_CMD_REGAD(reg) | GCB_MIIM_MII_CMD_WRDATA(val) |
 202                GCB_MIIM_MII_CMD_OPR_WRITE, miim->regs + GCB_MIIM_MII_CMD);
 203
 204 out:
 205         return ret;
 206 }
 207
 208 static struct mii_dev *servalt_mdiobus_init(phys_addr_t miim_base,
 209                                             unsigned long miim_size)
 210 {
 211         struct mii_dev *bus;
 212
 213         bus = mdio_alloc();
 214         if (!bus)
 215                 return NULL;
 216
 217         ++miim_count;
 218         sprintf(bus->name, "miim-bus%d", miim_count);
 219
 220         miim[miim_count].regs = ioremap(miim_base, miim_size);
 221         miim[miim_count].miim_base = miim_base;
 222         miim[miim_count].miim_size = miim_size;
 223         bus->priv = &miim[miim_count];
 224         bus->read = mscc_miim_read;
 225         bus->write = mscc_miim_write;
 226
 227         if (mdio_register(bus))
 228                 return NULL;
 229
 230         miim[miim_count].bus = bus;
 231         return bus;
 232 }
 233
 234 static void mscc_phy_reset(void)
 235 {
 236         writel(0, BASE_DEVCPU_GCB + GCB_PHY_CFG + PHY_CFG);
 237         writel(PHY_CFG_RST | PHY_CFG_COMMON_RST
 238                | PHY_CFG_ENA, BASE_DEVCPU_GCB + GCB_PHY_CFG + PHY_CFG);
 239         if (wait_for_bit_le32((const void *)(BASE_DEVCPU_GCB + GCB_PHY_CFG) +
 240                               PHY_STAT, PHY_STAT_SUPERVISOR_COMPLETE,
 241                               true, 2000, false)) {
 242                 pr_err("Timeout in phy reset\n");
 243         }
 244 }
 245
 246 static void servalt_cpu_capture_setup(struct servalt_private *priv)
 247 {
 248         /* ASM: No preamble and IFH prefix on CPU injected frames */
 249         writel(ASM_CFG_PORT_NO_PREAMBLE_ENA |
 250                ASM_CFG_PORT_INJ_FORMAT_CFG(1),
 251                priv->regs[ASM] + ASM_CFG_PORT(CPU_PORT));
 252
 253         /* Set Manual injection via DEVCPU_QS registers for CPU queue 0 */
 254         writel(0x5, priv->regs[QS] + QS_INJ_GRP_CFG(0));
 255
 256         /* Set Manual extraction via DEVCPU_QS registers for CPU queue 0 */
 257         writel(0x7, priv->regs[QS] + QS_XTR_GRP_CFG(0));
 258
 259         /* Enable CPU port for any frame transfer */
 260         setbits_le32(priv->regs[QFWD] + QFWD_SYSTEM_SWITCH_PORT_MODE(CPU_PORT),
 261                      QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA);
 262
 263         /* Send a copy to CPU when found as forwarding entry */
 264         setbits_le32(priv->regs[ANA_L2] + ANA_L2_COMMON_FWD_CFG,
 265                      ANA_L2_COMMON_FWD_CFG_CPU_DMAC_COPY_ENA);
 266 }
 267
 268 static void servalt_port_init(struct servalt_private *priv, int port)
 269 {
 270         void __iomem *regs = priv->regs[port];
 271
 272         /* Enable PCS */
 273         writel(DEV_PCS1G_CFG_PCS1G_CFG_PCS_ENA,
 274                regs + DEV_PCS1G_CFG_PCS1G_CFG);
 275
 276         /* Disable Signal Detect */
 277         writel(0, regs + DEV_PCS1G_CFG_PCS1G_SD);
 278
 279         /* Enable MAC RX and TX */
 280         writel(DEV_MAC_CFG_MAC_ENA_RX_ENA |
 281                DEV_MAC_CFG_MAC_ENA_TX_ENA,
 282                regs + DEV_MAC_CFG_MAC_ENA);
 283
 284         /* Clear sgmii_mode_ena */
 285         writel(0, regs + DEV_PCS1G_CFG_PCS1G_MODE);
 286
 287         /*
 288          * Clear sw_resolve_ena(bit 0) and set adv_ability to
 289          * something meaningful just in case
 290          */
 291         writel(DEV_PCS1G_CFG_PCS1G_ANEG_ADV_ABILITY(0x20),
 292                regs + DEV_PCS1G_CFG_PCS1G_ANEG);
 293
 294         /* Set MAC IFG Gaps */
 295         writel(DEV_MAC_CFG_MAC_IFG_TX_IFG(4) |
 296                DEV_MAC_CFG_MAC_IFG_RX_IFG1(5) |
 297                DEV_MAC_CFG_MAC_IFG_RX_IFG2(1),
 298                regs + DEV_MAC_CFG_MAC_IFG);
 299
 300         /* Set link speed and release all resets */
 301         writel(DEV_DEV_CFG_DEV_RST_CTRL_SPEED_SEL(2),
 302                regs + DEV_DEV_CFG_DEV_RST_CTRL);
 303
 304         /* Make VLAN aware for CPU traffic */
 305         writel(ANA_CL_PORT_VLAN_CFG_AWARE_ENA |
 306                ANA_CL_PORT_VLAN_CFG_POP_CNT(1) |
 307                MAC_VID,
 308                priv->regs[ANA_CL] + ANA_CL_PORT_VLAN_CFG(port));
 309
 310         /* Enable CPU port for any frame transfer */
 311         setbits_le32(priv->regs[QFWD] + QFWD_SYSTEM_SWITCH_PORT_MODE(port),
 312                      QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA);
 313 }
 314
 315 static int ram_init(u32 val, void __iomem *addr)
 316 {
 317         writel(val, addr);
 318
 319         if (wait_for_bit_le32(addr, BIT(1), false, 2000, false)) {
 320                 printf("Timeout in memory reset, reg = 0x%08x\n", val);
 321                 return 1;
 322         }
 323
 324         return 0;
 325 }
 326
 327 static int servalt_switch_init(struct servalt_private *priv)
 328 {
 329         /* Initialize memories */
 330         ram_init(0x3, priv->regs[QSYS] + QSYS_RAM_CTRL_RAM_INIT);
 331         ram_init(0x3, priv->regs[ASM] + ASM_RAM_CTRL_RAM_INIT);
 332         ram_init(0x3, priv->regs[ANA_AC] + ANA_AC_RAM_CTRL_RAM_INIT);
 333         ram_init(0x3, priv->regs[REW] + REW_RAM_CTRL_RAM_INIT);
 334
 335         /* Reset counters */
 336         writel(0x1, priv->regs[ANA_AC] + ANA_AC_STAT_GLOBAL_CFG_PORT_RESET);
 337         writel(0x1, priv->regs[ASM] + ASM_CFG_STAT_CFG);
 338
 339         /* Enable switch-core and queue system */
 340         writel(0x1, priv->regs[QSYS] + QSYS_SYSTEM_RESET_CFG);
 341
 342         return 0;
 343 }
 344
 345 static void servalt_switch_config(struct servalt_private *priv)
 346 {
 347         writel(0x55555555, priv->regs[QSYS] + QSYS_CALCFG_CAL_AUTO);
 348
 349         writel(readl(priv->regs[QSYS] + QSYS_CALCFG_CAL_CTRL) |
 350                QSYS_CALCFG_CAL_CTRL_CAL_MODE(8),
 351                priv->regs[QSYS] + QSYS_CALCFG_CAL_CTRL);
 352 }
 353
 354 static int servalt_initialize(struct servalt_private *priv)
 355 {
 356         int ret, i;
 357
 358         /* Initialize switch memories, enable core */
 359         ret = servalt_switch_init(priv);
 360         if (ret)
 361                 return ret;
 362
 363         servalt_switch_config(priv);
 364
 365         for (i = 0; i < MAX_PORT; i++)
 366                 servalt_port_init(priv, i);
 367
 368         servalt_cpu_capture_setup(priv);
 369
 370         return 0;
 371 }
 372
 373 static inline
 374 int servalt_vlant_wait_for_completion(struct servalt_private *priv)
 375 {
 376         if (wait_for_bit_le32(priv->regs[LRN] + LRN_COMMON_ACCESS_CTRL,
 377                               LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT,
 378                               false, 2000, false))
 379                 return -ETIMEDOUT;
 380
 381         return 0;
 382 }
 383
 384 static int servalt_mac_table_add(struct servalt_private *priv,
 385                                  const unsigned char mac[ETH_ALEN], int pgid)
 386 {
 387         u32 macl = 0, mach = 0;
 388
 389         /*
 390          * Set the MAC address to handle and the vlan associated in a format
 391          * understood by the hardware.
 392          */
 393         mach |= MAC_VID << 16;
 394         mach |= ((u32)mac[0]) << 8;
 395         mach |= ((u32)mac[1]) << 0;
 396         macl |= ((u32)mac[2]) << 24;
 397         macl |= ((u32)mac[3]) << 16;
 398         macl |= ((u32)mac[4]) << 8;
 399         macl |= ((u32)mac[5]) << 0;
 400
 401         writel(mach, priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG0);
 402         writel(macl, priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG1);
 403
 404         writel(LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_ADDR(pgid) |
 405                LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_TYPE(0x3) |
 406                LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_COPY |
 407                LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_QU(0) |
 408                LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_VLD |
 409                LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_LOCKED,
 410                priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG2);
 411
 412         writel(LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT,
 413                priv->regs[LRN] + LRN_COMMON_ACCESS_CTRL);
 414
 415         return servalt_vlant_wait_for_completion(priv);
 416 }
 417
 418 static int servalt_write_hwaddr(struct udevice *dev)
 419 {
 420         struct servalt_private *priv = dev_get_priv(dev);
 421         struct eth_pdata *pdata = dev_get_platdata(dev);
 422
 423         return servalt_mac_table_add(priv, pdata->enetaddr, PGID_UNICAST);
 424 }
 425
 426 static int servalt_start(struct udevice *dev)
 427 {
 428         struct servalt_private *priv = dev_get_priv(dev);
 429         struct eth_pdata *pdata = dev_get_platdata(dev);
 430         const unsigned char mac[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff,
 431                 0xff };
 432         int ret;
 433
 434         ret = servalt_initialize(priv);
 435         if (ret)
 436                 return ret;
 437
 438         /* Set MAC address tables entries for CPU redirection */
 439         ret = servalt_mac_table_add(priv, mac, PGID_BROADCAST);
 440         if (ret)
 441                 return ret;
 442
 443         ret = servalt_mac_table_add(priv, pdata->enetaddr, PGID_UNICAST);
 444         if (ret)
 445                 return ret;
 446
 447         return 0;
 448 }
 449
 450 static void servalt_stop(struct udevice *dev)
 451 {
 452 }
 453
 454 static int servalt_send(struct udevice *dev, void *packet, int length)
 455 {
 456         struct servalt_private *priv = dev_get_priv(dev);
 457         u32 ifh[IFH_LEN];
 458         u32 *buf = packet;
 459
 460         memset(ifh, '\0', IFH_LEN * 4);
 461
 462         /* Set DST PORT_MASK */
 463         ifh[0] = htonl(0);
 464         ifh[1] = htonl(0x1FFFFF);
 465         ifh[2] = htonl(~0);
 466         /* Set DST_MODE to INJECT and UPDATE_FCS */
 467         ifh[5] = htonl(0x4c0);
 468
 469         return mscc_send(priv->regs[QS], servalt_regs_qs,
 470                          ifh, IFH_LEN, buf, length);
 471 }
 472
 473 static int servalt_recv(struct udevice *dev, int flags, uchar **packetp)
 474 {
 475         struct servalt_private *priv = dev_get_priv(dev);
 476         u32 *rxbuf = (u32 *)net_rx_packets[0];
 477         int byte_cnt = 0;
 478
 479         byte_cnt = mscc_recv(priv->regs[QS], servalt_regs_qs, rxbuf, IFH_LEN,
 480                              false);
 481
 482         *packetp = net_rx_packets[0];
 483
 484         return byte_cnt;
 485 }
 486
 487 static struct mii_dev *get_mdiobus(phys_addr_t base, unsigned long size)
 488 {
 489         int i = 0;
 490
 491         for (i = 0; i < SERVALT_MIIM_BUS_COUNT; ++i)
 492                 if (miim[i].miim_base == base && miim[i].miim_size == size)
 493                         return miim[i].bus;
 494
 495         return NULL;
 496 }
 497
 498 static void add_port_entry(struct servalt_private *priv, size_t index,
 499                            size_t phy_addr, struct mii_dev *bus)
 500 {
 501         priv->ports[index].phy_addr = phy_addr;
 502         priv->ports[index].bus = bus;
 503 }
 504
 505 static int servalt_probe(struct udevice *dev)
 506 {
 507         struct servalt_private *priv = dev_get_priv(dev);
 508         int i;
 509         struct resource res;
 510         fdt32_t faddr;
 511         phys_addr_t addr_base;
 512         unsigned long addr_size;
 513         ofnode eth_node, node, mdio_node;
 514         size_t phy_addr;
 515         struct mii_dev *bus;
 516         struct ofnode_phandle_args phandle;
 517
 518         if (!priv)
 519                 return -EINVAL;
 520
 521         /* Get registers and map them to the private structure */
 522         for (i = 0; i < ARRAY_SIZE(regs_names); i++) {
 523                 priv->regs[i] = dev_remap_addr_name(dev, regs_names[i]);
 524                 if (!priv->regs[i]) {
 525                         debug
 526                             ("Error can't get regs base addresses for %s\n",
 527                              regs_names[i]);
 528                         return -ENOMEM;
 529                 }
 530         }
 531
 532         /* Initialize miim buses */
 533         memset(&miim, 0x0, sizeof(struct mscc_miim_dev) *
 534                SERVALT_MIIM_BUS_COUNT);
 535
 536         /* iterate all the ports and find out on which bus they are */
 537         i = 0;
 538         eth_node = dev_read_first_subnode(dev);
 539         for (node = ofnode_first_subnode(eth_node);
 540              ofnode_valid(node);
 541              node = ofnode_next_subnode(node)) {
 542                 if (ofnode_read_resource(node, 0, &res))
 543                         return -ENOMEM;
 544                 i = res.start;
 545
 546                 ofnode_parse_phandle_with_args(node, "phy-handle", NULL, 0, 0,
 547                                                &phandle);
 548
 549                 /* Get phy address on mdio bus */
 550                 if (ofnode_read_resource(phandle.node, 0, &res))
 551                         return -ENOMEM;
 552                 phy_addr = res.start;
 553
 554                 /* Get mdio node */
 555                 mdio_node = ofnode_get_parent(phandle.node);
 556
 557                 if (ofnode_read_resource(mdio_node, 0, &res))
 558                         return -ENOMEM;
 559                 faddr = cpu_to_fdt32(res.start);
 560
 561                 addr_base = ofnode_translate_address(mdio_node, &faddr);
 562                 addr_size = res.end - res.start;
 563
 564                 /* If the bus is new then create a new bus */
 565                 if (!get_mdiobus(addr_base, addr_size))
 566                         priv->bus[miim_count] =
 567                                 servalt_mdiobus_init(addr_base, addr_size);
 568
 569                 /* Connect mdio bus with the port */
 570                 bus = get_mdiobus(addr_base, addr_size);
 571                 add_port_entry(priv, i, phy_addr, bus);
 572         }
 573
 574         mscc_phy_reset();
 575
 576         for (i = 0; i < MAX_PORT; i++) {
 577                 if (!priv->ports[i].bus)
 578                         continue;
 579
 580                 phy_connect(priv->ports[i].bus, priv->ports[i].phy_addr, dev,
 581                             PHY_INTERFACE_MODE_NONE);
 582         }
 583
 584         return 0;
 585 }
 586
 587 static int servalt_remove(struct udevice *dev)
 588 {
 589         struct servalt_private *priv = dev_get_priv(dev);
 590         int i;
 591
 592         for (i = 0; i < SERVALT_MIIM_BUS_COUNT; i++) {
 593                 mdio_unregister(priv->bus[i]);
 594                 mdio_free(priv->bus[i]);
 595         }
 596
 597         return 0;
 598 }
 599
 600 static const struct eth_ops servalt_ops = {
 601         .start        = servalt_start,
 602         .stop         = servalt_stop,
 603         .send         = servalt_send,
 604         .recv         = servalt_recv,
 605         .write_hwaddr = servalt_write_hwaddr,
 606 };
 607
 608 static const struct udevice_id mscc_servalt_ids[] = {
 609         {.compatible = "mscc,vsc7437-switch" },
 610         { /* Sentinel */ }
 611 };
 612
 613 U_BOOT_DRIVER(servalt) = {
 614         .name                           = "servalt-switch",
 615         .id                             = UCLASS_ETH,
 616         .of_match                       = mscc_servalt_ids,
 617         .probe                          = servalt_probe,
 618         .remove                         = servalt_remove,
 619         .ops                            = &servalt_ops,
 620         .priv_auto_alloc_size           = sizeof(struct servalt_private),
 621         .platdata_auto_alloc_size       = sizeof(struct eth_pdata),
 622 };