drivers/net/phy/mv88e61xx.c

   1 /*
   2  * (C) Copyright 2015
   3  * Elecsys Corporation <www.elecsyscorp.com>
   4  * Kevin Smith <kevin.smith@elecsyscorp.com>
   5  *
   6  * Original driver:
   7  * (C) Copyright 2009
   8  * Marvell Semiconductor <www.marvell.com>
   9  * Prafulla Wadaskar <prafulla@marvell.com>
  10  *
  11  * SPDX-License-Identifier:     GPL-2.0+
  12  */
  13
  14 /*
  15  * PHY driver for mv88e61xx ethernet switches.
  16  *
  17  * This driver configures the mv88e61xx for basic use as a PHY.  The switch
  18  * supports a VLAN configuration that determines how traffic will be routed
  19  * between the ports.  This driver uses a simple configuration that routes
  20  * traffic from each PHY port only to the CPU port, and from the CPU port to
  21  * any PHY port.
  22  *
  23  * The configuration determines which PHY ports to activate using the
  24  * CONFIG_MV88E61XX_PHY_PORTS bitmask.  Setting bit 0 will activate port 0, bit
  25  * 1 activates port 1, etc.  Do not set the bit for the port the CPU is
  26  * connected to unless it is connected over a PHY interface (not MII).
  27  *
  28  * This driver was written for and tested on the mv88e6176 with an SGMII
  29  * connection.  Other configurations should be supported, but some additions or
  30  * changes may be required.
  31  */
  32
  33 #include <common.h>
  34
  35 #include <bitfield.h>
  36 #include <errno.h>
  37 #include <malloc.h>
  38 #include <miiphy.h>
  39 #include <netdev.h>
  40
  41 #define PHY_AUTONEGOTIATE_TIMEOUT       5000
  42
  43 #define PORT_COUNT                      11
  44 #define PORT_MASK                       ((1 << PORT_COUNT) - 1)
  45
  46 /* Device addresses */
  47 #define DEVADDR_PHY(p)                  (p)
  48 #define DEVADDR_PORT(p)                 (0x10 + (p))
  49 #define DEVADDR_SERDES                  0x0F
  50 #define DEVADDR_GLOBAL_1                0x1B
  51 #define DEVADDR_GLOBAL_2                0x1C
  52
  53 /* SMI indirection registers for multichip addressing mode */
  54 #define SMI_CMD_REG                     0x00
  55 #define SMI_DATA_REG                    0x01
  56
  57 /* Global registers */
  58 #define GLOBAL1_STATUS                  0x00
  59 #define GLOBAL1_CTRL                    0x04
  60 #define GLOBAL1_MON_CTRL                0x1A
  61
  62 /* Global 2 registers */
  63 #define GLOBAL2_REG_PHY_CMD             0x18
  64 #define GLOBAL2_REG_PHY_DATA            0x19
  65
  66 /* Port registers */
  67 #define PORT_REG_STATUS                 0x00
  68 #define PORT_REG_PHYS_CTRL              0x01
  69 #define PORT_REG_SWITCH_ID              0x03
  70 #define PORT_REG_CTRL                   0x04
  71 #define PORT_REG_VLAN_MAP               0x06
  72 #define PORT_REG_VLAN_ID                0x07
  73
  74 /* Phy registers */
  75 #define PHY_REG_CTRL1                   0x10
  76 #define PHY_REG_STATUS1                 0x11
  77 #define PHY_REG_PAGE                    0x16
  78
  79 /* Serdes registers */
  80 #define SERDES_REG_CTRL_1               0x10
  81
  82 /* Phy page numbers */
  83 #define PHY_PAGE_COPPER                 0
  84 #define PHY_PAGE_SERDES                 1
  85
  86 /* Register fields */
  87 #define GLOBAL1_CTRL_SWRESET            BIT(15)
  88
  89 #define GLOBAL1_MON_CTRL_CPUDEST_SHIFT  4
  90 #define GLOBAL1_MON_CTRL_CPUDEST_WIDTH  4
  91
  92 #define PORT_REG_STATUS_LINK            BIT(11)
  93 #define PORT_REG_STATUS_DUPLEX          BIT(10)
  94
  95 #define PORT_REG_STATUS_SPEED_SHIFT     8
  96 #define PORT_REG_STATUS_SPEED_WIDTH     2
  97 #define PORT_REG_STATUS_SPEED_10        0
  98 #define PORT_REG_STATUS_SPEED_100       1
  99 #define PORT_REG_STATUS_SPEED_1000      2
 100
 101 #define PORT_REG_STATUS_CMODE_MASK              0xF
 102 #define PORT_REG_STATUS_CMODE_100BASE_X         0x8
 103 #define PORT_REG_STATUS_CMODE_1000BASE_X        0x9
 104 #define PORT_REG_STATUS_CMODE_SGMII             0xa
 105
 106 #define PORT_REG_PHYS_CTRL_LINK_VALUE   BIT(5)
 107 #define PORT_REG_PHYS_CTRL_LINK_FORCE   BIT(4)
 108
 109 #define PORT_REG_CTRL_PSTATE_SHIFT      0
 110 #define PORT_REG_CTRL_PSTATE_WIDTH      2
 111
 112 #define PORT_REG_VLAN_ID_DEF_VID_SHIFT  0
 113 #define PORT_REG_VLAN_ID_DEF_VID_WIDTH  12
 114
 115 #define PORT_REG_VLAN_MAP_TABLE_SHIFT   0
 116 #define PORT_REG_VLAN_MAP_TABLE_WIDTH   11
 117
 118 #define SERDES_REG_CTRL_1_FORCE_LINK    BIT(10)
 119
 120 #define PHY_REG_CTRL1_ENERGY_DET_SHIFT  8
 121 #define PHY_REG_CTRL1_ENERGY_DET_WIDTH  2
 122
 123 /* Field values */
 124 #define PORT_REG_CTRL_PSTATE_DISABLED   0
 125 #define PORT_REG_CTRL_PSTATE_FORWARD    3
 126
 127 #define PHY_REG_CTRL1_ENERGY_DET_OFF    0
 128 #define PHY_REG_CTRL1_ENERGY_DET_SENSE_ONLY     2
 129 #define PHY_REG_CTRL1_ENERGY_DET_SENSE_XMIT     3
 130
 131 /* PHY Status Register */
 132 #define PHY_REG_STATUS1_SPEED           0xc000
 133 #define PHY_REG_STATUS1_GBIT            0x8000
 134 #define PHY_REG_STATUS1_100             0x4000
 135 #define PHY_REG_STATUS1_DUPLEX          0x2000
 136 #define PHY_REG_STATUS1_SPDDONE         0x0800
 137 #define PHY_REG_STATUS1_LINK            0x0400
 138 #define PHY_REG_STATUS1_ENERGY          0x0010
 139
 140 /*
 141  * Macros for building commands for indirect addressing modes.  These are valid
 142  * for both the indirect multichip addressing mode and the PHY indirection
 143  * required for the writes to any PHY register.
 144  */
 145 #define SMI_BUSY                        BIT(15)
 146 #define SMI_CMD_CLAUSE_22               BIT(12)
 147 #define SMI_CMD_CLAUSE_22_OP_READ       (2 << 10)
 148 #define SMI_CMD_CLAUSE_22_OP_WRITE      (1 << 10)
 149
 150 #define SMI_CMD_READ                    (SMI_BUSY | SMI_CMD_CLAUSE_22 | \
 151                                          SMI_CMD_CLAUSE_22_OP_READ)
 152 #define SMI_CMD_WRITE                   (SMI_BUSY | SMI_CMD_CLAUSE_22 | \
 153                                          SMI_CMD_CLAUSE_22_OP_WRITE)
 154
 155 #define SMI_CMD_ADDR_SHIFT              5
 156 #define SMI_CMD_ADDR_WIDTH              5
 157 #define SMI_CMD_REG_SHIFT               0
 158 #define SMI_CMD_REG_WIDTH               5
 159
 160 /* Check for required macros */
 161 #ifndef CONFIG_MV88E61XX_PHY_PORTS
 162 #error Define CONFIG_MV88E61XX_PHY_PORTS to indicate which physical ports \
 163         to activate
 164 #endif
 165 #ifndef CONFIG_MV88E61XX_CPU_PORT
 166 #error Define CONFIG_MV88E61XX_CPU_PORT to the port the CPU is attached to
 167 #endif
 168
 169 /* ID register values for different switch models */
 170 #define PORT_SWITCH_ID_6096             0x0980
 171 #define PORT_SWITCH_ID_6097             0x0990
 172 #define PORT_SWITCH_ID_6172             0x1720
 173 #define PORT_SWITCH_ID_6176             0x1760
 174 #define PORT_SWITCH_ID_6240             0x2400
 175 #define PORT_SWITCH_ID_6352             0x3520
 176
 177 struct mv88e61xx_phy_priv {
 178         struct mii_dev *mdio_bus;
 179         int smi_addr;
 180         int id;
 181 };
 182
 183 static inline int smi_cmd(int cmd, int addr, int reg)
 184 {
 185         cmd = bitfield_replace(cmd, SMI_CMD_ADDR_SHIFT, SMI_CMD_ADDR_WIDTH,
 186                                addr);
 187         cmd = bitfield_replace(cmd, SMI_CMD_REG_SHIFT, SMI_CMD_REG_WIDTH, reg);
 188         return cmd;
 189 }
 190
 191 static inline int smi_cmd_read(int addr, int reg)
 192 {
 193         return smi_cmd(SMI_CMD_READ, addr, reg);
 194 }
 195
 196 static inline int smi_cmd_write(int addr, int reg)
 197 {
 198         return smi_cmd(SMI_CMD_WRITE, addr, reg);
 199 }
 200
 201 __weak int mv88e61xx_hw_reset(struct phy_device *phydev)
 202 {
 203         return 0;
 204 }
 205
 206 /* Wait for the current SMI indirect command to complete */
 207 static int mv88e61xx_smi_wait(struct mii_dev *bus, int smi_addr)
 208 {
 209         int val;
 210         u32 timeout = 100;
 211
 212         do {
 213                 val = bus->read(bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG);
 214                 if (val >= 0 && (val & SMI_BUSY) == 0)
 215                         return 0;
 216
 217                 mdelay(1);
 218         } while (--timeout);
 219
 220         puts("SMI busy timeout\n");
 221         return -ETIMEDOUT;
 222 }
 223
 224 /*
 225  * The mv88e61xx has three types of addresses: the smi bus address, the device
 226  * address, and the register address.  The smi bus address distinguishes it on
 227  * the smi bus from other PHYs or switches.  The device address determines
 228  * which on-chip register set you are reading/writing (the various PHYs, their
 229  * associated ports, or global configuration registers).  The register address
 230  * is the offset of the register you are reading/writing.
 231  *
 232  * When the mv88e61xx is hardware configured to have address zero, it behaves in
 233  * single-chip addressing mode, where it responds to all SMI addresses, using
 234  * the smi address as its device address.  This obviously only works when this
 235  * is the only chip on the SMI bus.  This allows the driver to access device
 236  * registers without using indirection.  When the chip is configured to a
 237  * non-zero address, it only responds to that SMI address and requires indirect
 238  * writes to access the different device addresses.
 239  */
 240 static int mv88e61xx_reg_read(struct phy_device *phydev, int dev, int reg)
 241 {
 242         struct mv88e61xx_phy_priv *priv = phydev->priv;
 243         struct mii_dev *mdio_bus = priv->mdio_bus;
 244         int smi_addr = priv->smi_addr;
 245         int res;
 246
 247         /* In single-chip mode, the device can be addressed directly */
 248         if (smi_addr == 0)
 249                 return mdio_bus->read(mdio_bus, dev, MDIO_DEVAD_NONE, reg);
 250
 251         /* Wait for the bus to become free */
 252         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 253         if (res < 0)
 254                 return res;
 255
 256         /* Issue the read command */
 257         res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG,
 258                          smi_cmd_read(dev, reg));
 259         if (res < 0)
 260                 return res;
 261
 262         /* Wait for the read command to complete */
 263         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 264         if (res < 0)
 265                 return res;
 266
 267         /* Read the data */
 268         res = mdio_bus->read(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_DATA_REG);
 269         if (res < 0)
 270                 return res;
 271
 272         return bitfield_extract(res, 0, 16);
 273 }
 274
 275 /* See the comment above mv88e61xx_reg_read */
 276 static int mv88e61xx_reg_write(struct phy_device *phydev, int dev, int reg,
 277                                u16 val)
 278 {
 279         struct mv88e61xx_phy_priv *priv = phydev->priv;
 280         struct mii_dev *mdio_bus = priv->mdio_bus;
 281         int smi_addr = priv->smi_addr;
 282         int res;
 283
 284         /* In single-chip mode, the device can be addressed directly */
 285         if (smi_addr == 0) {
 286                 return mdio_bus->write(mdio_bus, dev, MDIO_DEVAD_NONE, reg,
 287                                 val);
 288         }
 289
 290         /* Wait for the bus to become free */
 291         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 292         if (res < 0)
 293                 return res;
 294
 295         /* Set the data to write */
 296         res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE,
 297                                 SMI_DATA_REG, val);
 298         if (res < 0)
 299                 return res;
 300
 301         /* Issue the write command */
 302         res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG,
 303                                 smi_cmd_write(dev, reg));
 304         if (res < 0)
 305                 return res;
 306
 307         /* Wait for the write command to complete */
 308         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 309         if (res < 0)
 310                 return res;
 311
 312         return 0;
 313 }
 314
 315 static int mv88e61xx_phy_wait(struct phy_device *phydev)
 316 {
 317         int val;
 318         u32 timeout = 100;
 319
 320         do {
 321                 val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_2,
 322                                          GLOBAL2_REG_PHY_CMD);
 323                 if (val >= 0 && (val & SMI_BUSY) == 0)
 324                         return 0;
 325
 326                 mdelay(1);
 327         } while (--timeout);
 328
 329         return -ETIMEDOUT;
 330 }
 331
 332 static int mv88e61xx_phy_read_indirect(struct mii_dev *smi_wrapper, int dev,
 333                 int devad, int reg)
 334 {
 335         struct phy_device *phydev;
 336         int res;
 337
 338         phydev = (struct phy_device *)smi_wrapper->priv;
 339
 340         /* Issue command to read */
 341         res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
 342                                   GLOBAL2_REG_PHY_CMD,
 343                                   smi_cmd_read(dev, reg));
 344
 345         /* Wait for data to be read */
 346         res = mv88e61xx_phy_wait(phydev);
 347         if (res < 0)
 348                 return res;
 349
 350         /* Read retrieved data */
 351         return mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_2,
 352                                   GLOBAL2_REG_PHY_DATA);
 353 }
 354
 355 static int mv88e61xx_phy_write_indirect(struct mii_dev *smi_wrapper, int dev,
 356                 int devad, int reg, u16 data)
 357 {
 358         struct phy_device *phydev;
 359         int res;
 360
 361         phydev = (struct phy_device *)smi_wrapper->priv;
 362
 363         /* Set the data to write */
 364         res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
 365                                   GLOBAL2_REG_PHY_DATA, data);
 366         if (res < 0)
 367                 return res;
 368         /* Issue the write command */
 369         res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
 370                                   GLOBAL2_REG_PHY_CMD,
 371                                   smi_cmd_write(dev, reg));
 372         if (res < 0)
 373                 return res;
 374
 375         /* Wait for command to complete */
 376         return mv88e61xx_phy_wait(phydev);
 377 }
 378
 379 /* Wrapper function to make calls to phy_read_indirect simpler */
 380 static int mv88e61xx_phy_read(struct phy_device *phydev, int phy, int reg)
 381 {
 382         return mv88e61xx_phy_read_indirect(phydev->bus, DEVADDR_PHY(phy),
 383                                            MDIO_DEVAD_NONE, reg);
 384 }
 385
 386 /* Wrapper function to make calls to phy_read_indirect simpler */
 387 static int mv88e61xx_phy_write(struct phy_device *phydev, int phy,
 388                 int reg, u16 val)
 389 {
 390         return mv88e61xx_phy_write_indirect(phydev->bus, DEVADDR_PHY(phy),
 391                                             MDIO_DEVAD_NONE, reg, val);
 392 }
 393
 394 static int mv88e61xx_port_read(struct phy_device *phydev, u8 port, u8 reg)
 395 {
 396         return mv88e61xx_reg_read(phydev, DEVADDR_PORT(port), reg);
 397 }
 398
 399 static int mv88e61xx_port_write(struct phy_device *phydev, u8 port, u8 reg,
 400                                                                 u16 val)
 401 {
 402         return mv88e61xx_reg_write(phydev, DEVADDR_PORT(port), reg, val);
 403 }
 404
 405 static int mv88e61xx_set_page(struct phy_device *phydev, u8 phy, u8 page)
 406 {
 407         return mv88e61xx_phy_write(phydev, phy, PHY_REG_PAGE, page);
 408 }
 409
 410 static int mv88e61xx_get_switch_id(struct phy_device *phydev)
 411 {
 412         int res;
 413
 414         res = mv88e61xx_port_read(phydev, 0, PORT_REG_SWITCH_ID);
 415         if (res < 0)
 416                 return res;
 417         return res & 0xfff0;
 418 }
 419
 420 static bool mv88e61xx_6352_family(struct phy_device *phydev)
 421 {
 422         struct mv88e61xx_phy_priv *priv = phydev->priv;
 423
 424         switch (priv->id) {
 425         case PORT_SWITCH_ID_6172:
 426         case PORT_SWITCH_ID_6176:
 427         case PORT_SWITCH_ID_6240:
 428         case PORT_SWITCH_ID_6352:
 429                 return true;
 430         }
 431         return false;
 432 }
 433
 434 static int mv88e61xx_get_cmode(struct phy_device *phydev, u8 port)
 435 {
 436         int res;
 437
 438         res = mv88e61xx_port_read(phydev, port, PORT_REG_STATUS);
 439         if (res < 0)
 440                 return res;
 441         return res & PORT_REG_STATUS_CMODE_MASK;
 442 }
 443
 444 static int mv88e61xx_parse_status(struct phy_device *phydev)
 445 {
 446         unsigned int speed;
 447         unsigned int mii_reg;
 448
 449         mii_reg = phy_read(phydev, MDIO_DEVAD_NONE, PHY_REG_STATUS1);
 450
 451         if ((mii_reg & PHY_REG_STATUS1_LINK) &&
 452             !(mii_reg & PHY_REG_STATUS1_SPDDONE)) {
 453                 int i = 0;
 454
 455                 puts("Waiting for PHY realtime link");
 456                 while (!(mii_reg & PHY_REG_STATUS1_SPDDONE)) {
 457                         /* Timeout reached ? */
 458                         if (i > PHY_AUTONEGOTIATE_TIMEOUT) {
 459                                 puts(" TIMEOUT !\n");
 460                                 phydev->link = 0;
 461                                 break;
 462                         }
 463
 464                         if ((i++ % 1000) == 0)
 465                                 putc('.');
 466                         udelay(1000);
 467                         mii_reg = phy_read(phydev, MDIO_DEVAD_NONE,
 468                                         PHY_REG_STATUS1);
 469                 }
 470                 puts(" done\n");
 471                 udelay(500000); /* another 500 ms (results in faster booting) */
 472         } else {
 473                 if (mii_reg & PHY_REG_STATUS1_LINK)
 474                         phydev->link = 1;
 475                 else
 476                         phydev->link = 0;
 477         }
 478
 479         if (mii_reg & PHY_REG_STATUS1_DUPLEX)
 480                 phydev->duplex = DUPLEX_FULL;
 481         else
 482                 phydev->duplex = DUPLEX_HALF;
 483
 484         speed = mii_reg & PHY_REG_STATUS1_SPEED;
 485
 486         switch (speed) {
 487         case PHY_REG_STATUS1_GBIT:
 488                 phydev->speed = SPEED_1000;
 489                 break;
 490         case PHY_REG_STATUS1_100:
 491                 phydev->speed = SPEED_100;
 492                 break;
 493         default:
 494                 phydev->speed = SPEED_10;
 495                 break;
 496         }
 497
 498         return 0;
 499 }
 500
 501 static int mv88e61xx_switch_reset(struct phy_device *phydev)
 502 {
 503         int time;
 504         int val;
 505         u8 port;
 506
 507         /* Disable all ports */
 508         for (port = 0; port < PORT_COUNT; port++) {
 509                 val = mv88e61xx_port_read(phydev, port, PORT_REG_CTRL);
 510                 if (val < 0)
 511                         return val;
 512                 val = bitfield_replace(val, PORT_REG_CTRL_PSTATE_SHIFT,
 513                                        PORT_REG_CTRL_PSTATE_WIDTH,
 514                                        PORT_REG_CTRL_PSTATE_DISABLED);
 515                 val = mv88e61xx_port_write(phydev, port, PORT_REG_CTRL, val);
 516                 if (val < 0)
 517                         return val;
 518         }
 519
 520         /* Wait 2 ms for queues to drain */
 521         udelay(2000);
 522
 523         /* Reset switch */
 524         val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1, GLOBAL1_CTRL);
 525         if (val < 0)
 526                 return val;
 527         val |= GLOBAL1_CTRL_SWRESET;
 528         val = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_1,
 529                                      GLOBAL1_CTRL, val);
 530         if (val < 0)
 531                 return val;
 532
 533         /* Wait up to 1 second for switch reset complete */
 534         for (time = 1000; time; time--) {
 535                 val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1,
 536                                             GLOBAL1_CTRL);
 537                 if (val >= 0 && ((val & GLOBAL1_CTRL_SWRESET) == 0))
 538                         break;
 539                 udelay(1000);
 540         }
 541         if (!time)
 542                 return -ETIMEDOUT;
 543
 544         return 0;
 545 }
 546
 547 static int mv88e61xx_serdes_init(struct phy_device *phydev)
 548 {
 549         int val;
 550
 551         val = mv88e61xx_set_page(phydev, DEVADDR_SERDES, PHY_PAGE_SERDES);
 552         if (val < 0)
 553                 return val;
 554
 555         /* Power up serdes module */
 556         val = mv88e61xx_phy_read(phydev, DEVADDR_SERDES, MII_BMCR);
 557         if (val < 0)
 558                 return val;
 559         val &= ~(BMCR_PDOWN);
 560         val = mv88e61xx_phy_write(phydev, DEVADDR_SERDES, MII_BMCR, val);
 561         if (val < 0)
 562                 return val;
 563
 564         return 0;
 565 }
 566
 567 static int mv88e61xx_port_enable(struct phy_device *phydev, u8 port)
 568 {
 569         int val;
 570
 571         val = mv88e61xx_port_read(phydev, port, PORT_REG_CTRL);
 572         if (val < 0)
 573                 return val;
 574         val = bitfield_replace(val, PORT_REG_CTRL_PSTATE_SHIFT,
 575                                PORT_REG_CTRL_PSTATE_WIDTH,
 576                                PORT_REG_CTRL_PSTATE_FORWARD);
 577         val = mv88e61xx_port_write(phydev, port, PORT_REG_CTRL, val);
 578         if (val < 0)
 579                 return val;
 580
 581         return 0;
 582 }
 583
 584 static int mv88e61xx_port_set_vlan(struct phy_device *phydev, u8 port,
 585                                                         u16 mask)
 586 {
 587         int val;
 588
 589         /* Set VID to port number plus one */
 590         val = mv88e61xx_port_read(phydev, port, PORT_REG_VLAN_ID);
 591         if (val < 0)
 592                 return val;
 593         val = bitfield_replace(val, PORT_REG_VLAN_ID_DEF_VID_SHIFT,
 594                                PORT_REG_VLAN_ID_DEF_VID_WIDTH,
 595                                port + 1);
 596         val = mv88e61xx_port_write(phydev, port, PORT_REG_VLAN_ID, val);
 597         if (val < 0)
 598                 return val;
 599
 600         /* Set VID mask */
 601         val = mv88e61xx_port_read(phydev, port, PORT_REG_VLAN_MAP);
 602         if (val < 0)
 603                 return val;
 604         val = bitfield_replace(val, PORT_REG_VLAN_MAP_TABLE_SHIFT,
 605                                PORT_REG_VLAN_MAP_TABLE_WIDTH,
 606                                mask);
 607         val = mv88e61xx_port_write(phydev, port, PORT_REG_VLAN_MAP, val);
 608         if (val < 0)
 609                 return val;
 610
 611         return 0;
 612 }
 613
 614 static int mv88e61xx_read_port_config(struct phy_device *phydev, u8 port)
 615 {
 616         int res;
 617         int val;
 618         bool forced = false;
 619
 620         val = mv88e61xx_port_read(phydev, port, PORT_REG_STATUS);
 621         if (val < 0)
 622                 return val;
 623         if (!(val & PORT_REG_STATUS_LINK)) {
 624                 /* Temporarily force link to read port configuration */
 625                 u32 timeout = 100;
 626                 forced = true;
 627
 628                 val = mv88e61xx_port_read(phydev, port, PORT_REG_PHYS_CTRL);
 629                 if (val < 0)
 630                         return val;
 631                 val |= (PORT_REG_PHYS_CTRL_LINK_FORCE |
 632                                 PORT_REG_PHYS_CTRL_LINK_VALUE);
 633                 val = mv88e61xx_port_write(phydev, port, PORT_REG_PHYS_CTRL,
 634                                            val);
 635                 if (val < 0)
 636                         return val;
 637
 638                 /* Wait for status register to reflect forced link */
 639                 do {
 640                         val = mv88e61xx_port_read(phydev, port,
 641                                                   PORT_REG_STATUS);
 642                         if (val < 0)
 643                                 goto unforce;
 644                         if (val & PORT_REG_STATUS_LINK)
 645                                 break;
 646                 } while (--timeout);
 647
 648                 if (timeout == 0) {
 649                         res = -ETIMEDOUT;
 650                         goto unforce;
 651                 }
 652         }
 653
 654         if (val & PORT_REG_STATUS_DUPLEX)
 655                 phydev->duplex = DUPLEX_FULL;
 656         else
 657                 phydev->duplex = DUPLEX_HALF;
 658
 659         val = bitfield_extract(val, PORT_REG_STATUS_SPEED_SHIFT,
 660                                PORT_REG_STATUS_SPEED_WIDTH);
 661         switch (val) {
 662         case PORT_REG_STATUS_SPEED_1000:
 663                 phydev->speed = SPEED_1000;
 664                 break;
 665         case PORT_REG_STATUS_SPEED_100:
 666                 phydev->speed = SPEED_100;
 667                 break;
 668         default:
 669                 phydev->speed = SPEED_10;
 670                 break;
 671         }
 672
 673         res = 0;
 674
 675 unforce:
 676         if (forced) {
 677                 val = mv88e61xx_port_read(phydev, port, PORT_REG_PHYS_CTRL);
 678                 if (val < 0)
 679                         return val;
 680                 val &= ~(PORT_REG_PHYS_CTRL_LINK_FORCE |
 681                                 PORT_REG_PHYS_CTRL_LINK_VALUE);
 682                 val = mv88e61xx_port_write(phydev, port, PORT_REG_PHYS_CTRL,
 683                                            val);
 684                 if (val < 0)
 685                         return val;
 686         }
 687
 688         return res;
 689 }
 690
 691 static int mv88e61xx_set_cpu_port(struct phy_device *phydev)
 692 {
 693         int val;
 694
 695         /* Set CPUDest */
 696         val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1, GLOBAL1_MON_CTRL);
 697         if (val < 0)
 698                 return val;
 699         val = bitfield_replace(val, GLOBAL1_MON_CTRL_CPUDEST_SHIFT,
 700                                GLOBAL1_MON_CTRL_CPUDEST_WIDTH,
 701                                CONFIG_MV88E61XX_CPU_PORT);
 702         val = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_1,
 703                                      GLOBAL1_MON_CTRL, val);
 704         if (val < 0)
 705                 return val;
 706
 707         /* Allow CPU to route to any port */
 708         val = PORT_MASK & ~(1 << CONFIG_MV88E61XX_CPU_PORT);
 709         val = mv88e61xx_port_set_vlan(phydev, CONFIG_MV88E61XX_CPU_PORT, val);
 710         if (val < 0)
 711                 return val;
 712
 713         /* Enable CPU port */
 714         val = mv88e61xx_port_enable(phydev, CONFIG_MV88E61XX_CPU_PORT);
 715         if (val < 0)
 716                 return val;
 717
 718         val = mv88e61xx_read_port_config(phydev, CONFIG_MV88E61XX_CPU_PORT);
 719         if (val < 0)
 720                 return val;
 721
 722         /* If CPU is connected to serdes, initialize serdes */
 723         if (mv88e61xx_6352_family(phydev)) {
 724                 val = mv88e61xx_get_cmode(phydev, CONFIG_MV88E61XX_CPU_PORT);
 725                 if (val < 0)
 726                         return val;
 727                 if (val == PORT_REG_STATUS_CMODE_100BASE_X ||
 728                     val == PORT_REG_STATUS_CMODE_1000BASE_X ||
 729                     val == PORT_REG_STATUS_CMODE_SGMII) {
 730                         val = mv88e61xx_serdes_init(phydev);
 731                         if (val < 0)
 732                                 return val;
 733                 }
 734         }
 735
 736         return 0;
 737 }
 738
 739 static int mv88e61xx_switch_init(struct phy_device *phydev)
 740 {
 741         static int init;
 742         int res;
 743
 744         if (init)
 745                 return 0;
 746
 747         res = mv88e61xx_switch_reset(phydev);
 748         if (res < 0)
 749                 return res;
 750
 751         res = mv88e61xx_set_cpu_port(phydev);
 752         if (res < 0)
 753                 return res;
 754
 755         init = 1;
 756
 757         return 0;
 758 }
 759
 760 static int mv88e61xx_phy_enable(struct phy_device *phydev, u8 phy)
 761 {
 762         int val;
 763
 764         val = mv88e61xx_phy_read(phydev, phy, MII_BMCR);
 765         if (val < 0)
 766                 return val;
 767         val &= ~(BMCR_PDOWN);
 768         val = mv88e61xx_phy_write(phydev, phy, MII_BMCR, val);
 769         if (val < 0)
 770                 return val;
 771
 772         return 0;
 773 }
 774
 775 static int mv88e61xx_phy_setup(struct phy_device *phydev, u8 phy)
 776 {
 777         int val;
 778
 779         /*
 780          * Enable energy-detect sensing on PHY, used to determine when a PHY
 781          * port is physically connected
 782          */
 783         val = mv88e61xx_phy_read(phydev, phy, PHY_REG_CTRL1);
 784         if (val < 0)
 785                 return val;
 786         val = bitfield_replace(val, PHY_REG_CTRL1_ENERGY_DET_SHIFT,
 787                                PHY_REG_CTRL1_ENERGY_DET_WIDTH,
 788                                PHY_REG_CTRL1_ENERGY_DET_SENSE_XMIT);
 789         val = mv88e61xx_phy_write(phydev, phy, PHY_REG_CTRL1, val);
 790         if (val < 0)
 791                 return val;
 792
 793         return 0;
 794 }
 795
 796 static int mv88e61xx_phy_config_port(struct phy_device *phydev, u8 phy)
 797 {
 798         int val;
 799
 800         val = mv88e61xx_port_enable(phydev, phy);
 801         if (val < 0)
 802                 return val;
 803
 804         val = mv88e61xx_port_set_vlan(phydev, phy,
 805                         1 << CONFIG_MV88E61XX_CPU_PORT);
 806         if (val < 0)
 807                 return val;
 808
 809         return 0;
 810 }
 811
 812 static int mv88e61xx_probe(struct phy_device *phydev)
 813 {
 814         struct mii_dev *smi_wrapper;
 815         struct mv88e61xx_phy_priv *priv;
 816         int res;
 817
 818         res = mv88e61xx_hw_reset(phydev);
 819         if (res < 0)
 820                 return res;
 821
 822         priv = malloc(sizeof(*priv));
 823         if (!priv)
 824                 return -ENOMEM;
 825
 826         memset(priv, 0, sizeof(*priv));
 827
 828         /*
 829          * This device requires indirect reads/writes to the PHY registers
 830          * which the generic PHY code can't handle.  Make a wrapper MII device
 831          * to handle reads/writes
 832          */
 833         smi_wrapper = mdio_alloc();
 834         if (!smi_wrapper) {
 835                 free(priv);
 836                 return -ENOMEM;
 837         }
 838
 839         /*
 840          * Store the mdio bus in the private data, as we are going to replace
 841          * the bus with the wrapper bus
 842          */
 843         priv->mdio_bus = phydev->bus;
 844
 845         /*
 846          * Store the smi bus address in private data.  This lets us use the
 847          * phydev addr field for device address instead, as the genphy code
 848          * expects.
 849          */
 850         priv->smi_addr = phydev->addr;
 851
 852         /*
 853          * Store the phy_device in the wrapper mii device. This lets us get it
 854          * back when genphy functions call phy_read/phy_write.
 855          */
 856         smi_wrapper->priv = phydev;
 857         strncpy(smi_wrapper->name, "indirect mii", sizeof(smi_wrapper->name));
 858         smi_wrapper->read = mv88e61xx_phy_read_indirect;
 859         smi_wrapper->write = mv88e61xx_phy_write_indirect;
 860
 861         /* Replace the bus with the wrapper device */
 862         phydev->bus = smi_wrapper;
 863
 864         phydev->priv = priv;
 865
 866         priv->id = mv88e61xx_get_switch_id(phydev);
 867
 868         return 0;
 869 }
 870
 871 static int mv88e61xx_phy_config(struct phy_device *phydev)
 872 {
 873         int res;
 874         int i;
 875         int ret = -1;
 876
 877         res = mv88e61xx_switch_init(phydev);
 878         if (res < 0)
 879                 return res;
 880
 881         for (i = 0; i < PORT_COUNT; i++) {
 882                 if ((1 << i) & CONFIG_MV88E61XX_PHY_PORTS) {
 883                         phydev->addr = i;
 884
 885                         res = mv88e61xx_phy_enable(phydev, i);
 886                         if (res < 0) {
 887                                 printf("Error enabling PHY %i\n", i);
 888                                 continue;
 889                         }
 890                         res = mv88e61xx_phy_setup(phydev, i);
 891                         if (res < 0) {
 892                                 printf("Error setting up PHY %i\n", i);
 893                                 continue;
 894                         }
 895                         res = mv88e61xx_phy_config_port(phydev, i);
 896                         if (res < 0) {
 897                                 printf("Error configuring PHY %i\n", i);
 898                                 continue;
 899                         }
 900
 901                         res = genphy_config_aneg(phydev);
 902                         if (res < 0) {
 903                                 printf("Error setting PHY %i autoneg\n", i);
 904                                 continue;
 905                         }
 906                         res = phy_reset(phydev);
 907                         if (res < 0) {
 908                                 printf("Error resetting PHY %i\n", i);
 909                                 continue;
 910                         }
 911
 912                         /* Return success if any PHY succeeds */
 913                         ret = 0;
 914                 }
 915         }
 916
 917         return ret;
 918 }
 919
 920 static int mv88e61xx_phy_is_connected(struct phy_device *phydev)
 921 {
 922         int val;
 923
 924         val = mv88e61xx_phy_read(phydev, phydev->addr, PHY_REG_STATUS1);
 925         if (val < 0)
 926                 return 0;
 927
 928         /*
 929          * After reset, the energy detect signal remains high for a few seconds
 930          * regardless of whether a cable is connected.  This function will
 931          * return false positives during this time.
 932          */
 933         return (val & PHY_REG_STATUS1_ENERGY) == 0;
 934 }
 935
 936 static int mv88e61xx_phy_startup(struct phy_device *phydev)
 937 {
 938         int i;
 939         int link = 0;
 940         int res;
 941         int speed = phydev->speed;
 942         int duplex = phydev->duplex;
 943
 944         for (i = 0; i < PORT_COUNT; i++) {
 945                 if ((1 << i) & CONFIG_MV88E61XX_PHY_PORTS) {
 946                         phydev->addr = i;
 947                         if (!mv88e61xx_phy_is_connected(phydev))
 948                                 continue;
 949                         res = genphy_update_link(phydev);
 950                         if (res < 0)
 951                                 continue;
 952                         res = mv88e61xx_parse_status(phydev);
 953                         if (res < 0)
 954                                 continue;
 955                         link = (link || phydev->link);
 956                 }
 957         }
 958         phydev->link = link;
 959
 960         /* Restore CPU interface speed and duplex after it was changed for
 961          * other ports */
 962         phydev->speed = speed;
 963         phydev->duplex = duplex;
 964
 965         return 0;
 966 }
 967
 968 static struct phy_driver mv88e61xx_driver = {
 969         .name = "Marvell MV88E61xx",
 970         .uid = 0x01410eb1,
 971         .mask = 0xfffffff0,
 972         .features = PHY_GBIT_FEATURES,
 973         .probe = mv88e61xx_probe,
 974         .config = mv88e61xx_phy_config,
 975         .startup = mv88e61xx_phy_startup,
 976         .shutdown = &genphy_shutdown,
 977 };
 978
 979 static struct phy_driver mv88e609x_driver = {
 980         .name = "Marvell MV88E609x",
 981         .uid = 0x1410c89,
 982         .mask = 0xfffffff0,
 983         .features = PHY_GBIT_FEATURES,
 984         .probe = mv88e61xx_probe,
 985         .config = mv88e61xx_phy_config,
 986         .startup = mv88e61xx_phy_startup,
 987         .shutdown = &genphy_shutdown,
 988 };
 989
 990 int phy_mv88e61xx_init(void)
 991 {
 992         phy_register(&mv88e61xx_driver);
 993         phy_register(&mv88e609x_driver);
 994
 995         return 0;
 996 }
 997
 998 /*
 999  * Overload weak get_phy_id definition since we need non-standard functions
1000  * to read PHY registers
1001  */
1002 int get_phy_id(struct mii_dev *bus, int smi_addr, int devad, u32 *phy_id)
1003 {
1004         struct phy_device temp_phy;
1005         struct mv88e61xx_phy_priv temp_priv;
1006         struct mii_dev temp_mii;
1007         int val;
1008
1009         /*
1010          * Buid temporary data structures that the chip reading code needs to
1011          * read the ID
1012          */
1013         temp_priv.mdio_bus = bus;
1014         temp_priv.smi_addr = smi_addr;
1015         temp_phy.priv = &temp_priv;
1016         temp_mii.priv = &temp_phy;
1017
1018         val = mv88e61xx_phy_read_indirect(&temp_mii, 0, devad, MII_PHYSID1);
1019         if (val < 0)
1020                 return -EIO;
1021
1022         *phy_id = val << 16;
1023
1024         val = mv88e61xx_phy_read_indirect(&temp_mii, 0, devad, MII_PHYSID2);
1025         if (val < 0)
1026                 return -EIO;
1027
1028         *phy_id |= (val & 0xffff);
1029
1030         return 0;
1031 }