git.sur5r.net Git - u-boot/blob - 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                      7
  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_6172             0x1720
 171 #define PORT_SWITCH_ID_6176             0x1760
 172 #define PORT_SWITCH_ID_6240             0x2400
 173 #define PORT_SWITCH_ID_6352             0x3520
 174
 175 struct mv88e61xx_phy_priv {
 176         struct mii_dev *mdio_bus;
 177         int smi_addr;
 178         int id;
 179 };
 180
 181 static inline int smi_cmd(int cmd, int addr, int reg)
 182 {
 183         cmd = bitfield_replace(cmd, SMI_CMD_ADDR_SHIFT, SMI_CMD_ADDR_WIDTH,
 184                                addr);
 185         cmd = bitfield_replace(cmd, SMI_CMD_REG_SHIFT, SMI_CMD_REG_WIDTH, reg);
 186         return cmd;
 187 }
 188
 189 static inline int smi_cmd_read(int addr, int reg)
 190 {
 191         return smi_cmd(SMI_CMD_READ, addr, reg);
 192 }
 193
 194 static inline int smi_cmd_write(int addr, int reg)
 195 {
 196         return smi_cmd(SMI_CMD_WRITE, addr, reg);
 197 }
 198
 199 __weak int mv88e61xx_hw_reset(struct phy_device *phydev)
 200 {
 201         return 0;
 202 }
 203
 204 /* Wait for the current SMI indirect command to complete */
 205 static int mv88e61xx_smi_wait(struct mii_dev *bus, int smi_addr)
 206 {
 207         int val;
 208         u32 timeout = 100;
 209
 210         do {
 211                 val = bus->read(bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG);
 212                 if (val >= 0 && (val & SMI_BUSY) == 0)
 213                         return 0;
 214
 215                 mdelay(1);
 216         } while (--timeout);
 217
 218         puts("SMI busy timeout\n");
 219         return -ETIMEDOUT;
 220 }
 221
 222 /*
 223  * The mv88e61xx has three types of addresses: the smi bus address, the device
 224  * address, and the register address.  The smi bus address distinguishes it on
 225  * the smi bus from other PHYs or switches.  The device address determines
 226  * which on-chip register set you are reading/writing (the various PHYs, their
 227  * associated ports, or global configuration registers).  The register address
 228  * is the offset of the register you are reading/writing.
 229  *
 230  * When the mv88e61xx is hardware configured to have address zero, it behaves in
 231  * single-chip addressing mode, where it responds to all SMI addresses, using
 232  * the smi address as its device address.  This obviously only works when this
 233  * is the only chip on the SMI bus.  This allows the driver to access device
 234  * registers without using indirection.  When the chip is configured to a
 235  * non-zero address, it only responds to that SMI address and requires indirect
 236  * writes to access the different device addresses.
 237  */
 238 static int mv88e61xx_reg_read(struct phy_device *phydev, int dev, int reg)
 239 {
 240         struct mv88e61xx_phy_priv *priv = phydev->priv;
 241         struct mii_dev *mdio_bus = priv->mdio_bus;
 242         int smi_addr = priv->smi_addr;
 243         int res;
 244
 245         /* In single-chip mode, the device can be addressed directly */
 246         if (smi_addr == 0)
 247                 return mdio_bus->read(mdio_bus, dev, MDIO_DEVAD_NONE, reg);
 248
 249         /* Wait for the bus to become free */
 250         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 251         if (res < 0)
 252                 return res;
 253
 254         /* Issue the read command */
 255         res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG,
 256                          smi_cmd_read(dev, reg));
 257         if (res < 0)
 258                 return res;
 259
 260         /* Wait for the read command to complete */
 261         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 262         if (res < 0)
 263                 return res;
 264
 265         /* Read the data */
 266         res = mdio_bus->read(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_DATA_REG);
 267         if (res < 0)
 268                 return res;
 269
 270         return bitfield_extract(res, 0, 16);
 271 }
 272
 273 /* See the comment above mv88e61xx_reg_read */
 274 static int mv88e61xx_reg_write(struct phy_device *phydev, int dev, int reg,
 275                                u16 val)
 276 {
 277         struct mv88e61xx_phy_priv *priv = phydev->priv;
 278         struct mii_dev *mdio_bus = priv->mdio_bus;
 279         int smi_addr = priv->smi_addr;
 280         int res;
 281
 282         /* In single-chip mode, the device can be addressed directly */
 283         if (smi_addr == 0) {
 284                 return mdio_bus->write(mdio_bus, dev, MDIO_DEVAD_NONE, reg,
 285                                 val);
 286         }
 287
 288         /* Wait for the bus to become free */
 289         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 290         if (res < 0)
 291                 return res;
 292
 293         /* Set the data to write */
 294         res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE,
 295                                 SMI_DATA_REG, val);
 296         if (res < 0)
 297                 return res;
 298
 299         /* Issue the write command */
 300         res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG,
 301                                 smi_cmd_write(dev, reg));
 302         if (res < 0)
 303                 return res;
 304
 305         /* Wait for the write command to complete */
 306         res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
 307         if (res < 0)
 308                 return res;
 309
 310         return 0;
 311 }
 312
 313 static int mv88e61xx_phy_wait(struct phy_device *phydev)
 314 {
 315         int val;
 316         u32 timeout = 100;
 317
 318         do {
 319                 val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_2,
 320                                          GLOBAL2_REG_PHY_CMD);
 321                 if (val >= 0 && (val & SMI_BUSY) == 0)
 322                         return 0;
 323
 324                 mdelay(1);
 325         } while (--timeout);
 326
 327         return -ETIMEDOUT;
 328 }
 329
 330 static int mv88e61xx_phy_read_indirect(struct mii_dev *smi_wrapper, int dev,
 331                 int devad, int reg)
 332 {
 333         struct phy_device *phydev;
 334         int res;
 335
 336         phydev = (struct phy_device *)smi_wrapper->priv;
 337
 338         /* Issue command to read */
 339         res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
 340                                   GLOBAL2_REG_PHY_CMD,
 341                                   smi_cmd_read(dev, reg));
 342
 343         /* Wait for data to be read */
 344         res = mv88e61xx_phy_wait(phydev);
 345         if (res < 0)
 346                 return res;
 347
 348         /* Read retrieved data */
 349         return mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_2,
 350                                   GLOBAL2_REG_PHY_DATA);
 351 }
 352
 353 static int mv88e61xx_phy_write_indirect(struct mii_dev *smi_wrapper, int dev,
 354                 int devad, int reg, u16 data)
 355 {
 356         struct phy_device *phydev;
 357         int res;
 358
 359         phydev = (struct phy_device *)smi_wrapper->priv;
 360
 361         /* Set the data to write */
 362         res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
 363                                   GLOBAL2_REG_PHY_DATA, data);
 364         if (res < 0)
 365                 return res;
 366         /* Issue the write command */
 367         res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
 368                                   GLOBAL2_REG_PHY_CMD,
 369                                   smi_cmd_write(dev, reg));
 370         if (res < 0)
 371                 return res;
 372
 373         /* Wait for command to complete */
 374         return mv88e61xx_phy_wait(phydev);
 375 }
 376
 377 /* Wrapper function to make calls to phy_read_indirect simpler */
 378 static int mv88e61xx_phy_read(struct phy_device *phydev, int phy, int reg)
 379 {
 380         return mv88e61xx_phy_read_indirect(phydev->bus, DEVADDR_PHY(phy),
 381                                            MDIO_DEVAD_NONE, reg);
 382 }
 383
 384 /* Wrapper function to make calls to phy_read_indirect simpler */
 385 static int mv88e61xx_phy_write(struct phy_device *phydev, int phy,
 386                 int reg, u16 val)
 387 {
 388         return mv88e61xx_phy_write_indirect(phydev->bus, DEVADDR_PHY(phy),
 389                                             MDIO_DEVAD_NONE, reg, val);
 390 }
 391
 392 static int mv88e61xx_port_read(struct phy_device *phydev, u8 port, u8 reg)
 393 {
 394         return mv88e61xx_reg_read(phydev, DEVADDR_PORT(port), reg);
 395 }
 396
 397 static int mv88e61xx_port_write(struct phy_device *phydev, u8 port, u8 reg,
 398                                                                 u16 val)
 399 {
 400         return mv88e61xx_reg_write(phydev, DEVADDR_PORT(port), reg, val);
 401 }
 402
 403 static int mv88e61xx_set_page(struct phy_device *phydev, u8 phy, u8 page)
 404 {
 405         return mv88e61xx_phy_write(phydev, phy, PHY_REG_PAGE, page);
 406 }
 407
 408 static int mv88e61xx_get_switch_id(struct phy_device *phydev)
 409 {
 410         int res;
 411
 412         res = mv88e61xx_port_read(phydev, 0, PORT_REG_SWITCH_ID);
 413         if (res < 0)
 414                 return res;
 415         return res & 0xfff0;
 416 }
 417
 418 static bool mv88e61xx_6352_family(struct phy_device *phydev)
 419 {
 420         struct mv88e61xx_phy_priv *priv = phydev->priv;
 421
 422         switch (priv->id) {
 423         case PORT_SWITCH_ID_6172:
 424         case PORT_SWITCH_ID_6176:
 425         case PORT_SWITCH_ID_6240:
 426         case PORT_SWITCH_ID_6352:
 427                 return true;
 428         }
 429         return false;
 430 }
 431
 432 static int mv88e61xx_get_cmode(struct phy_device *phydev, u8 port)
 433 {
 434         int res;
 435
 436         res = mv88e61xx_port_read(phydev, port, PORT_REG_STATUS);
 437         if (res < 0)
 438                 return res;
 439         return res & PORT_REG_STATUS_CMODE_MASK;
 440 }
 441
 442 static int mv88e61xx_parse_status(struct phy_device *phydev)
 443 {
 444         unsigned int speed;
 445         unsigned int mii_reg;
 446
 447         mii_reg = phy_read(phydev, MDIO_DEVAD_NONE, PHY_REG_STATUS1);
 448
 449         if ((mii_reg & PHY_REG_STATUS1_LINK) &&
 450             !(mii_reg & PHY_REG_STATUS1_SPDDONE)) {
 451                 int i = 0;
 452
 453                 puts("Waiting for PHY realtime link");
 454                 while (!(mii_reg & PHY_REG_STATUS1_SPDDONE)) {
 455                         /* Timeout reached ? */
 456                         if (i > PHY_AUTONEGOTIATE_TIMEOUT) {
 457                                 puts(" TIMEOUT !\n");
 458                                 phydev->link = 0;
 459                                 break;
 460                         }
 461
 462                         if ((i++ % 1000) == 0)
 463                                 putc('.');
 464                         udelay(1000);
 465                         mii_reg = phy_read(phydev, MDIO_DEVAD_NONE,
 466                                         PHY_REG_STATUS1);
 467                 }
 468                 puts(" done\n");
 469                 udelay(500000); /* another 500 ms (results in faster booting) */
 470         } else {
 471                 if (mii_reg & PHY_REG_STATUS1_LINK)
 472                         phydev->link = 1;
 473                 else
 474                         phydev->link = 0;
 475         }
 476
 477         if (mii_reg & PHY_REG_STATUS1_DUPLEX)
 478                 phydev->duplex = DUPLEX_FULL;
 479         else
 480                 phydev->duplex = DUPLEX_HALF;
 481
 482         speed = mii_reg & PHY_REG_STATUS1_SPEED;
 483
 484         switch (speed) {
 485         case PHY_REG_STATUS1_GBIT:
 486                 phydev->speed = SPEED_1000;
 487                 break;
 488         case PHY_REG_STATUS1_100:
 489                 phydev->speed = SPEED_100;
 490                 break;
 491         default:
 492                 phydev->speed = SPEED_10;
 493                 break;
 494         }
 495
 496         return 0;
 497 }
 498
 499 static int mv88e61xx_switch_reset(struct phy_device *phydev)
 500 {
 501         int time;
 502         int val;
 503         u8 port;
 504
 505         /* Disable all ports */
 506         for (port = 0; port < PORT_COUNT; port++) {
 507                 val = mv88e61xx_port_read(phydev, port, PORT_REG_CTRL);
 508                 if (val < 0)
 509                         return val;
 510                 val = bitfield_replace(val, PORT_REG_CTRL_PSTATE_SHIFT,
 511                                        PORT_REG_CTRL_PSTATE_WIDTH,
 512                                        PORT_REG_CTRL_PSTATE_DISABLED);
 513                 val = mv88e61xx_port_write(phydev, port, PORT_REG_CTRL, val);
 514                 if (val < 0)
 515                         return val;
 516         }
 517
 518         /* Wait 2 ms for queues to drain */
 519         udelay(2000);
 520
 521         /* Reset switch */
 522         val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1, GLOBAL1_CTRL);
 523         if (val < 0)
 524                 return val;
 525         val |= GLOBAL1_CTRL_SWRESET;
 526         val = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_1,
 527                                      GLOBAL1_CTRL, val);
 528         if (val < 0)
 529                 return val;
 530
 531         /* Wait up to 1 second for switch reset complete */
 532         for (time = 1000; time; time--) {
 533                 val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1,
 534                                             GLOBAL1_CTRL);
 535                 if (val >= 0 && ((val & GLOBAL1_CTRL_SWRESET) == 0))
 536                         break;
 537                 udelay(1000);
 538         }
 539         if (!time)
 540                 return -ETIMEDOUT;
 541
 542         return 0;
 543 }
 544
 545 static int mv88e61xx_serdes_init(struct phy_device *phydev)
 546 {
 547         int val;
 548
 549         val = mv88e61xx_set_page(phydev, DEVADDR_SERDES, PHY_PAGE_SERDES);
 550         if (val < 0)
 551                 return val;
 552
 553         /* Power up serdes module */
 554         val = mv88e61xx_phy_read(phydev, DEVADDR_SERDES, MII_BMCR);
 555         if (val < 0)
 556                 return val;
 557         val &= ~(BMCR_PDOWN);
 558         val = mv88e61xx_phy_write(phydev, DEVADDR_SERDES, MII_BMCR, val);
 559         if (val < 0)
 560                 return val;
 561
 562         return 0;
 563 }
 564
 565 static int mv88e61xx_port_enable(struct phy_device *phydev, u8 port)
 566 {
 567         int val;
 568
 569         val = mv88e61xx_port_read(phydev, port, PORT_REG_CTRL);
 570         if (val < 0)
 571                 return val;
 572         val = bitfield_replace(val, PORT_REG_CTRL_PSTATE_SHIFT,
 573                                PORT_REG_CTRL_PSTATE_WIDTH,
 574                                PORT_REG_CTRL_PSTATE_FORWARD);
 575         val = mv88e61xx_port_write(phydev, port, PORT_REG_CTRL, val);
 576         if (val < 0)
 577                 return val;
 578
 579         return 0;
 580 }
 581
 582 static int mv88e61xx_port_set_vlan(struct phy_device *phydev, u8 port,
 583                                                         u8 mask)
 584 {
 585         int val;
 586
 587         /* Set VID to port number plus one */
 588         val = mv88e61xx_port_read(phydev, port, PORT_REG_VLAN_ID);
 589         if (val < 0)
 590                 return val;
 591         val = bitfield_replace(val, PORT_REG_VLAN_ID_DEF_VID_SHIFT,
 592                                PORT_REG_VLAN_ID_DEF_VID_WIDTH,
 593                                port + 1);
 594         val = mv88e61xx_port_write(phydev, port, PORT_REG_VLAN_ID, val);
 595         if (val < 0)
 596                 return val;
 597
 598         /* Set VID mask */
 599         val = mv88e61xx_port_read(phydev, port, PORT_REG_VLAN_MAP);
 600         if (val < 0)
 601                 return val;
 602         val = bitfield_replace(val, PORT_REG_VLAN_MAP_TABLE_SHIFT,
 603                                PORT_REG_VLAN_MAP_TABLE_WIDTH,
 604                                mask);
 605         val = mv88e61xx_port_write(phydev, port, PORT_REG_VLAN_MAP, val);
 606         if (val < 0)
 607                 return val;
 608
 609         return 0;
 610 }
 611
 612 static int mv88e61xx_read_port_config(struct phy_device *phydev, u8 port)
 613 {
 614         int res;
 615         int val;
 616         bool forced = false;
 617
 618         val = mv88e61xx_port_read(phydev, port, PORT_REG_STATUS);
 619         if (val < 0)
 620                 return val;
 621         if (!(val & PORT_REG_STATUS_LINK)) {
 622                 /* Temporarily force link to read port configuration */
 623                 u32 timeout = 100;
 624                 forced = true;
 625
 626                 val = mv88e61xx_port_read(phydev, port, PORT_REG_PHYS_CTRL);
 627                 if (val < 0)
 628                         return val;
 629                 val |= (PORT_REG_PHYS_CTRL_LINK_FORCE |
 630                                 PORT_REG_PHYS_CTRL_LINK_VALUE);
 631                 val = mv88e61xx_port_write(phydev, port, PORT_REG_PHYS_CTRL,
 632                                            val);
 633                 if (val < 0)
 634                         return val;
 635
 636                 /* Wait for status register to reflect forced link */
 637                 do {
 638                         val = mv88e61xx_port_read(phydev, port,
 639                                                   PORT_REG_STATUS);
 640                         if (val < 0)
 641                                 goto unforce;
 642                         if (val & PORT_REG_STATUS_LINK)
 643                                 break;
 644                 } while (--timeout);
 645
 646                 if (timeout == 0) {
 647                         res = -ETIMEDOUT;
 648                         goto unforce;
 649                 }
 650         }
 651
 652         if (val & PORT_REG_STATUS_DUPLEX)
 653                 phydev->duplex = DUPLEX_FULL;
 654         else
 655                 phydev->duplex = DUPLEX_HALF;
 656
 657         val = bitfield_extract(val, PORT_REG_STATUS_SPEED_SHIFT,
 658                                PORT_REG_STATUS_SPEED_WIDTH);
 659         switch (val) {
 660         case PORT_REG_STATUS_SPEED_1000:
 661                 phydev->speed = SPEED_1000;
 662                 break;
 663         case PORT_REG_STATUS_SPEED_100:
 664                 phydev->speed = SPEED_100;
 665                 break;
 666         default:
 667                 phydev->speed = SPEED_10;
 668                 break;
 669         }
 670
 671         res = 0;
 672
 673 unforce:
 674         if (forced) {
 675                 val = mv88e61xx_port_read(phydev, port, PORT_REG_PHYS_CTRL);
 676                 if (val < 0)
 677                         return val;
 678                 val &= ~(PORT_REG_PHYS_CTRL_LINK_FORCE |
 679                                 PORT_REG_PHYS_CTRL_LINK_VALUE);
 680                 val = mv88e61xx_port_write(phydev, port, PORT_REG_PHYS_CTRL,
 681                                            val);
 682                 if (val < 0)
 683                         return val;
 684         }
 685
 686         return res;
 687 }
 688
 689 static int mv88e61xx_set_cpu_port(struct phy_device *phydev)
 690 {
 691         int val;
 692
 693         /* Set CPUDest */
 694         val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1, GLOBAL1_MON_CTRL);
 695         if (val < 0)
 696                 return val;
 697         val = bitfield_replace(val, GLOBAL1_MON_CTRL_CPUDEST_SHIFT,
 698                                GLOBAL1_MON_CTRL_CPUDEST_WIDTH,
 699                                CONFIG_MV88E61XX_CPU_PORT);
 700         val = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_1,
 701                                      GLOBAL1_MON_CTRL, val);
 702         if (val < 0)
 703                 return val;
 704
 705         /* Allow CPU to route to any port */
 706         val = PORT_MASK & ~(1 << CONFIG_MV88E61XX_CPU_PORT);
 707         val = mv88e61xx_port_set_vlan(phydev, CONFIG_MV88E61XX_CPU_PORT, val);
 708         if (val < 0)
 709                 return val;
 710
 711         /* Enable CPU port */
 712         val = mv88e61xx_port_enable(phydev, CONFIG_MV88E61XX_CPU_PORT);
 713         if (val < 0)
 714                 return val;
 715
 716         val = mv88e61xx_read_port_config(phydev, CONFIG_MV88E61XX_CPU_PORT);
 717         if (val < 0)
 718                 return val;
 719
 720         /* If CPU is connected to serdes, initialize serdes */
 721         if (mv88e61xx_6352_family(phydev)) {
 722                 val = mv88e61xx_get_cmode(phydev, CONFIG_MV88E61XX_CPU_PORT);
 723                 if (val < 0)
 724                         return val;
 725                 if (val == PORT_REG_STATUS_CMODE_100BASE_X ||
 726                     val == PORT_REG_STATUS_CMODE_1000BASE_X ||
 727                     val == PORT_REG_STATUS_CMODE_SGMII) {
 728                         val = mv88e61xx_serdes_init(phydev);
 729                         if (val < 0)
 730                                 return val;
 731                 }
 732         }
 733
 734         return 0;
 735 }
 736
 737 static int mv88e61xx_switch_init(struct phy_device *phydev)
 738 {
 739         static int init;
 740         int res;
 741
 742         if (init)
 743                 return 0;
 744
 745         res = mv88e61xx_switch_reset(phydev);
 746         if (res < 0)
 747                 return res;
 748
 749         res = mv88e61xx_set_cpu_port(phydev);
 750         if (res < 0)
 751                 return res;
 752
 753         init = 1;
 754
 755         return 0;
 756 }
 757
 758 static int mv88e61xx_phy_enable(struct phy_device *phydev, u8 phy)
 759 {
 760         int val;
 761
 762         val = mv88e61xx_phy_read(phydev, phy, MII_BMCR);
 763         if (val < 0)
 764                 return val;
 765         val &= ~(BMCR_PDOWN);
 766         val = mv88e61xx_phy_write(phydev, phy, MII_BMCR, val);
 767         if (val < 0)
 768                 return val;
 769
 770         return 0;
 771 }
 772
 773 static int mv88e61xx_phy_setup(struct phy_device *phydev, u8 phy)
 774 {
 775         int val;
 776
 777         /*
 778          * Enable energy-detect sensing on PHY, used to determine when a PHY
 779          * port is physically connected
 780          */
 781         val = mv88e61xx_phy_read(phydev, phy, PHY_REG_CTRL1);
 782         if (val < 0)
 783                 return val;
 784         val = bitfield_replace(val, PHY_REG_CTRL1_ENERGY_DET_SHIFT,
 785                                PHY_REG_CTRL1_ENERGY_DET_WIDTH,
 786                                PHY_REG_CTRL1_ENERGY_DET_SENSE_XMIT);
 787         val = mv88e61xx_phy_write(phydev, phy, PHY_REG_CTRL1, val);
 788         if (val < 0)
 789                 return val;
 790
 791         return 0;
 792 }
 793
 794 static int mv88e61xx_phy_config_port(struct phy_device *phydev, u8 phy)
 795 {
 796         int val;
 797
 798         val = mv88e61xx_port_enable(phydev, phy);
 799         if (val < 0)
 800                 return val;
 801
 802         val = mv88e61xx_port_set_vlan(phydev, phy,
 803                         1 << CONFIG_MV88E61XX_CPU_PORT);
 804         if (val < 0)
 805                 return val;
 806
 807         return 0;
 808 }
 809
 810 static int mv88e61xx_probe(struct phy_device *phydev)
 811 {
 812         struct mii_dev *smi_wrapper;
 813         struct mv88e61xx_phy_priv *priv;
 814         int res;
 815
 816         res = mv88e61xx_hw_reset(phydev);
 817         if (res < 0)
 818                 return res;
 819
 820         priv = malloc(sizeof(*priv));
 821         if (!priv)
 822                 return -ENOMEM;
 823
 824         memset(priv, 0, sizeof(*priv));
 825
 826         /*
 827          * This device requires indirect reads/writes to the PHY registers
 828          * which the generic PHY code can't handle.  Make a wrapper MII device
 829          * to handle reads/writes
 830          */
 831         smi_wrapper = mdio_alloc();
 832         if (!smi_wrapper) {
 833                 free(priv);
 834                 return -ENOMEM;
 835         }
 836
 837         /*
 838          * Store the mdio bus in the private data, as we are going to replace
 839          * the bus with the wrapper bus
 840          */
 841         priv->mdio_bus = phydev->bus;
 842
 843         /*
 844          * Store the smi bus address in private data.  This lets us use the
 845          * phydev addr field for device address instead, as the genphy code
 846          * expects.
 847          */
 848         priv->smi_addr = phydev->addr;
 849
 850         /*
 851          * Store the phy_device in the wrapper mii device. This lets us get it
 852          * back when genphy functions call phy_read/phy_write.
 853          */
 854         smi_wrapper->priv = phydev;
 855         strncpy(smi_wrapper->name, "indirect mii", sizeof(smi_wrapper->name));
 856         smi_wrapper->read = mv88e61xx_phy_read_indirect;
 857         smi_wrapper->write = mv88e61xx_phy_write_indirect;
 858
 859         /* Replace the bus with the wrapper device */
 860         phydev->bus = smi_wrapper;
 861
 862         phydev->priv = priv;
 863
 864         priv->id = mv88e61xx_get_switch_id(phydev);
 865
 866         return 0;
 867 }
 868
 869 static int mv88e61xx_phy_config(struct phy_device *phydev)
 870 {
 871         int res;
 872         int i;
 873         int ret = -1;
 874
 875         res = mv88e61xx_switch_init(phydev);
 876         if (res < 0)
 877                 return res;
 878
 879         for (i = 0; i < PORT_COUNT; i++) {
 880                 if ((1 << i) & CONFIG_MV88E61XX_PHY_PORTS) {
 881                         phydev->addr = i;
 882
 883                         res = mv88e61xx_phy_enable(phydev, i);
 884                         if (res < 0) {
 885                                 printf("Error enabling PHY %i\n", i);
 886                                 continue;
 887                         }
 888                         res = mv88e61xx_phy_setup(phydev, i);
 889                         if (res < 0) {
 890                                 printf("Error setting up PHY %i\n", i);
 891                                 continue;
 892                         }
 893                         res = mv88e61xx_phy_config_port(phydev, i);
 894                         if (res < 0) {
 895                                 printf("Error configuring PHY %i\n", i);
 896                                 continue;
 897                         }
 898
 899                         res = genphy_config_aneg(phydev);
 900                         if (res < 0) {
 901                                 printf("Error setting PHY %i autoneg\n", i);
 902                                 continue;
 903                         }
 904                         res = phy_reset(phydev);
 905                         if (res < 0) {
 906                                 printf("Error resetting PHY %i\n", i);
 907                                 continue;
 908                         }
 909
 910                         /* Return success if any PHY succeeds */
 911                         ret = 0;
 912                 }
 913         }
 914
 915         return ret;
 916 }
 917
 918 static int mv88e61xx_phy_is_connected(struct phy_device *phydev)
 919 {
 920         int val;
 921
 922         val = mv88e61xx_phy_read(phydev, phydev->addr, PHY_REG_STATUS1);
 923         if (val < 0)
 924                 return 0;
 925
 926         /*
 927          * After reset, the energy detect signal remains high for a few seconds
 928          * regardless of whether a cable is connected.  This function will
 929          * return false positives during this time.
 930          */
 931         return (val & PHY_REG_STATUS1_ENERGY) == 0;
 932 }
 933
 934 static int mv88e61xx_phy_startup(struct phy_device *phydev)
 935 {
 936         int i;
 937         int link = 0;
 938         int res;
 939         int speed = phydev->speed;
 940         int duplex = phydev->duplex;
 941
 942         for (i = 0; i < PORT_COUNT; i++) {
 943                 if ((1 << i) & CONFIG_MV88E61XX_PHY_PORTS) {
 944                         phydev->addr = i;
 945                         if (!mv88e61xx_phy_is_connected(phydev))
 946                                 continue;
 947                         res = genphy_update_link(phydev);
 948                         if (res < 0)
 949                                 continue;
 950                         res = mv88e61xx_parse_status(phydev);
 951                         if (res < 0)
 952                                 continue;
 953                         link = (link || phydev->link);
 954                 }
 955         }
 956         phydev->link = link;
 957
 958         /* Restore CPU interface speed and duplex after it was changed for
 959          * other ports */
 960         phydev->speed = speed;
 961         phydev->duplex = duplex;
 962
 963         return 0;
 964 }
 965
 966 static struct phy_driver mv88e61xx_driver = {
 967         .name = "Marvell MV88E61xx",
 968         .uid = 0x01410eb1,
 969         .mask = 0xfffffff0,
 970         .features = PHY_GBIT_FEATURES,
 971         .probe = mv88e61xx_probe,
 972         .config = mv88e61xx_phy_config,
 973         .startup = mv88e61xx_phy_startup,
 974         .shutdown = &genphy_shutdown,
 975 };
 976
 977 int phy_mv88e61xx_init(void)
 978 {
 979         phy_register(&mv88e61xx_driver);
 980
 981         return 0;
 982 }
 983
 984 /*
 985  * Overload weak get_phy_id definition since we need non-standard functions
 986  * to read PHY registers
 987  */
 988 int get_phy_id(struct mii_dev *bus, int smi_addr, int devad, u32 *phy_id)
 989 {
 990         struct phy_device temp_phy;
 991         struct mv88e61xx_phy_priv temp_priv;
 992         struct mii_dev temp_mii;
 993         int val;
 994
 995         /*
 996          * Buid temporary data structures that the chip reading code needs to
 997          * read the ID
 998          */
 999         temp_priv.mdio_bus = bus;
1000         temp_priv.smi_addr = smi_addr;
1001         temp_phy.priv = &temp_priv;
1002         temp_mii.priv = &temp_phy;
1003
1004         val = mv88e61xx_phy_read_indirect(&temp_mii, 0, devad, MII_PHYSID1);
1005         if (val < 0)
1006                 return -EIO;
1007
1008         *phy_id = val << 16;
1009
1010         val = mv88e61xx_phy_read_indirect(&temp_mii, 0, devad, MII_PHYSID2);
1011         if (val < 0)
1012                 return -EIO;
1013
1014         *phy_id |= (val & 0xffff);
1015
1016         return 0;
1017 }