drivers/net/dsa/mv88e6xxx.c

   1 /*
   2  * net/dsa/mv88e6xxx.c - Marvell 88e6xxx switch chip support
   3  * Copyright (c) 2008 Marvell Semiconductor
   4  *
   5  * Copyright (c) 2015 CMC Electronics, Inc.
   6  *      Added support for VLAN Table Unit operations
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; either version 2 of the License, or
  11  * (at your option) any later version.
  12  */
  13
  14 #include <linux/delay.h>
  15 #include <linux/etherdevice.h>
  16 #include <linux/ethtool.h>
  17 #include <linux/if_bridge.h>
  18 #include <linux/jiffies.h>
  19 #include <linux/list.h>
  20 #include <linux/module.h>
  21 #include <linux/netdevice.h>
  22 #include <linux/phy.h>
  23 #include <net/dsa.h>
  24 #include <net/switchdev.h>
  25 #include "mv88e6xxx.h"
  26
  27 /* MDIO bus access can be nested in the case of PHYs connected to the
  28  * internal MDIO bus of the switch, which is accessed via MDIO bus of
  29  * the Ethernet interface. Avoid lockdep false positives by using
  30  * mutex_lock_nested().
  31  */
  32 static int mv88e6xxx_mdiobus_read(struct mii_bus *bus, int addr, u32 regnum)
  33 {
  34         int ret;
  35
  36         mutex_lock_nested(&bus->mdio_lock, SINGLE_DEPTH_NESTING);
  37         ret = bus->read(bus, addr, regnum);
  38         mutex_unlock(&bus->mdio_lock);
  39
  40         return ret;
  41 }
  42
  43 static int mv88e6xxx_mdiobus_write(struct mii_bus *bus, int addr, u32 regnum,
  44                                    u16 val)
  45 {
  46         int ret;
  47
  48         mutex_lock_nested(&bus->mdio_lock, SINGLE_DEPTH_NESTING);
  49         ret = bus->write(bus, addr, regnum, val);
  50         mutex_unlock(&bus->mdio_lock);
  51
  52         return ret;
  53 }
  54
  55 /* If the switch's ADDR[4:0] strap pins are strapped to zero, it will
  56  * use all 32 SMI bus addresses on its SMI bus, and all switch registers
  57  * will be directly accessible on some {device address,register address}
  58  * pair.  If the ADDR[4:0] pins are not strapped to zero, the switch
  59  * will only respond to SMI transactions to that specific address, and
  60  * an indirect addressing mechanism needs to be used to access its
  61  * registers.
  62  */
  63 static int mv88e6xxx_reg_wait_ready(struct mii_bus *bus, int sw_addr)
  64 {
  65         int ret;
  66         int i;
  67
  68         for (i = 0; i < 16; i++) {
  69                 ret = mv88e6xxx_mdiobus_read(bus, sw_addr, SMI_CMD);
  70                 if (ret < 0)
  71                         return ret;
  72
  73                 if ((ret & SMI_CMD_BUSY) == 0)
  74                         return 0;
  75         }
  76
  77         return -ETIMEDOUT;
  78 }
  79
  80 int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
  81 {
  82         int ret;
  83
  84         if (sw_addr == 0)
  85                 return mv88e6xxx_mdiobus_read(bus, addr, reg);
  86
  87         /* Wait for the bus to become free. */
  88         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
  89         if (ret < 0)
  90                 return ret;
  91
  92         /* Transmit the read command. */
  93         ret = mv88e6xxx_mdiobus_write(bus, sw_addr, SMI_CMD,
  94                                       SMI_CMD_OP_22_READ | (addr << 5) | reg);
  95         if (ret < 0)
  96                 return ret;
  97
  98         /* Wait for the read command to complete. */
  99         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 100         if (ret < 0)
 101                 return ret;
 102
 103         /* Read the data. */
 104         ret = mv88e6xxx_mdiobus_read(bus, sw_addr, SMI_DATA);
 105         if (ret < 0)
 106                 return ret;
 107
 108         return ret & 0xffff;
 109 }
 110
 111 /* Must be called with SMI mutex held */
 112 static int _mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
 113 {
 114         struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
 115         int ret;
 116
 117         if (bus == NULL)
 118                 return -EINVAL;
 119
 120         ret = __mv88e6xxx_reg_read(bus, ds->pd->sw_addr, addr, reg);
 121         if (ret < 0)
 122                 return ret;
 123
 124         dev_dbg(ds->master_dev, "<- addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
 125                 addr, reg, ret);
 126
 127         return ret;
 128 }
 129
 130 int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
 131 {
 132         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 133         int ret;
 134
 135         mutex_lock(&ps->smi_mutex);
 136         ret = _mv88e6xxx_reg_read(ds, addr, reg);
 137         mutex_unlock(&ps->smi_mutex);
 138
 139         return ret;
 140 }
 141
 142 int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 143                           int reg, u16 val)
 144 {
 145         int ret;
 146
 147         if (sw_addr == 0)
 148                 return mv88e6xxx_mdiobus_write(bus, addr, reg, val);
 149
 150         /* Wait for the bus to become free. */
 151         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 152         if (ret < 0)
 153                 return ret;
 154
 155         /* Transmit the data to write. */
 156         ret = mv88e6xxx_mdiobus_write(bus, sw_addr, SMI_DATA, val);
 157         if (ret < 0)
 158                 return ret;
 159
 160         /* Transmit the write command. */
 161         ret = mv88e6xxx_mdiobus_write(bus, sw_addr, SMI_CMD,
 162                                       SMI_CMD_OP_22_WRITE | (addr << 5) | reg);
 163         if (ret < 0)
 164                 return ret;
 165
 166         /* Wait for the write command to complete. */
 167         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 168         if (ret < 0)
 169                 return ret;
 170
 171         return 0;
 172 }
 173
 174 /* Must be called with SMI mutex held */
 175 static int _mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg,
 176                                 u16 val)
 177 {
 178         struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
 179
 180         if (bus == NULL)
 181                 return -EINVAL;
 182
 183         dev_dbg(ds->master_dev, "-> addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
 184                 addr, reg, val);
 185
 186         return __mv88e6xxx_reg_write(bus, ds->pd->sw_addr, addr, reg, val);
 187 }
 188
 189 int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 190 {
 191         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 192         int ret;
 193
 194         mutex_lock(&ps->smi_mutex);
 195         ret = _mv88e6xxx_reg_write(ds, addr, reg, val);
 196         mutex_unlock(&ps->smi_mutex);
 197
 198         return ret;
 199 }
 200
 201 int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr)
 202 {
 203         REG_WRITE(REG_GLOBAL, GLOBAL_MAC_01, (addr[0] << 8) | addr[1]);
 204         REG_WRITE(REG_GLOBAL, GLOBAL_MAC_23, (addr[2] << 8) | addr[3]);
 205         REG_WRITE(REG_GLOBAL, GLOBAL_MAC_45, (addr[4] << 8) | addr[5]);
 206
 207         return 0;
 208 }
 209
 210 int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 211 {
 212         int i;
 213         int ret;
 214
 215         for (i = 0; i < 6; i++) {
 216                 int j;
 217
 218                 /* Write the MAC address byte. */
 219                 REG_WRITE(REG_GLOBAL2, GLOBAL2_SWITCH_MAC,
 220                           GLOBAL2_SWITCH_MAC_BUSY | (i << 8) | addr[i]);
 221
 222                 /* Wait for the write to complete. */
 223                 for (j = 0; j < 16; j++) {
 224                         ret = REG_READ(REG_GLOBAL2, GLOBAL2_SWITCH_MAC);
 225                         if ((ret & GLOBAL2_SWITCH_MAC_BUSY) == 0)
 226                                 break;
 227                 }
 228                 if (j == 16)
 229                         return -ETIMEDOUT;
 230         }
 231
 232         return 0;
 233 }
 234
 235 /* Must be called with SMI mutex held */
 236 static int _mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
 237 {
 238         if (addr >= 0)
 239                 return _mv88e6xxx_reg_read(ds, addr, regnum);
 240         return 0xffff;
 241 }
 242
 243 /* Must be called with SMI mutex held */
 244 static int _mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum,
 245                                 u16 val)
 246 {
 247         if (addr >= 0)
 248                 return _mv88e6xxx_reg_write(ds, addr, regnum, val);
 249         return 0;
 250 }
 251
 252 #ifdef CONFIG_NET_DSA_MV88E6XXX_NEED_PPU
 253 static int mv88e6xxx_ppu_disable(struct dsa_switch *ds)
 254 {
 255         int ret;
 256         unsigned long timeout;
 257
 258         ret = REG_READ(REG_GLOBAL, GLOBAL_CONTROL);
 259         REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL,
 260                   ret & ~GLOBAL_CONTROL_PPU_ENABLE);
 261
 262         timeout = jiffies + 1 * HZ;
 263         while (time_before(jiffies, timeout)) {
 264                 ret = REG_READ(REG_GLOBAL, GLOBAL_STATUS);
 265                 usleep_range(1000, 2000);
 266                 if ((ret & GLOBAL_STATUS_PPU_MASK) !=
 267                     GLOBAL_STATUS_PPU_POLLING)
 268                         return 0;
 269         }
 270
 271         return -ETIMEDOUT;
 272 }
 273
 274 static int mv88e6xxx_ppu_enable(struct dsa_switch *ds)
 275 {
 276         int ret;
 277         unsigned long timeout;
 278
 279         ret = REG_READ(REG_GLOBAL, GLOBAL_CONTROL);
 280         REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL, ret | GLOBAL_CONTROL_PPU_ENABLE);
 281
 282         timeout = jiffies + 1 * HZ;
 283         while (time_before(jiffies, timeout)) {
 284                 ret = REG_READ(REG_GLOBAL, GLOBAL_STATUS);
 285                 usleep_range(1000, 2000);
 286                 if ((ret & GLOBAL_STATUS_PPU_MASK) ==
 287                     GLOBAL_STATUS_PPU_POLLING)
 288                         return 0;
 289         }
 290
 291         return -ETIMEDOUT;
 292 }
 293
 294 static void mv88e6xxx_ppu_reenable_work(struct work_struct *ugly)
 295 {
 296         struct mv88e6xxx_priv_state *ps;
 297
 298         ps = container_of(ugly, struct mv88e6xxx_priv_state, ppu_work);
 299         if (mutex_trylock(&ps->ppu_mutex)) {
 300                 struct dsa_switch *ds = ((struct dsa_switch *)ps) - 1;
 301
 302                 if (mv88e6xxx_ppu_enable(ds) == 0)
 303                         ps->ppu_disabled = 0;
 304                 mutex_unlock(&ps->ppu_mutex);
 305         }
 306 }
 307
 308 static void mv88e6xxx_ppu_reenable_timer(unsigned long _ps)
 309 {
 310         struct mv88e6xxx_priv_state *ps = (void *)_ps;
 311
 312         schedule_work(&ps->ppu_work);
 313 }
 314
 315 static int mv88e6xxx_ppu_access_get(struct dsa_switch *ds)
 316 {
 317         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 318         int ret;
 319
 320         mutex_lock(&ps->ppu_mutex);
 321
 322         /* If the PHY polling unit is enabled, disable it so that
 323          * we can access the PHY registers.  If it was already
 324          * disabled, cancel the timer that is going to re-enable
 325          * it.
 326          */
 327         if (!ps->ppu_disabled) {
 328                 ret = mv88e6xxx_ppu_disable(ds);
 329                 if (ret < 0) {
 330                         mutex_unlock(&ps->ppu_mutex);
 331                         return ret;
 332                 }
 333                 ps->ppu_disabled = 1;
 334         } else {
 335                 del_timer(&ps->ppu_timer);
 336                 ret = 0;
 337         }
 338
 339         return ret;
 340 }
 341
 342 static void mv88e6xxx_ppu_access_put(struct dsa_switch *ds)
 343 {
 344         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 345
 346         /* Schedule a timer to re-enable the PHY polling unit. */
 347         mod_timer(&ps->ppu_timer, jiffies + msecs_to_jiffies(10));
 348         mutex_unlock(&ps->ppu_mutex);
 349 }
 350
 351 void mv88e6xxx_ppu_state_init(struct dsa_switch *ds)
 352 {
 353         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 354
 355         mutex_init(&ps->ppu_mutex);
 356         INIT_WORK(&ps->ppu_work, mv88e6xxx_ppu_reenable_work);
 357         init_timer(&ps->ppu_timer);
 358         ps->ppu_timer.data = (unsigned long)ps;
 359         ps->ppu_timer.function = mv88e6xxx_ppu_reenable_timer;
 360 }
 361
 362 int mv88e6xxx_phy_read_ppu(struct dsa_switch *ds, int addr, int regnum)
 363 {
 364         int ret;
 365
 366         ret = mv88e6xxx_ppu_access_get(ds);
 367         if (ret >= 0) {
 368                 ret = mv88e6xxx_reg_read(ds, addr, regnum);
 369                 mv88e6xxx_ppu_access_put(ds);
 370         }
 371
 372         return ret;
 373 }
 374
 375 int mv88e6xxx_phy_write_ppu(struct dsa_switch *ds, int addr,
 376                             int regnum, u16 val)
 377 {
 378         int ret;
 379
 380         ret = mv88e6xxx_ppu_access_get(ds);
 381         if (ret >= 0) {
 382                 ret = mv88e6xxx_reg_write(ds, addr, regnum, val);
 383                 mv88e6xxx_ppu_access_put(ds);
 384         }
 385
 386         return ret;
 387 }
 388 #endif
 389
 390 static bool mv88e6xxx_6065_family(struct dsa_switch *ds)
 391 {
 392         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 393
 394         switch (ps->id) {
 395         case PORT_SWITCH_ID_6031:
 396         case PORT_SWITCH_ID_6061:
 397         case PORT_SWITCH_ID_6035:
 398         case PORT_SWITCH_ID_6065:
 399                 return true;
 400         }
 401         return false;
 402 }
 403
 404 static bool mv88e6xxx_6095_family(struct dsa_switch *ds)
 405 {
 406         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 407
 408         switch (ps->id) {
 409         case PORT_SWITCH_ID_6092:
 410         case PORT_SWITCH_ID_6095:
 411                 return true;
 412         }
 413         return false;
 414 }
 415
 416 static bool mv88e6xxx_6097_family(struct dsa_switch *ds)
 417 {
 418         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 419
 420         switch (ps->id) {
 421         case PORT_SWITCH_ID_6046:
 422         case PORT_SWITCH_ID_6085:
 423         case PORT_SWITCH_ID_6096:
 424         case PORT_SWITCH_ID_6097:
 425                 return true;
 426         }
 427         return false;
 428 }
 429
 430 static bool mv88e6xxx_6165_family(struct dsa_switch *ds)
 431 {
 432         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 433
 434         switch (ps->id) {
 435         case PORT_SWITCH_ID_6123:
 436         case PORT_SWITCH_ID_6161:
 437         case PORT_SWITCH_ID_6165:
 438                 return true;
 439         }
 440         return false;
 441 }
 442
 443 static bool mv88e6xxx_6185_family(struct dsa_switch *ds)
 444 {
 445         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 446
 447         switch (ps->id) {
 448         case PORT_SWITCH_ID_6121:
 449         case PORT_SWITCH_ID_6122:
 450         case PORT_SWITCH_ID_6152:
 451         case PORT_SWITCH_ID_6155:
 452         case PORT_SWITCH_ID_6182:
 453         case PORT_SWITCH_ID_6185:
 454         case PORT_SWITCH_ID_6108:
 455         case PORT_SWITCH_ID_6131:
 456                 return true;
 457         }
 458         return false;
 459 }
 460
 461 static bool mv88e6xxx_6320_family(struct dsa_switch *ds)
 462 {
 463         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 464
 465         switch (ps->id) {
 466         case PORT_SWITCH_ID_6320:
 467         case PORT_SWITCH_ID_6321:
 468                 return true;
 469         }
 470         return false;
 471 }
 472
 473 static bool mv88e6xxx_6351_family(struct dsa_switch *ds)
 474 {
 475         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 476
 477         switch (ps->id) {
 478         case PORT_SWITCH_ID_6171:
 479         case PORT_SWITCH_ID_6175:
 480         case PORT_SWITCH_ID_6350:
 481         case PORT_SWITCH_ID_6351:
 482                 return true;
 483         }
 484         return false;
 485 }
 486
 487 static bool mv88e6xxx_6352_family(struct dsa_switch *ds)
 488 {
 489         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 490
 491         switch (ps->id) {
 492         case PORT_SWITCH_ID_6172:
 493         case PORT_SWITCH_ID_6176:
 494         case PORT_SWITCH_ID_6240:
 495         case PORT_SWITCH_ID_6352:
 496                 return true;
 497         }
 498         return false;
 499 }
 500
 501 /* We expect the switch to perform auto negotiation if there is a real
 502  * phy. However, in the case of a fixed link phy, we force the port
 503  * settings from the fixed link settings.
 504  */
 505 void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
 506                            struct phy_device *phydev)
 507 {
 508         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 509         u32 reg;
 510         int ret;
 511
 512         if (!phy_is_pseudo_fixed_link(phydev))
 513                 return;
 514
 515         mutex_lock(&ps->smi_mutex);
 516
 517         ret = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_PCS_CTRL);
 518         if (ret < 0)
 519                 goto out;
 520
 521         reg = ret & ~(PORT_PCS_CTRL_LINK_UP |
 522                       PORT_PCS_CTRL_FORCE_LINK |
 523                       PORT_PCS_CTRL_DUPLEX_FULL |
 524                       PORT_PCS_CTRL_FORCE_DUPLEX |
 525                       PORT_PCS_CTRL_UNFORCED);
 526
 527         reg |= PORT_PCS_CTRL_FORCE_LINK;
 528         if (phydev->link)
 529                         reg |= PORT_PCS_CTRL_LINK_UP;
 530
 531         if (mv88e6xxx_6065_family(ds) && phydev->speed > SPEED_100)
 532                 goto out;
 533
 534         switch (phydev->speed) {
 535         case SPEED_1000:
 536                 reg |= PORT_PCS_CTRL_1000;
 537                 break;
 538         case SPEED_100:
 539                 reg |= PORT_PCS_CTRL_100;
 540                 break;
 541         case SPEED_10:
 542                 reg |= PORT_PCS_CTRL_10;
 543                 break;
 544         default:
 545                 pr_info("Unknown speed");
 546                 goto out;
 547         }
 548
 549         reg |= PORT_PCS_CTRL_FORCE_DUPLEX;
 550         if (phydev->duplex == DUPLEX_FULL)
 551                 reg |= PORT_PCS_CTRL_DUPLEX_FULL;
 552
 553         if ((mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds)) &&
 554             (port >= ps->num_ports - 2)) {
 555                 if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID)
 556                         reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK;
 557                 if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)
 558                         reg |= PORT_PCS_CTRL_RGMII_DELAY_TXCLK;
 559                 if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID)
 560                         reg |= (PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
 561                                 PORT_PCS_CTRL_RGMII_DELAY_TXCLK);
 562         }
 563         _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_PCS_CTRL, reg);
 564
 565 out:
 566         mutex_unlock(&ps->smi_mutex);
 567 }
 568
 569 /* Must be called with SMI mutex held */
 570 static int _mv88e6xxx_stats_wait(struct dsa_switch *ds)
 571 {
 572         int ret;
 573         int i;
 574
 575         for (i = 0; i < 10; i++) {
 576                 ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_OP);
 577                 if ((ret & GLOBAL_STATS_OP_BUSY) == 0)
 578                         return 0;
 579         }
 580
 581         return -ETIMEDOUT;
 582 }
 583
 584 /* Must be called with SMI mutex held */
 585 static int _mv88e6xxx_stats_snapshot(struct dsa_switch *ds, int port)
 586 {
 587         int ret;
 588
 589         if (mv88e6xxx_6320_family(ds) || mv88e6xxx_6352_family(ds))
 590                 port = (port + 1) << 5;
 591
 592         /* Snapshot the hardware statistics counters for this port. */
 593         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_STATS_OP,
 594                                    GLOBAL_STATS_OP_CAPTURE_PORT |
 595                                    GLOBAL_STATS_OP_HIST_RX_TX | port);
 596         if (ret < 0)
 597                 return ret;
 598
 599         /* Wait for the snapshotting to complete. */
 600         ret = _mv88e6xxx_stats_wait(ds);
 601         if (ret < 0)
 602                 return ret;
 603
 604         return 0;
 605 }
 606
 607 /* Must be called with SMI mutex held */
 608 static void _mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
 609 {
 610         u32 _val;
 611         int ret;
 612
 613         *val = 0;
 614
 615         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_STATS_OP,
 616                                    GLOBAL_STATS_OP_READ_CAPTURED |
 617                                    GLOBAL_STATS_OP_HIST_RX_TX | stat);
 618         if (ret < 0)
 619                 return;
 620
 621         ret = _mv88e6xxx_stats_wait(ds);
 622         if (ret < 0)
 623                 return;
 624
 625         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_32);
 626         if (ret < 0)
 627                 return;
 628
 629         _val = ret << 16;
 630
 631         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_01);
 632         if (ret < 0)
 633                 return;
 634
 635         *val = _val | ret;
 636 }
 637
 638 static struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
 639         { "in_good_octets", 8, 0x00, },
 640         { "in_bad_octets", 4, 0x02, },
 641         { "in_unicast", 4, 0x04, },
 642         { "in_broadcasts", 4, 0x06, },
 643         { "in_multicasts", 4, 0x07, },
 644         { "in_pause", 4, 0x16, },
 645         { "in_undersize", 4, 0x18, },
 646         { "in_fragments", 4, 0x19, },
 647         { "in_oversize", 4, 0x1a, },
 648         { "in_jabber", 4, 0x1b, },
 649         { "in_rx_error", 4, 0x1c, },
 650         { "in_fcs_error", 4, 0x1d, },
 651         { "out_octets", 8, 0x0e, },
 652         { "out_unicast", 4, 0x10, },
 653         { "out_broadcasts", 4, 0x13, },
 654         { "out_multicasts", 4, 0x12, },
 655         { "out_pause", 4, 0x15, },
 656         { "excessive", 4, 0x11, },
 657         { "collisions", 4, 0x1e, },
 658         { "deferred", 4, 0x05, },
 659         { "single", 4, 0x14, },
 660         { "multiple", 4, 0x17, },
 661         { "out_fcs_error", 4, 0x03, },
 662         { "late", 4, 0x1f, },
 663         { "hist_64bytes", 4, 0x08, },
 664         { "hist_65_127bytes", 4, 0x09, },
 665         { "hist_128_255bytes", 4, 0x0a, },
 666         { "hist_256_511bytes", 4, 0x0b, },
 667         { "hist_512_1023bytes", 4, 0x0c, },
 668         { "hist_1024_max_bytes", 4, 0x0d, },
 669         /* Not all devices have the following counters */
 670         { "sw_in_discards", 4, 0x110, },
 671         { "sw_in_filtered", 2, 0x112, },
 672         { "sw_out_filtered", 2, 0x113, },
 673
 674 };
 675
 676 static bool have_sw_in_discards(struct dsa_switch *ds)
 677 {
 678         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 679
 680         switch (ps->id) {
 681         case PORT_SWITCH_ID_6095: case PORT_SWITCH_ID_6161:
 682         case PORT_SWITCH_ID_6165: case PORT_SWITCH_ID_6171:
 683         case PORT_SWITCH_ID_6172: case PORT_SWITCH_ID_6176:
 684         case PORT_SWITCH_ID_6182: case PORT_SWITCH_ID_6185:
 685         case PORT_SWITCH_ID_6352:
 686                 return true;
 687         default:
 688                 return false;
 689         }
 690 }
 691
 692 static void _mv88e6xxx_get_strings(struct dsa_switch *ds,
 693                                    int nr_stats,
 694                                    struct mv88e6xxx_hw_stat *stats,
 695                                    int port, uint8_t *data)
 696 {
 697         int i;
 698
 699         for (i = 0; i < nr_stats; i++) {
 700                 memcpy(data + i * ETH_GSTRING_LEN,
 701                        stats[i].string, ETH_GSTRING_LEN);
 702         }
 703 }
 704
 705 static uint64_t _mv88e6xxx_get_ethtool_stat(struct dsa_switch *ds,
 706                                             int stat,
 707                                             struct mv88e6xxx_hw_stat *stats,
 708                                             int port)
 709 {
 710         struct mv88e6xxx_hw_stat *s = stats + stat;
 711         u32 low;
 712         u32 high = 0;
 713         int ret;
 714         u64 value;
 715
 716         if (s->reg >= 0x100) {
 717                 ret = _mv88e6xxx_reg_read(ds, REG_PORT(port),
 718                                           s->reg - 0x100);
 719                 if (ret < 0)
 720                         return UINT64_MAX;
 721
 722                 low = ret;
 723                 if (s->sizeof_stat == 4) {
 724                         ret = _mv88e6xxx_reg_read(ds, REG_PORT(port),
 725                                                   s->reg - 0x100 + 1);
 726                         if (ret < 0)
 727                                 return UINT64_MAX;
 728                         high = ret;
 729                 }
 730         } else {
 731                 _mv88e6xxx_stats_read(ds, s->reg, &low);
 732                 if (s->sizeof_stat == 8)
 733                         _mv88e6xxx_stats_read(ds, s->reg + 1, &high);
 734         }
 735         value = (((u64)high) << 16) | low;
 736         return value;
 737 }
 738
 739 static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 740                                          int nr_stats,
 741                                          struct mv88e6xxx_hw_stat *stats,
 742                                          int port, uint64_t *data)
 743 {
 744         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 745         int ret;
 746         int i;
 747
 748         mutex_lock(&ps->smi_mutex);
 749
 750         ret = _mv88e6xxx_stats_snapshot(ds, port);
 751         if (ret < 0) {
 752                 mutex_unlock(&ps->smi_mutex);
 753                 return;
 754         }
 755
 756         /* Read each of the counters. */
 757         for (i = 0; i < nr_stats; i++)
 758                 data[i] = _mv88e6xxx_get_ethtool_stat(ds, i, stats, port);
 759
 760         mutex_unlock(&ps->smi_mutex);
 761 }
 762
 763 /* All the statistics in the table */
 764 void
 765 mv88e6xxx_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
 766 {
 767         if (have_sw_in_discards(ds))
 768                 _mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
 769                                        mv88e6xxx_hw_stats, port, data);
 770         else
 771                 _mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
 772                                        mv88e6xxx_hw_stats, port, data);
 773 }
 774
 775 int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
 776 {
 777         if (have_sw_in_discards(ds))
 778                 return ARRAY_SIZE(mv88e6xxx_hw_stats);
 779         return ARRAY_SIZE(mv88e6xxx_hw_stats) - 3;
 780 }
 781
 782 void
 783 mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 784                             int port, uint64_t *data)
 785 {
 786         if (have_sw_in_discards(ds))
 787                 _mv88e6xxx_get_ethtool_stats(
 788                         ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
 789                         mv88e6xxx_hw_stats, port, data);
 790         else
 791                 _mv88e6xxx_get_ethtool_stats(
 792                         ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
 793                         mv88e6xxx_hw_stats, port, data);
 794 }
 795
 796 int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
 797 {
 798         return 32 * sizeof(u16);
 799 }
 800
 801 void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 802                         struct ethtool_regs *regs, void *_p)
 803 {
 804         u16 *p = _p;
 805         int i;
 806
 807         regs->version = 0;
 808
 809         memset(p, 0xff, 32 * sizeof(u16));
 810
 811         for (i = 0; i < 32; i++) {
 812                 int ret;
 813
 814                 ret = mv88e6xxx_reg_read(ds, REG_PORT(port), i);
 815                 if (ret >= 0)
 816                         p[i] = ret;
 817         }
 818 }
 819
 820 /* Must be called with SMI lock held */
 821 static int _mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset,
 822                            u16 mask)
 823 {
 824         unsigned long timeout = jiffies + HZ / 10;
 825
 826         while (time_before(jiffies, timeout)) {
 827                 int ret;
 828
 829                 ret = _mv88e6xxx_reg_read(ds, reg, offset);
 830                 if (ret < 0)
 831                         return ret;
 832                 if (!(ret & mask))
 833                         return 0;
 834
 835                 usleep_range(1000, 2000);
 836         }
 837         return -ETIMEDOUT;
 838 }
 839
 840 static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
 841 {
 842         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 843         int ret;
 844
 845         mutex_lock(&ps->smi_mutex);
 846         ret = _mv88e6xxx_wait(ds, reg, offset, mask);
 847         mutex_unlock(&ps->smi_mutex);
 848
 849         return ret;
 850 }
 851
 852 static int _mv88e6xxx_phy_wait(struct dsa_switch *ds)
 853 {
 854         return _mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
 855                                GLOBAL2_SMI_OP_BUSY);
 856 }
 857
 858 int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds)
 859 {
 860         return mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
 861                               GLOBAL2_EEPROM_OP_LOAD);
 862 }
 863
 864 int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds)
 865 {
 866         return mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
 867                               GLOBAL2_EEPROM_OP_BUSY);
 868 }
 869
 870 /* Must be called with SMI lock held */
 871 static int _mv88e6xxx_atu_wait(struct dsa_switch *ds)
 872 {
 873         return _mv88e6xxx_wait(ds, REG_GLOBAL, GLOBAL_ATU_OP,
 874                                GLOBAL_ATU_OP_BUSY);
 875 }
 876
 877 /* Must be called with SMI mutex held */
 878 static int _mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr,
 879                                         int regnum)
 880 {
 881         int ret;
 882
 883         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
 884                                    GLOBAL2_SMI_OP_22_READ | (addr << 5) |
 885                                    regnum);
 886         if (ret < 0)
 887                 return ret;
 888
 889         ret = _mv88e6xxx_phy_wait(ds);
 890         if (ret < 0)
 891                 return ret;
 892
 893         return _mv88e6xxx_reg_read(ds, REG_GLOBAL2, GLOBAL2_SMI_DATA);
 894 }
 895
 896 /* Must be called with SMI mutex held */
 897 static int _mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr,
 898                                          int regnum, u16 val)
 899 {
 900         int ret;
 901
 902         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL2, GLOBAL2_SMI_DATA, val);
 903         if (ret < 0)
 904                 return ret;
 905
 906         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
 907                                    GLOBAL2_SMI_OP_22_WRITE | (addr << 5) |
 908                                    regnum);
 909
 910         return _mv88e6xxx_phy_wait(ds);
 911 }
 912
 913 int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
 914 {
 915         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 916         int reg;
 917
 918         mutex_lock(&ps->smi_mutex);
 919
 920         reg = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 921         if (reg < 0)
 922                 goto out;
 923
 924         e->eee_enabled = !!(reg & 0x0200);
 925         e->tx_lpi_enabled = !!(reg & 0x0100);
 926
 927         reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_STATUS);
 928         if (reg < 0)
 929                 goto out;
 930
 931         e->eee_active = !!(reg & PORT_STATUS_EEE);
 932         reg = 0;
 933
 934 out:
 935         mutex_unlock(&ps->smi_mutex);
 936         return reg;
 937 }
 938
 939 int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
 940                       struct phy_device *phydev, struct ethtool_eee *e)
 941 {
 942         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 943         int reg;
 944         int ret;
 945
 946         mutex_lock(&ps->smi_mutex);
 947
 948         ret = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 949         if (ret < 0)
 950                 goto out;
 951
 952         reg = ret & ~0x0300;
 953         if (e->eee_enabled)
 954                 reg |= 0x0200;
 955         if (e->tx_lpi_enabled)
 956                 reg |= 0x0100;
 957
 958         ret = _mv88e6xxx_phy_write_indirect(ds, port, 16, reg);
 959 out:
 960         mutex_unlock(&ps->smi_mutex);
 961
 962         return ret;
 963 }
 964
 965 static int _mv88e6xxx_atu_cmd(struct dsa_switch *ds, u16 cmd)
 966 {
 967         int ret;
 968
 969         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_OP, cmd);
 970         if (ret < 0)
 971                 return ret;
 972
 973         return _mv88e6xxx_atu_wait(ds);
 974 }
 975
 976 static int _mv88e6xxx_atu_data_write(struct dsa_switch *ds,
 977                                      struct mv88e6xxx_atu_entry *entry)
 978 {
 979         u16 data = entry->state & GLOBAL_ATU_DATA_STATE_MASK;
 980
 981         if (entry->state != GLOBAL_ATU_DATA_STATE_UNUSED) {
 982                 unsigned int mask, shift;
 983
 984                 if (entry->trunk) {
 985                         data |= GLOBAL_ATU_DATA_TRUNK;
 986                         mask = GLOBAL_ATU_DATA_TRUNK_ID_MASK;
 987                         shift = GLOBAL_ATU_DATA_TRUNK_ID_SHIFT;
 988                 } else {
 989                         mask = GLOBAL_ATU_DATA_PORT_VECTOR_MASK;
 990                         shift = GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT;
 991                 }
 992
 993                 data |= (entry->portv_trunkid << shift) & mask;
 994         }
 995
 996         return _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_DATA, data);
 997 }
 998
 999 static int _mv88e6xxx_atu_flush_move(struct dsa_switch *ds,
1000                                      struct mv88e6xxx_atu_entry *entry,
1001                                      bool static_too)
1002 {
1003         int op;
1004         int err;
1005
1006         err = _mv88e6xxx_atu_wait(ds);
1007         if (err)
1008                 return err;
1009
1010         err = _mv88e6xxx_atu_data_write(ds, entry);
1011         if (err)
1012                 return err;
1013
1014         if (entry->fid) {
1015                 err = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_FID,
1016                                            entry->fid);
1017                 if (err)
1018                         return err;
1019
1020                 op = static_too ? GLOBAL_ATU_OP_FLUSH_MOVE_ALL_DB :
1021                         GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC_DB;
1022         } else {
1023                 op = static_too ? GLOBAL_ATU_OP_FLUSH_MOVE_ALL :
1024                         GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC;
1025         }
1026
1027         return _mv88e6xxx_atu_cmd(ds, op);
1028 }
1029
1030 static int _mv88e6xxx_atu_flush(struct dsa_switch *ds, u16 fid, bool static_too)
1031 {
1032         struct mv88e6xxx_atu_entry entry = {
1033                 .fid = fid,
1034                 .state = 0, /* EntryState bits must be 0 */
1035         };
1036
1037         return _mv88e6xxx_atu_flush_move(ds, &entry, static_too);
1038 }
1039
1040 static int _mv88e6xxx_atu_move(struct dsa_switch *ds, u16 fid, int from_port,
1041                                int to_port, bool static_too)
1042 {
1043         struct mv88e6xxx_atu_entry entry = {
1044                 .trunk = false,
1045                 .fid = fid,
1046         };
1047
1048         /* EntryState bits must be 0xF */
1049         entry.state = GLOBAL_ATU_DATA_STATE_MASK;
1050
1051         /* ToPort and FromPort are respectively in PortVec bits 7:4 and 3:0 */
1052         entry.portv_trunkid = (to_port & 0x0f) << 4;
1053         entry.portv_trunkid |= from_port & 0x0f;
1054
1055         return _mv88e6xxx_atu_flush_move(ds, &entry, static_too);
1056 }
1057
1058 static int _mv88e6xxx_atu_remove(struct dsa_switch *ds, u16 fid, int port,
1059                                  bool static_too)
1060 {
1061         /* Destination port 0xF means remove the entries */
1062         return _mv88e6xxx_atu_move(ds, fid, port, 0x0f, static_too);
1063 }
1064
1065 static int mv88e6xxx_set_port_state(struct dsa_switch *ds, int port, u8 state)
1066 {
1067         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1068         int reg, ret = 0;
1069         u8 oldstate;
1070
1071         mutex_lock(&ps->smi_mutex);
1072
1073         reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_CONTROL);
1074         if (reg < 0) {
1075                 ret = reg;
1076                 goto abort;
1077         }
1078
1079         oldstate = reg & PORT_CONTROL_STATE_MASK;
1080         if (oldstate != state) {
1081                 /* Flush forwarding database if we're moving a port
1082                  * from Learning or Forwarding state to Disabled or
1083                  * Blocking or Listening state.
1084                  */
1085                 if (oldstate >= PORT_CONTROL_STATE_LEARNING &&
1086                     state <= PORT_CONTROL_STATE_BLOCKING) {
1087                         ret = _mv88e6xxx_atu_remove(ds, 0, port, false);
1088                         if (ret)
1089                                 goto abort;
1090                 }
1091                 reg = (reg & ~PORT_CONTROL_STATE_MASK) | state;
1092                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_CONTROL,
1093                                            reg);
1094         }
1095
1096 abort:
1097         mutex_unlock(&ps->smi_mutex);
1098         return ret;
1099 }
1100
1101 static int _mv88e6xxx_port_vlan_map_set(struct dsa_switch *ds, int port,
1102                                         u16 output_ports)
1103 {
1104         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1105         const u16 mask = (1 << ps->num_ports) - 1;
1106         int reg;
1107
1108         reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_BASE_VLAN);
1109         if (reg < 0)
1110                 return reg;
1111
1112         reg &= ~mask;
1113         reg |= output_ports & mask;
1114
1115         return _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_BASE_VLAN, reg);
1116 }
1117
1118 int mv88e6xxx_port_stp_update(struct dsa_switch *ds, int port, u8 state)
1119 {
1120         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1121         int stp_state;
1122
1123         switch (state) {
1124         case BR_STATE_DISABLED:
1125                 stp_state = PORT_CONTROL_STATE_DISABLED;
1126                 break;
1127         case BR_STATE_BLOCKING:
1128         case BR_STATE_LISTENING:
1129                 stp_state = PORT_CONTROL_STATE_BLOCKING;
1130                 break;
1131         case BR_STATE_LEARNING:
1132                 stp_state = PORT_CONTROL_STATE_LEARNING;
1133                 break;
1134         case BR_STATE_FORWARDING:
1135         default:
1136                 stp_state = PORT_CONTROL_STATE_FORWARDING;
1137                 break;
1138         }
1139
1140         netdev_dbg(ds->ports[port], "port state %d [%d]\n", state, stp_state);
1141
1142         /* mv88e6xxx_port_stp_update may be called with softirqs disabled,
1143          * so we can not update the port state directly but need to schedule it.
1144          */
1145         ps->port_state[port] = stp_state;
1146         set_bit(port, &ps->port_state_update_mask);
1147         schedule_work(&ps->bridge_work);
1148
1149         return 0;
1150 }
1151
1152 int mv88e6xxx_port_pvid_get(struct dsa_switch *ds, int port, u16 *pvid)
1153 {
1154         int ret;
1155
1156         ret = mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_DEFAULT_VLAN);
1157         if (ret < 0)
1158                 return ret;
1159
1160         *pvid = ret & PORT_DEFAULT_VLAN_MASK;
1161
1162         return 0;
1163 }
1164
1165 int mv88e6xxx_port_pvid_set(struct dsa_switch *ds, int port, u16 pvid)
1166 {
1167         return mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_DEFAULT_VLAN,
1168                                    pvid & PORT_DEFAULT_VLAN_MASK);
1169 }
1170
1171 static int _mv88e6xxx_vtu_wait(struct dsa_switch *ds)
1172 {
1173         return _mv88e6xxx_wait(ds, REG_GLOBAL, GLOBAL_VTU_OP,
1174                                GLOBAL_VTU_OP_BUSY);
1175 }
1176
1177 static int _mv88e6xxx_vtu_cmd(struct dsa_switch *ds, u16 op)
1178 {
1179         int ret;
1180
1181         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_OP, op);
1182         if (ret < 0)
1183                 return ret;
1184
1185         return _mv88e6xxx_vtu_wait(ds);
1186 }
1187
1188 static int _mv88e6xxx_vtu_stu_flush(struct dsa_switch *ds)
1189 {
1190         int ret;
1191
1192         ret = _mv88e6xxx_vtu_wait(ds);
1193         if (ret < 0)
1194                 return ret;
1195
1196         return _mv88e6xxx_vtu_cmd(ds, GLOBAL_VTU_OP_FLUSH_ALL);
1197 }
1198
1199 static int _mv88e6xxx_vtu_stu_data_read(struct dsa_switch *ds,
1200                                         struct mv88e6xxx_vtu_stu_entry *entry,
1201                                         unsigned int nibble_offset)
1202 {
1203         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1204         u16 regs[3];
1205         int i;
1206         int ret;
1207
1208         for (i = 0; i < 3; ++i) {
1209                 ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL,
1210                                           GLOBAL_VTU_DATA_0_3 + i);
1211                 if (ret < 0)
1212                         return ret;
1213
1214                 regs[i] = ret;
1215         }
1216
1217         for (i = 0; i < ps->num_ports; ++i) {
1218                 unsigned int shift = (i % 4) * 4 + nibble_offset;
1219                 u16 reg = regs[i / 4];
1220
1221                 entry->data[i] = (reg >> shift) & GLOBAL_VTU_STU_DATA_MASK;
1222         }
1223
1224         return 0;
1225 }
1226
1227 static int _mv88e6xxx_vtu_stu_data_write(struct dsa_switch *ds,
1228                                          struct mv88e6xxx_vtu_stu_entry *entry,
1229                                          unsigned int nibble_offset)
1230 {
1231         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1232         u16 regs[3] = { 0 };
1233         int i;
1234         int ret;
1235
1236         for (i = 0; i < ps->num_ports; ++i) {
1237                 unsigned int shift = (i % 4) * 4 + nibble_offset;
1238                 u8 data = entry->data[i];
1239
1240                 regs[i / 4] |= (data & GLOBAL_VTU_STU_DATA_MASK) << shift;
1241         }
1242
1243         for (i = 0; i < 3; ++i) {
1244                 ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL,
1245                                            GLOBAL_VTU_DATA_0_3 + i, regs[i]);
1246                 if (ret < 0)
1247                         return ret;
1248         }
1249
1250         return 0;
1251 }
1252
1253 static int _mv88e6xxx_vtu_vid_write(struct dsa_switch *ds, u16 vid)
1254 {
1255         return _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_VID,
1256                                     vid & GLOBAL_VTU_VID_MASK);
1257 }
1258
1259 static int _mv88e6xxx_vtu_getnext(struct dsa_switch *ds,
1260                                   struct mv88e6xxx_vtu_stu_entry *entry)
1261 {
1262         struct mv88e6xxx_vtu_stu_entry next = { 0 };
1263         int ret;
1264
1265         ret = _mv88e6xxx_vtu_wait(ds);
1266         if (ret < 0)
1267                 return ret;
1268
1269         ret = _mv88e6xxx_vtu_cmd(ds, GLOBAL_VTU_OP_VTU_GET_NEXT);
1270         if (ret < 0)
1271                 return ret;
1272
1273         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_VTU_VID);
1274         if (ret < 0)
1275                 return ret;
1276
1277         next.vid = ret & GLOBAL_VTU_VID_MASK;
1278         next.valid = !!(ret & GLOBAL_VTU_VID_VALID);
1279
1280         if (next.valid) {
1281                 ret = _mv88e6xxx_vtu_stu_data_read(ds, &next, 0);
1282                 if (ret < 0)
1283                         return ret;
1284
1285                 if (mv88e6xxx_6097_family(ds) || mv88e6xxx_6165_family(ds) ||
1286                     mv88e6xxx_6351_family(ds) || mv88e6xxx_6352_family(ds)) {
1287                         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL,
1288                                                   GLOBAL_VTU_FID);
1289                         if (ret < 0)
1290                                 return ret;
1291
1292                         next.fid = ret & GLOBAL_VTU_FID_MASK;
1293
1294                         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL,
1295                                                   GLOBAL_VTU_SID);
1296                         if (ret < 0)
1297                                 return ret;
1298
1299                         next.sid = ret & GLOBAL_VTU_SID_MASK;
1300                 }
1301         }
1302
1303         *entry = next;
1304         return 0;
1305 }
1306
1307 static int _mv88e6xxx_vtu_loadpurge(struct dsa_switch *ds,
1308                                     struct mv88e6xxx_vtu_stu_entry *entry)
1309 {
1310         u16 reg = 0;
1311         int ret;
1312
1313         ret = _mv88e6xxx_vtu_wait(ds);
1314         if (ret < 0)
1315                 return ret;
1316
1317         if (!entry->valid)
1318                 goto loadpurge;
1319
1320         /* Write port member tags */
1321         ret = _mv88e6xxx_vtu_stu_data_write(ds, entry, 0);
1322         if (ret < 0)
1323                 return ret;
1324
1325         if (mv88e6xxx_6097_family(ds) || mv88e6xxx_6165_family(ds) ||
1326             mv88e6xxx_6351_family(ds) || mv88e6xxx_6352_family(ds)) {
1327                 reg = entry->sid & GLOBAL_VTU_SID_MASK;
1328                 ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_SID, reg);
1329                 if (ret < 0)
1330                         return ret;
1331
1332                 reg = entry->fid & GLOBAL_VTU_FID_MASK;
1333                 ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_FID, reg);
1334                 if (ret < 0)
1335                         return ret;
1336         }
1337
1338         reg = GLOBAL_VTU_VID_VALID;
1339 loadpurge:
1340         reg |= entry->vid & GLOBAL_VTU_VID_MASK;
1341         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_VID, reg);
1342         if (ret < 0)
1343                 return ret;
1344
1345         return _mv88e6xxx_vtu_cmd(ds, GLOBAL_VTU_OP_VTU_LOAD_PURGE);
1346 }
1347
1348 static int _mv88e6xxx_stu_getnext(struct dsa_switch *ds, u8 sid,
1349                                   struct mv88e6xxx_vtu_stu_entry *entry)
1350 {
1351         struct mv88e6xxx_vtu_stu_entry next = { 0 };
1352         int ret;
1353
1354         ret = _mv88e6xxx_vtu_wait(ds);
1355         if (ret < 0)
1356                 return ret;
1357
1358         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_SID,
1359                                    sid & GLOBAL_VTU_SID_MASK);
1360         if (ret < 0)
1361                 return ret;
1362
1363         ret = _mv88e6xxx_vtu_cmd(ds, GLOBAL_VTU_OP_STU_GET_NEXT);
1364         if (ret < 0)
1365                 return ret;
1366
1367         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_VTU_SID);
1368         if (ret < 0)
1369                 return ret;
1370
1371         next.sid = ret & GLOBAL_VTU_SID_MASK;
1372
1373         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_VTU_VID);
1374         if (ret < 0)
1375                 return ret;
1376
1377         next.valid = !!(ret & GLOBAL_VTU_VID_VALID);
1378
1379         if (next.valid) {
1380                 ret = _mv88e6xxx_vtu_stu_data_read(ds, &next, 2);
1381                 if (ret < 0)
1382                         return ret;
1383         }
1384
1385         *entry = next;
1386         return 0;
1387 }
1388
1389 static int _mv88e6xxx_stu_loadpurge(struct dsa_switch *ds,
1390                                     struct mv88e6xxx_vtu_stu_entry *entry)
1391 {
1392         u16 reg = 0;
1393         int ret;
1394
1395         ret = _mv88e6xxx_vtu_wait(ds);
1396         if (ret < 0)
1397                 return ret;
1398
1399         if (!entry->valid)
1400                 goto loadpurge;
1401
1402         /* Write port states */
1403         ret = _mv88e6xxx_vtu_stu_data_write(ds, entry, 2);
1404         if (ret < 0)
1405                 return ret;
1406
1407         reg = GLOBAL_VTU_VID_VALID;
1408 loadpurge:
1409         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_VID, reg);
1410         if (ret < 0)
1411                 return ret;
1412
1413         reg = entry->sid & GLOBAL_VTU_SID_MASK;
1414         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_VTU_SID, reg);
1415         if (ret < 0)
1416                 return ret;
1417
1418         return _mv88e6xxx_vtu_cmd(ds, GLOBAL_VTU_OP_STU_LOAD_PURGE);
1419 }
1420
1421 static int _mv88e6xxx_vlan_init(struct dsa_switch *ds, u16 vid,
1422                                 struct mv88e6xxx_vtu_stu_entry *entry)
1423 {
1424         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1425         struct mv88e6xxx_vtu_stu_entry vlan = {
1426                 .valid = true,
1427                 .vid = vid,
1428                 .fid = vid, /* We use one FID per VLAN */
1429         };
1430         int i;
1431
1432         /* exclude all ports except the CPU */
1433         for (i = 0; i < ps->num_ports; ++i)
1434                 vlan.data[i] = dsa_is_cpu_port(ds, i) ?
1435                         GLOBAL_VTU_DATA_MEMBER_TAG_TAGGED :
1436                         GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
1437
1438         if (mv88e6xxx_6097_family(ds) || mv88e6xxx_6165_family(ds) ||
1439             mv88e6xxx_6351_family(ds) || mv88e6xxx_6352_family(ds)) {
1440                 struct mv88e6xxx_vtu_stu_entry vstp;
1441                 int err;
1442
1443                 /* Adding a VTU entry requires a valid STU entry. As VSTP is not
1444                  * implemented, only one STU entry is needed to cover all VTU
1445                  * entries. Thus, validate the SID 0.
1446                  */
1447                 vlan.sid = 0;
1448                 err = _mv88e6xxx_stu_getnext(ds, GLOBAL_VTU_SID_MASK, &vstp);
1449                 if (err)
1450                         return err;
1451
1452                 if (vstp.sid != vlan.sid || !vstp.valid) {
1453                         memset(&vstp, 0, sizeof(vstp));
1454                         vstp.valid = true;
1455                         vstp.sid = vlan.sid;
1456
1457                         err = _mv88e6xxx_stu_loadpurge(ds, &vstp);
1458                         if (err)
1459                                 return err;
1460                 }
1461
1462                 /* Clear all MAC addresses from the new database */
1463                 err = _mv88e6xxx_atu_flush(ds, vlan.fid, true);
1464                 if (err)
1465                         return err;
1466         }
1467
1468         *entry = vlan;
1469         return 0;
1470 }
1471
1472 int mv88e6xxx_port_vlan_add(struct dsa_switch *ds, int port, u16 vid,
1473                             bool untagged)
1474 {
1475         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1476         struct mv88e6xxx_vtu_stu_entry vlan;
1477         int err;
1478
1479         mutex_lock(&ps->smi_mutex);
1480
1481         err = _mv88e6xxx_vtu_vid_write(ds, vid - 1);
1482         if (err)
1483                 goto unlock;
1484
1485         err = _mv88e6xxx_vtu_getnext(ds, &vlan);
1486         if (err)
1487                 goto unlock;
1488
1489         if (vlan.vid != vid || !vlan.valid) {
1490                 err = _mv88e6xxx_vlan_init(ds, vid, &vlan);
1491                 if (err)
1492                         goto unlock;
1493         }
1494
1495         vlan.data[port] = untagged ?
1496                 GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED :
1497                 GLOBAL_VTU_DATA_MEMBER_TAG_TAGGED;
1498
1499         err = _mv88e6xxx_vtu_loadpurge(ds, &vlan);
1500 unlock:
1501         mutex_unlock(&ps->smi_mutex);
1502
1503         return err;
1504 }
1505
1506 int mv88e6xxx_port_vlan_del(struct dsa_switch *ds, int port, u16 vid)
1507 {
1508         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1509         struct mv88e6xxx_vtu_stu_entry vlan;
1510         int i, err;
1511
1512         mutex_lock(&ps->smi_mutex);
1513
1514         err = _mv88e6xxx_vtu_vid_write(ds, vid - 1);
1515         if (err)
1516                 goto unlock;
1517
1518         err = _mv88e6xxx_vtu_getnext(ds, &vlan);
1519         if (err)
1520                 goto unlock;
1521
1522         if (vlan.vid != vid || !vlan.valid ||
1523             vlan.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER) {
1524                 err = -ENOENT;
1525                 goto unlock;
1526         }
1527
1528         vlan.data[port] = GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
1529
1530         /* keep the VLAN unless all ports are excluded */
1531         vlan.valid = false;
1532         for (i = 0; i < ps->num_ports; ++i) {
1533                 if (dsa_is_cpu_port(ds, i))
1534                         continue;
1535
1536                 if (vlan.data[i] != GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER) {
1537                         vlan.valid = true;
1538                         break;
1539                 }
1540         }
1541
1542         err = _mv88e6xxx_vtu_loadpurge(ds, &vlan);
1543         if (err)
1544                 goto unlock;
1545
1546         err = _mv88e6xxx_atu_remove(ds, vlan.fid, port, false);
1547 unlock:
1548         mutex_unlock(&ps->smi_mutex);
1549
1550         return err;
1551 }
1552
1553 int mv88e6xxx_vlan_getnext(struct dsa_switch *ds, u16 *vid,
1554                            unsigned long *ports, unsigned long *untagged)
1555 {
1556         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1557         struct mv88e6xxx_vtu_stu_entry next;
1558         int port;
1559         int err;
1560
1561         if (*vid == 4095)
1562                 return -ENOENT;
1563
1564         mutex_lock(&ps->smi_mutex);
1565         err = _mv88e6xxx_vtu_vid_write(ds, *vid);
1566         if (err)
1567                 goto unlock;
1568
1569         err = _mv88e6xxx_vtu_getnext(ds, &next);
1570 unlock:
1571         mutex_unlock(&ps->smi_mutex);
1572
1573         if (err)
1574                 return err;
1575
1576         if (!next.valid)
1577                 return -ENOENT;
1578
1579         *vid = next.vid;
1580
1581         for (port = 0; port < ps->num_ports; ++port) {
1582                 clear_bit(port, ports);
1583                 clear_bit(port, untagged);
1584
1585                 if (dsa_is_cpu_port(ds, port))
1586                         continue;
1587
1588                 if (next.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_TAGGED ||
1589                     next.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED)
1590                         set_bit(port, ports);
1591
1592                 if (next.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED)
1593                         set_bit(port, untagged);
1594         }
1595
1596         return 0;
1597 }
1598
1599 static int _mv88e6xxx_atu_mac_write(struct dsa_switch *ds,
1600                                     const unsigned char *addr)
1601 {
1602         int i, ret;
1603
1604         for (i = 0; i < 3; i++) {
1605                 ret = _mv88e6xxx_reg_write(
1606                         ds, REG_GLOBAL, GLOBAL_ATU_MAC_01 + i,
1607                         (addr[i * 2] << 8) | addr[i * 2 + 1]);
1608                 if (ret < 0)
1609                         return ret;
1610         }
1611
1612         return 0;
1613 }
1614
1615 static int _mv88e6xxx_atu_mac_read(struct dsa_switch *ds, unsigned char *addr)
1616 {
1617         int i, ret;
1618
1619         for (i = 0; i < 3; i++) {
1620                 ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL,
1621                                           GLOBAL_ATU_MAC_01 + i);
1622                 if (ret < 0)
1623                         return ret;
1624                 addr[i * 2] = ret >> 8;
1625                 addr[i * 2 + 1] = ret & 0xff;
1626         }
1627
1628         return 0;
1629 }
1630
1631 static int _mv88e6xxx_atu_load(struct dsa_switch *ds,
1632                                struct mv88e6xxx_atu_entry *entry)
1633 {
1634         int ret;
1635
1636         ret = _mv88e6xxx_atu_wait(ds);
1637         if (ret < 0)
1638                 return ret;
1639
1640         ret = _mv88e6xxx_atu_mac_write(ds, entry->mac);
1641         if (ret < 0)
1642                 return ret;
1643
1644         ret = _mv88e6xxx_atu_data_write(ds, entry);
1645         if (ret < 0)
1646                 return ret;
1647
1648         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_FID, entry->fid);
1649         if (ret < 0)
1650                 return ret;
1651
1652         return _mv88e6xxx_atu_cmd(ds, GLOBAL_ATU_OP_LOAD_DB);
1653 }
1654
1655 static int _mv88e6xxx_port_fdb_load(struct dsa_switch *ds, int port,
1656                                     const unsigned char *addr, u16 vid,
1657                                     u8 state)
1658 {
1659         struct mv88e6xxx_atu_entry entry = { 0 };
1660
1661         entry.fid = vid; /* We use one FID per VLAN */
1662         entry.state = state;
1663         ether_addr_copy(entry.mac, addr);
1664         if (state != GLOBAL_ATU_DATA_STATE_UNUSED) {
1665                 entry.trunk = false;
1666                 entry.portv_trunkid = BIT(port);
1667         }
1668
1669         return _mv88e6xxx_atu_load(ds, &entry);
1670 }
1671
1672 int mv88e6xxx_port_fdb_prepare(struct dsa_switch *ds, int port,
1673                                const struct switchdev_obj_port_fdb *fdb,
1674                                struct switchdev_trans *trans)
1675 {
1676         /* We don't use per-port FDB */
1677         if (fdb->vid == 0)
1678                 return -EOPNOTSUPP;
1679
1680         /* We don't need any dynamic resource from the kernel (yet),
1681          * so skip the prepare phase.
1682          */
1683         return 0;
1684 }
1685
1686 int mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
1687                            const struct switchdev_obj_port_fdb *fdb,
1688                            struct switchdev_trans *trans)
1689 {
1690         int state = is_multicast_ether_addr(fdb->addr) ?
1691                 GLOBAL_ATU_DATA_STATE_MC_STATIC :
1692                 GLOBAL_ATU_DATA_STATE_UC_STATIC;
1693         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1694         int ret;
1695
1696         mutex_lock(&ps->smi_mutex);
1697         ret = _mv88e6xxx_port_fdb_load(ds, port, fdb->addr, fdb->vid, state);
1698         mutex_unlock(&ps->smi_mutex);
1699
1700         return ret;
1701 }
1702
1703 int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
1704                            const struct switchdev_obj_port_fdb *fdb)
1705 {
1706         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1707         int ret;
1708
1709         mutex_lock(&ps->smi_mutex);
1710         ret = _mv88e6xxx_port_fdb_load(ds, port, fdb->addr, fdb->vid,
1711                                        GLOBAL_ATU_DATA_STATE_UNUSED);
1712         mutex_unlock(&ps->smi_mutex);
1713
1714         return ret;
1715 }
1716
1717 static int _mv88e6xxx_atu_getnext(struct dsa_switch *ds, u16 fid,
1718                                   struct mv88e6xxx_atu_entry *entry)
1719 {
1720         struct mv88e6xxx_atu_entry next = { 0 };
1721         int ret;
1722
1723         next.fid = fid;
1724
1725         ret = _mv88e6xxx_atu_wait(ds);
1726         if (ret < 0)
1727                 return ret;
1728
1729         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_ATU_FID, fid);
1730         if (ret < 0)
1731                 return ret;
1732
1733         ret = _mv88e6xxx_atu_cmd(ds, GLOBAL_ATU_OP_GET_NEXT_DB);
1734         if (ret < 0)
1735                 return ret;
1736
1737         ret = _mv88e6xxx_atu_mac_read(ds, next.mac);
1738         if (ret < 0)
1739                 return ret;
1740
1741         ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_ATU_DATA);
1742         if (ret < 0)
1743                 return ret;
1744
1745         next.state = ret & GLOBAL_ATU_DATA_STATE_MASK;
1746         if (next.state != GLOBAL_ATU_DATA_STATE_UNUSED) {
1747                 unsigned int mask, shift;
1748
1749                 if (ret & GLOBAL_ATU_DATA_TRUNK) {
1750                         next.trunk = true;
1751                         mask = GLOBAL_ATU_DATA_TRUNK_ID_MASK;
1752                         shift = GLOBAL_ATU_DATA_TRUNK_ID_SHIFT;
1753                 } else {
1754                         next.trunk = false;
1755                         mask = GLOBAL_ATU_DATA_PORT_VECTOR_MASK;
1756                         shift = GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT;
1757                 }
1758
1759                 next.portv_trunkid = (ret & mask) >> shift;
1760         }
1761
1762         *entry = next;
1763         return 0;
1764 }
1765
1766 int mv88e6xxx_port_fdb_dump(struct dsa_switch *ds, int port,
1767                             struct switchdev_obj_port_fdb *fdb,
1768                             int (*cb)(struct switchdev_obj *obj))
1769 {
1770         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1771         struct mv88e6xxx_vtu_stu_entry vlan = {
1772                 .vid = GLOBAL_VTU_VID_MASK, /* all ones */
1773         };
1774         int err;
1775
1776         mutex_lock(&ps->smi_mutex);
1777
1778         err = _mv88e6xxx_vtu_vid_write(ds, vlan.vid);
1779         if (err)
1780                 goto unlock;
1781
1782         do {
1783                 struct mv88e6xxx_atu_entry addr = {
1784                         .mac = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff },
1785                 };
1786
1787                 err = _mv88e6xxx_vtu_getnext(ds, &vlan);
1788                 if (err)
1789                         goto unlock;
1790
1791                 if (!vlan.valid)
1792                         break;
1793
1794                 err = _mv88e6xxx_atu_mac_write(ds, addr.mac);
1795                 if (err)
1796                         goto unlock;
1797
1798                 do {
1799                         err = _mv88e6xxx_atu_getnext(ds, vlan.fid, &addr);
1800                         if (err)
1801                                 goto unlock;
1802
1803                         if (addr.state == GLOBAL_ATU_DATA_STATE_UNUSED)
1804                                 break;
1805
1806                         if (!addr.trunk && addr.portv_trunkid & BIT(port)) {
1807                                 bool is_static = addr.state ==
1808                                         (is_multicast_ether_addr(addr.mac) ?
1809                                          GLOBAL_ATU_DATA_STATE_MC_STATIC :
1810                                          GLOBAL_ATU_DATA_STATE_UC_STATIC);
1811
1812                                 fdb->vid = vlan.vid;
1813                                 ether_addr_copy(fdb->addr, addr.mac);
1814                                 fdb->ndm_state = is_static ? NUD_NOARP :
1815                                         NUD_REACHABLE;
1816
1817                                 err = cb(&fdb->obj);
1818                                 if (err)
1819                                         goto unlock;
1820                         }
1821                 } while (!is_broadcast_ether_addr(addr.mac));
1822
1823         } while (vlan.vid < GLOBAL_VTU_VID_MASK);
1824
1825 unlock:
1826         mutex_unlock(&ps->smi_mutex);
1827
1828         return err;
1829 }
1830
1831 static void mv88e6xxx_bridge_work(struct work_struct *work)
1832 {
1833         struct mv88e6xxx_priv_state *ps;
1834         struct dsa_switch *ds;
1835         int port;
1836
1837         ps = container_of(work, struct mv88e6xxx_priv_state, bridge_work);
1838         ds = ((struct dsa_switch *)ps) - 1;
1839
1840         while (ps->port_state_update_mask) {
1841                 port = __ffs(ps->port_state_update_mask);
1842                 clear_bit(port, &ps->port_state_update_mask);
1843                 mv88e6xxx_set_port_state(ds, port, ps->port_state[port]);
1844         }
1845 }
1846
1847 static int mv88e6xxx_setup_port(struct dsa_switch *ds, int port)
1848 {
1849         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1850         int ret;
1851         u16 reg;
1852
1853         mutex_lock(&ps->smi_mutex);
1854
1855         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1856             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
1857             mv88e6xxx_6185_family(ds) || mv88e6xxx_6095_family(ds) ||
1858             mv88e6xxx_6065_family(ds) || mv88e6xxx_6320_family(ds)) {
1859                 /* MAC Forcing register: don't force link, speed,
1860                  * duplex or flow control state to any particular
1861                  * values on physical ports, but force the CPU port
1862                  * and all DSA ports to their maximum bandwidth and
1863                  * full duplex.
1864                  */
1865                 reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_PCS_CTRL);
1866                 if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
1867                         reg &= ~PORT_PCS_CTRL_UNFORCED;
1868                         reg |= PORT_PCS_CTRL_FORCE_LINK |
1869                                 PORT_PCS_CTRL_LINK_UP |
1870                                 PORT_PCS_CTRL_DUPLEX_FULL |
1871                                 PORT_PCS_CTRL_FORCE_DUPLEX;
1872                         if (mv88e6xxx_6065_family(ds))
1873                                 reg |= PORT_PCS_CTRL_100;
1874                         else
1875                                 reg |= PORT_PCS_CTRL_1000;
1876                 } else {
1877                         reg |= PORT_PCS_CTRL_UNFORCED;
1878                 }
1879
1880                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
1881                                            PORT_PCS_CTRL, reg);
1882                 if (ret)
1883                         goto abort;
1884         }
1885
1886         /* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
1887          * disable Header mode, enable IGMP/MLD snooping, disable VLAN
1888          * tunneling, determine priority by looking at 802.1p and IP
1889          * priority fields (IP prio has precedence), and set STP state
1890          * to Forwarding.
1891          *
1892          * If this is the CPU link, use DSA or EDSA tagging depending
1893          * on which tagging mode was configured.
1894          *
1895          * If this is a link to another switch, use DSA tagging mode.
1896          *
1897          * If this is the upstream port for this switch, enable
1898          * forwarding of unknown unicasts and multicasts.
1899          */
1900         reg = 0;
1901         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1902             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
1903             mv88e6xxx_6095_family(ds) || mv88e6xxx_6065_family(ds) ||
1904             mv88e6xxx_6185_family(ds) || mv88e6xxx_6320_family(ds))
1905                 reg = PORT_CONTROL_IGMP_MLD_SNOOP |
1906                 PORT_CONTROL_USE_TAG | PORT_CONTROL_USE_IP |
1907                 PORT_CONTROL_STATE_FORWARDING;
1908         if (dsa_is_cpu_port(ds, port)) {
1909                 if (mv88e6xxx_6095_family(ds) || mv88e6xxx_6185_family(ds))
1910                         reg |= PORT_CONTROL_DSA_TAG;
1911                 if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1912                     mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
1913                     mv88e6xxx_6320_family(ds)) {
1914                         if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
1915                                 reg |= PORT_CONTROL_FRAME_ETHER_TYPE_DSA;
1916                         else
1917                                 reg |= PORT_CONTROL_FRAME_MODE_DSA;
1918                         reg |= PORT_CONTROL_FORWARD_UNKNOWN |
1919                                 PORT_CONTROL_FORWARD_UNKNOWN_MC;
1920                 }
1921
1922                 if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1923                     mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
1924                     mv88e6xxx_6095_family(ds) || mv88e6xxx_6065_family(ds) ||
1925                     mv88e6xxx_6185_family(ds) || mv88e6xxx_6320_family(ds)) {
1926                         if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
1927                                 reg |= PORT_CONTROL_EGRESS_ADD_TAG;
1928                 }
1929         }
1930         if (dsa_is_dsa_port(ds, port)) {
1931                 if (mv88e6xxx_6095_family(ds) || mv88e6xxx_6185_family(ds))
1932                         reg |= PORT_CONTROL_DSA_TAG;
1933                 if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1934                     mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
1935                     mv88e6xxx_6320_family(ds)) {
1936                         reg |= PORT_CONTROL_FRAME_MODE_DSA;
1937                 }
1938
1939                 if (port == dsa_upstream_port(ds))
1940                         reg |= PORT_CONTROL_FORWARD_UNKNOWN |
1941                                 PORT_CONTROL_FORWARD_UNKNOWN_MC;
1942         }
1943         if (reg) {
1944                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
1945                                            PORT_CONTROL, reg);
1946                 if (ret)
1947                         goto abort;
1948         }
1949
1950         /* Port Control 2: don't force a good FCS, set the maximum frame size to
1951          * 10240 bytes, enable secure 802.1q tags, don't discard tagged or
1952          * untagged frames on this port, do a destination address lookup on all
1953          * received packets as usual, disable ARP mirroring and don't send a
1954          * copy of all transmitted/received frames on this port to the CPU.
1955          */
1956         reg = 0;
1957         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1958             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
1959             mv88e6xxx_6095_family(ds) || mv88e6xxx_6320_family(ds))
1960                 reg = PORT_CONTROL_2_MAP_DA;
1961
1962         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
1963             mv88e6xxx_6165_family(ds) || mv88e6xxx_6320_family(ds))
1964                 reg |= PORT_CONTROL_2_JUMBO_10240;
1965
1966         if (mv88e6xxx_6095_family(ds) || mv88e6xxx_6185_family(ds)) {
1967                 /* Set the upstream port this port should use */
1968                 reg |= dsa_upstream_port(ds);
1969                 /* enable forwarding of unknown multicast addresses to
1970                  * the upstream port
1971                  */
1972                 if (port == dsa_upstream_port(ds))
1973                         reg |= PORT_CONTROL_2_FORWARD_UNKNOWN;
1974         }
1975
1976         reg |= PORT_CONTROL_2_8021Q_SECURE;
1977
1978         if (reg) {
1979                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
1980                                            PORT_CONTROL_2, reg);
1981                 if (ret)
1982                         goto abort;
1983         }
1984
1985         /* Port Association Vector: when learning source addresses
1986          * of packets, add the address to the address database using
1987          * a port bitmap that has only the bit for this port set and
1988          * the other bits clear.
1989          */
1990         ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_ASSOC_VECTOR,
1991                                    1 << port);
1992         if (ret)
1993                 goto abort;
1994
1995         /* Egress rate control 2: disable egress rate control. */
1996         ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_RATE_CONTROL_2,
1997                                    0x0000);
1998         if (ret)
1999                 goto abort;
2000
2001         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
2002             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
2003             mv88e6xxx_6320_family(ds)) {
2004                 /* Do not limit the period of time that this port can
2005                  * be paused for by the remote end or the period of
2006                  * time that this port can pause the remote end.
2007                  */
2008                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2009                                            PORT_PAUSE_CTRL, 0x0000);
2010                 if (ret)
2011                         goto abort;
2012
2013                 /* Port ATU control: disable limiting the number of
2014                  * address database entries that this port is allowed
2015                  * to use.
2016                  */
2017                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2018                                            PORT_ATU_CONTROL, 0x0000);
2019                 /* Priority Override: disable DA, SA and VTU priority
2020                  * override.
2021                  */
2022                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2023                                            PORT_PRI_OVERRIDE, 0x0000);
2024                 if (ret)
2025                         goto abort;
2026
2027                 /* Port Ethertype: use the Ethertype DSA Ethertype
2028                  * value.
2029                  */
2030                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2031                                            PORT_ETH_TYPE, ETH_P_EDSA);
2032                 if (ret)
2033                         goto abort;
2034                 /* Tag Remap: use an identity 802.1p prio -> switch
2035                  * prio mapping.
2036                  */
2037                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2038                                            PORT_TAG_REGMAP_0123, 0x3210);
2039                 if (ret)
2040                         goto abort;
2041
2042                 /* Tag Remap 2: use an identity 802.1p prio -> switch
2043                  * prio mapping.
2044                  */
2045                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2046                                            PORT_TAG_REGMAP_4567, 0x7654);
2047                 if (ret)
2048                         goto abort;
2049         }
2050
2051         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
2052             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
2053             mv88e6xxx_6185_family(ds) || mv88e6xxx_6095_family(ds) ||
2054             mv88e6xxx_6320_family(ds)) {
2055                 /* Rate Control: disable ingress rate limiting. */
2056                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
2057                                            PORT_RATE_CONTROL, 0x0001);
2058                 if (ret)
2059                         goto abort;
2060         }
2061
2062         /* Port Control 1: disable trunking, disable sending
2063          * learning messages to this port.
2064          */
2065         ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_CONTROL_1, 0x0000);
2066         if (ret)
2067                 goto abort;
2068
2069         /* Port based VLAN map: do not give each port its own address
2070          * database, and allow every port to egress frames on all other ports.
2071          */
2072         reg = BIT(ps->num_ports) - 1; /* all ports */
2073         ret = _mv88e6xxx_port_vlan_map_set(ds, port, reg & ~port);
2074         if (ret)
2075                 goto abort;
2076
2077         /* Default VLAN ID and priority: don't set a default VLAN
2078          * ID, and set the default packet priority to zero.
2079          */
2080         ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_DEFAULT_VLAN,
2081                                    0x0000);
2082 abort:
2083         mutex_unlock(&ps->smi_mutex);
2084         return ret;
2085 }
2086
2087 int mv88e6xxx_setup_ports(struct dsa_switch *ds)
2088 {
2089         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2090         int ret;
2091         int i;
2092
2093         for (i = 0; i < ps->num_ports; i++) {
2094                 ret = mv88e6xxx_setup_port(ds, i);
2095                 if (ret < 0)
2096                         return ret;
2097         }
2098         return 0;
2099 }
2100
2101 int mv88e6xxx_setup_common(struct dsa_switch *ds)
2102 {
2103         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2104
2105         mutex_init(&ps->smi_mutex);
2106
2107         ps->id = REG_READ(REG_PORT(0), PORT_SWITCH_ID) & 0xfff0;
2108
2109         INIT_WORK(&ps->bridge_work, mv88e6xxx_bridge_work);
2110
2111         return 0;
2112 }
2113
2114 int mv88e6xxx_setup_global(struct dsa_switch *ds)
2115 {
2116         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2117         int ret;
2118         int i;
2119
2120         /* Set the default address aging time to 5 minutes, and
2121          * enable address learn messages to be sent to all message
2122          * ports.
2123          */
2124         REG_WRITE(REG_GLOBAL, GLOBAL_ATU_CONTROL,
2125                   0x0140 | GLOBAL_ATU_CONTROL_LEARN2ALL);
2126
2127         /* Configure the IP ToS mapping registers. */
2128         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_0, 0x0000);
2129         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_1, 0x0000);
2130         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_2, 0x5555);
2131         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_3, 0x5555);
2132         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_4, 0xaaaa);
2133         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_5, 0xaaaa);
2134         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_6, 0xffff);
2135         REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_7, 0xffff);
2136
2137         /* Configure the IEEE 802.1p priority mapping register. */
2138         REG_WRITE(REG_GLOBAL, GLOBAL_IEEE_PRI, 0xfa41);
2139
2140         /* Send all frames with destination addresses matching
2141          * 01:80:c2:00:00:0x to the CPU port.
2142          */
2143         REG_WRITE(REG_GLOBAL2, GLOBAL2_MGMT_EN_0X, 0xffff);
2144
2145         /* Ignore removed tag data on doubly tagged packets, disable
2146          * flow control messages, force flow control priority to the
2147          * highest, and send all special multicast frames to the CPU
2148          * port at the highest priority.
2149          */
2150         REG_WRITE(REG_GLOBAL2, GLOBAL2_SWITCH_MGMT,
2151                   0x7 | GLOBAL2_SWITCH_MGMT_RSVD2CPU | 0x70 |
2152                   GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI);
2153
2154         /* Program the DSA routing table. */
2155         for (i = 0; i < 32; i++) {
2156                 int nexthop = 0x1f;
2157
2158                 if (ds->pd->rtable &&
2159                     i != ds->index && i < ds->dst->pd->nr_chips)
2160                         nexthop = ds->pd->rtable[i] & 0x1f;
2161
2162                 REG_WRITE(REG_GLOBAL2, GLOBAL2_DEVICE_MAPPING,
2163                           GLOBAL2_DEVICE_MAPPING_UPDATE |
2164                           (i << GLOBAL2_DEVICE_MAPPING_TARGET_SHIFT) |
2165                           nexthop);
2166         }
2167
2168         /* Clear all trunk masks. */
2169         for (i = 0; i < 8; i++)
2170                 REG_WRITE(REG_GLOBAL2, GLOBAL2_TRUNK_MASK,
2171                           0x8000 | (i << GLOBAL2_TRUNK_MASK_NUM_SHIFT) |
2172                           ((1 << ps->num_ports) - 1));
2173
2174         /* Clear all trunk mappings. */
2175         for (i = 0; i < 16; i++)
2176                 REG_WRITE(REG_GLOBAL2, GLOBAL2_TRUNK_MAPPING,
2177                           GLOBAL2_TRUNK_MAPPING_UPDATE |
2178                           (i << GLOBAL2_TRUNK_MAPPING_ID_SHIFT));
2179
2180         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
2181             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
2182             mv88e6xxx_6320_family(ds)) {
2183                 /* Send all frames with destination addresses matching
2184                  * 01:80:c2:00:00:2x to the CPU port.
2185                  */
2186                 REG_WRITE(REG_GLOBAL2, GLOBAL2_MGMT_EN_2X, 0xffff);
2187
2188                 /* Initialise cross-chip port VLAN table to reset
2189                  * defaults.
2190                  */
2191                 REG_WRITE(REG_GLOBAL2, GLOBAL2_PVT_ADDR, 0x9000);
2192
2193                 /* Clear the priority override table. */
2194                 for (i = 0; i < 16; i++)
2195                         REG_WRITE(REG_GLOBAL2, GLOBAL2_PRIO_OVERRIDE,
2196                                   0x8000 | (i << 8));
2197         }
2198
2199         if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
2200             mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
2201             mv88e6xxx_6185_family(ds) || mv88e6xxx_6095_family(ds) ||
2202             mv88e6xxx_6320_family(ds)) {
2203                 /* Disable ingress rate limiting by resetting all
2204                  * ingress rate limit registers to their initial
2205                  * state.
2206                  */
2207                 for (i = 0; i < ps->num_ports; i++)
2208                         REG_WRITE(REG_GLOBAL2, GLOBAL2_INGRESS_OP,
2209                                   0x9000 | (i << 8));
2210         }
2211
2212         /* Clear the statistics counters for all ports */
2213         REG_WRITE(REG_GLOBAL, GLOBAL_STATS_OP, GLOBAL_STATS_OP_FLUSH_ALL);
2214
2215         /* Wait for the flush to complete. */
2216         mutex_lock(&ps->smi_mutex);
2217         ret = _mv88e6xxx_stats_wait(ds);
2218         if (ret < 0)
2219                 goto unlock;
2220
2221         /* Clear all ATU entries */
2222         ret = _mv88e6xxx_atu_flush(ds, 0, true);
2223         if (ret < 0)
2224                 goto unlock;
2225
2226         /* Clear all the VTU and STU entries */
2227         ret = _mv88e6xxx_vtu_stu_flush(ds);
2228 unlock:
2229         mutex_unlock(&ps->smi_mutex);
2230
2231         return ret;
2232 }
2233
2234 int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active)
2235 {
2236         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2237         u16 is_reset = (ppu_active ? 0x8800 : 0xc800);
2238         unsigned long timeout;
2239         int ret;
2240         int i;
2241
2242         /* Set all ports to the disabled state. */
2243         for (i = 0; i < ps->num_ports; i++) {
2244                 ret = REG_READ(REG_PORT(i), PORT_CONTROL);
2245                 REG_WRITE(REG_PORT(i), PORT_CONTROL, ret & 0xfffc);
2246         }
2247
2248         /* Wait for transmit queues to drain. */
2249         usleep_range(2000, 4000);
2250
2251         /* Reset the switch. Keep the PPU active if requested. The PPU
2252          * needs to be active to support indirect phy register access
2253          * through global registers 0x18 and 0x19.
2254          */
2255         if (ppu_active)
2256                 REG_WRITE(REG_GLOBAL, 0x04, 0xc000);
2257         else
2258                 REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
2259
2260         /* Wait up to one second for reset to complete. */
2261         timeout = jiffies + 1 * HZ;
2262         while (time_before(jiffies, timeout)) {
2263                 ret = REG_READ(REG_GLOBAL, 0x00);
2264                 if ((ret & is_reset) == is_reset)
2265                         break;
2266                 usleep_range(1000, 2000);
2267         }
2268         if (time_after(jiffies, timeout))
2269                 return -ETIMEDOUT;
2270
2271         return 0;
2272 }
2273
2274 int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg)
2275 {
2276         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2277         int ret;
2278
2279         mutex_lock(&ps->smi_mutex);
2280         ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
2281         if (ret < 0)
2282                 goto error;
2283         ret = _mv88e6xxx_phy_read_indirect(ds, port, reg);
2284 error:
2285         _mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
2286         mutex_unlock(&ps->smi_mutex);
2287         return ret;
2288 }
2289
2290 int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
2291                              int reg, int val)
2292 {
2293         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2294         int ret;
2295
2296         mutex_lock(&ps->smi_mutex);
2297         ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
2298         if (ret < 0)
2299                 goto error;
2300
2301         ret = _mv88e6xxx_phy_write_indirect(ds, port, reg, val);
2302 error:
2303         _mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
2304         mutex_unlock(&ps->smi_mutex);
2305         return ret;
2306 }
2307
2308 static int mv88e6xxx_port_to_phy_addr(struct dsa_switch *ds, int port)
2309 {
2310         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2311
2312         if (port >= 0 && port < ps->num_ports)
2313                 return port;
2314         return -EINVAL;
2315 }
2316
2317 int
2318 mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum)
2319 {
2320         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2321         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
2322         int ret;
2323
2324         if (addr < 0)
2325                 return addr;
2326
2327         mutex_lock(&ps->smi_mutex);
2328         ret = _mv88e6xxx_phy_read(ds, addr, regnum);
2329         mutex_unlock(&ps->smi_mutex);
2330         return ret;
2331 }
2332
2333 int
2334 mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
2335 {
2336         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2337         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
2338         int ret;
2339
2340         if (addr < 0)
2341                 return addr;
2342
2343         mutex_lock(&ps->smi_mutex);
2344         ret = _mv88e6xxx_phy_write(ds, addr, regnum, val);
2345         mutex_unlock(&ps->smi_mutex);
2346         return ret;
2347 }
2348
2349 int
2350 mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int port, int regnum)
2351 {
2352         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2353         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
2354         int ret;
2355
2356         if (addr < 0)
2357                 return addr;
2358
2359         mutex_lock(&ps->smi_mutex);
2360         ret = _mv88e6xxx_phy_read_indirect(ds, addr, regnum);
2361         mutex_unlock(&ps->smi_mutex);
2362         return ret;
2363 }
2364
2365 int
2366 mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int port, int regnum,
2367                              u16 val)
2368 {
2369         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2370         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
2371         int ret;
2372
2373         if (addr < 0)
2374                 return addr;
2375
2376         mutex_lock(&ps->smi_mutex);
2377         ret = _mv88e6xxx_phy_write_indirect(ds, addr, regnum, val);
2378         mutex_unlock(&ps->smi_mutex);
2379         return ret;
2380 }
2381
2382 #ifdef CONFIG_NET_DSA_HWMON
2383
2384 static int mv88e61xx_get_temp(struct dsa_switch *ds, int *temp)
2385 {
2386         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2387         int ret;
2388         int val;
2389
2390         *temp = 0;
2391
2392         mutex_lock(&ps->smi_mutex);
2393
2394         ret = _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
2395         if (ret < 0)
2396                 goto error;
2397
2398         /* Enable temperature sensor */
2399         ret = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
2400         if (ret < 0)
2401                 goto error;
2402
2403         ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret | (1 << 5));
2404         if (ret < 0)
2405                 goto error;
2406
2407         /* Wait for temperature to stabilize */
2408         usleep_range(10000, 12000);
2409
2410         val = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
2411         if (val < 0) {
2412                 ret = val;
2413                 goto error;
2414         }
2415
2416         /* Disable temperature sensor */
2417         ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret & ~(1 << 5));
2418         if (ret < 0)
2419                 goto error;
2420
2421         *temp = ((val & 0x1f) - 5) * 5;
2422
2423 error:
2424         _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
2425         mutex_unlock(&ps->smi_mutex);
2426         return ret;
2427 }
2428
2429 static int mv88e63xx_get_temp(struct dsa_switch *ds, int *temp)
2430 {
2431         int phy = mv88e6xxx_6320_family(ds) ? 3 : 0;
2432         int ret;
2433
2434         *temp = 0;
2435
2436         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 27);
2437         if (ret < 0)
2438                 return ret;
2439
2440         *temp = (ret & 0xff) - 25;
2441
2442         return 0;
2443 }
2444
2445 int mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
2446 {
2447         if (mv88e6xxx_6320_family(ds) || mv88e6xxx_6352_family(ds))
2448                 return mv88e63xx_get_temp(ds, temp);
2449
2450         return mv88e61xx_get_temp(ds, temp);
2451 }
2452
2453 int mv88e6xxx_get_temp_limit(struct dsa_switch *ds, int *temp)
2454 {
2455         int phy = mv88e6xxx_6320_family(ds) ? 3 : 0;
2456         int ret;
2457
2458         if (!mv88e6xxx_6320_family(ds) && !mv88e6xxx_6352_family(ds))
2459                 return -EOPNOTSUPP;
2460
2461         *temp = 0;
2462
2463         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 26);
2464         if (ret < 0)
2465                 return ret;
2466
2467         *temp = (((ret >> 8) & 0x1f) * 5) - 25;
2468
2469         return 0;
2470 }
2471
2472 int mv88e6xxx_set_temp_limit(struct dsa_switch *ds, int temp)
2473 {
2474         int phy = mv88e6xxx_6320_family(ds) ? 3 : 0;
2475         int ret;
2476
2477         if (!mv88e6xxx_6320_family(ds) && !mv88e6xxx_6352_family(ds))
2478                 return -EOPNOTSUPP;
2479
2480         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 26);
2481         if (ret < 0)
2482                 return ret;
2483         temp = clamp_val(DIV_ROUND_CLOSEST(temp, 5) + 5, 0, 0x1f);
2484         return mv88e6xxx_phy_page_write(ds, phy, 6, 26,
2485                                         (ret & 0xe0ff) | (temp << 8));
2486 }
2487
2488 int mv88e6xxx_get_temp_alarm(struct dsa_switch *ds, bool *alarm)
2489 {
2490         int phy = mv88e6xxx_6320_family(ds) ? 3 : 0;
2491         int ret;
2492
2493         if (!mv88e6xxx_6320_family(ds) && !mv88e6xxx_6352_family(ds))
2494                 return -EOPNOTSUPP;
2495
2496         *alarm = false;
2497
2498         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 26);
2499         if (ret < 0)
2500                 return ret;
2501
2502         *alarm = !!(ret & 0x40);
2503
2504         return 0;
2505 }
2506 #endif /* CONFIG_NET_DSA_HWMON */
2507
2508 static int __init mv88e6xxx_init(void)
2509 {
2510 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6131)
2511         register_switch_driver(&mv88e6131_switch_driver);
2512 #endif
2513 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6123_61_65)
2514         register_switch_driver(&mv88e6123_61_65_switch_driver);
2515 #endif
2516 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6352)
2517         register_switch_driver(&mv88e6352_switch_driver);
2518 #endif
2519 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6171)
2520         register_switch_driver(&mv88e6171_switch_driver);
2521 #endif
2522         return 0;
2523 }
2524 module_init(mv88e6xxx_init);
2525
2526 static void __exit mv88e6xxx_cleanup(void)
2527 {
2528 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6171)
2529         unregister_switch_driver(&mv88e6171_switch_driver);
2530 #endif
2531 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6352)
2532         unregister_switch_driver(&mv88e6352_switch_driver);
2533 #endif
2534 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6123_61_65)
2535         unregister_switch_driver(&mv88e6123_61_65_switch_driver);
2536 #endif
2537 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6131)
2538         unregister_switch_driver(&mv88e6131_switch_driver);
2539 #endif
2540 }
2541 module_exit(mv88e6xxx_cleanup);
2542
2543 MODULE_AUTHOR("Lennert Buytenhek <buytenh@wantstofly.org>");
2544 MODULE_DESCRIPTION("Driver for Marvell 88E6XXX ethernet switch chips");
2545 MODULE_LICENSE("GPL");