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  * Copyright (c) 2016 Andrew Lunn <andrew@lunn.ch>
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License as published by
  12  * the Free Software Foundation; either version 2 of the License, or
  13  * (at your option) any later version.
  14  */
  15
  16 #include <linux/delay.h>
  17 #include <linux/etherdevice.h>
  18 #include <linux/ethtool.h>
  19 #include <linux/if_bridge.h>
  20 #include <linux/jiffies.h>
  21 #include <linux/list.h>
  22 #include <linux/mdio.h>
  23 #include <linux/module.h>
  24 #include <linux/netdevice.h>
  25 #include <linux/gpio/consumer.h>
  26 #include <linux/phy.h>
  27 #include <net/dsa.h>
  28 #include <net/switchdev.h>
  29 #include "mv88e6xxx.h"
  30
  31 static void assert_smi_lock(struct mv88e6xxx_priv_state *ps)
  32 {
  33         if (unlikely(!mutex_is_locked(&ps->smi_mutex))) {
  34                 dev_err(ps->dev, "SMI lock not held!\n");
  35                 dump_stack();
  36         }
  37 }
  38
  39 /* If the switch's ADDR[4:0] strap pins are strapped to zero, it will
  40  * use all 32 SMI bus addresses on its SMI bus, and all switch registers
  41  * will be directly accessible on some {device address,register address}
  42  * pair.  If the ADDR[4:0] pins are not strapped to zero, the switch
  43  * will only respond to SMI transactions to that specific address, and
  44  * an indirect addressing mechanism needs to be used to access its
  45  * registers.
  46  */
  47 static int mv88e6xxx_reg_wait_ready(struct mii_bus *bus, int sw_addr)
  48 {
  49         int ret;
  50         int i;
  51
  52         for (i = 0; i < 16; i++) {
  53                 ret = mdiobus_read_nested(bus, sw_addr, SMI_CMD);
  54                 if (ret < 0)
  55                         return ret;
  56
  57                 if ((ret & SMI_CMD_BUSY) == 0)
  58                         return 0;
  59         }
  60
  61         return -ETIMEDOUT;
  62 }
  63
  64 static int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr,
  65                                 int reg)
  66 {
  67         int ret;
  68
  69         if (sw_addr == 0)
  70                 return mdiobus_read_nested(bus, addr, reg);
  71
  72         /* Wait for the bus to become free. */
  73         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
  74         if (ret < 0)
  75                 return ret;
  76
  77         /* Transmit the read command. */
  78         ret = mdiobus_write_nested(bus, sw_addr, SMI_CMD,
  79                                    SMI_CMD_OP_22_READ | (addr << 5) | reg);
  80         if (ret < 0)
  81                 return ret;
  82
  83         /* Wait for the read command to complete. */
  84         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
  85         if (ret < 0)
  86                 return ret;
  87
  88         /* Read the data. */
  89         ret = mdiobus_read_nested(bus, sw_addr, SMI_DATA);
  90         if (ret < 0)
  91                 return ret;
  92
  93         return ret & 0xffff;
  94 }
  95
  96 static int _mv88e6xxx_reg_read(struct mv88e6xxx_priv_state *ps,
  97                                int addr, int reg)
  98 {
  99         int ret;
 100
 101         assert_smi_lock(ps);
 102
 103         ret = __mv88e6xxx_reg_read(ps->bus, ps->sw_addr, addr, reg);
 104         if (ret < 0)
 105                 return ret;
 106
 107         dev_dbg(ps->dev, "<- addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
 108                 addr, reg, ret);
 109
 110         return ret;
 111 }
 112
 113 int mv88e6xxx_reg_read(struct mv88e6xxx_priv_state *ps, int addr, int reg)
 114 {
 115         int ret;
 116
 117         mutex_lock(&ps->smi_mutex);
 118         ret = _mv88e6xxx_reg_read(ps, addr, reg);
 119         mutex_unlock(&ps->smi_mutex);
 120
 121         return ret;
 122 }
 123
 124 static int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 125                                  int reg, u16 val)
 126 {
 127         int ret;
 128
 129         if (sw_addr == 0)
 130                 return mdiobus_write_nested(bus, addr, reg, val);
 131
 132         /* Wait for the bus to become free. */
 133         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 134         if (ret < 0)
 135                 return ret;
 136
 137         /* Transmit the data to write. */
 138         ret = mdiobus_write_nested(bus, sw_addr, SMI_DATA, val);
 139         if (ret < 0)
 140                 return ret;
 141
 142         /* Transmit the write command. */
 143         ret = mdiobus_write_nested(bus, sw_addr, SMI_CMD,
 144                                    SMI_CMD_OP_22_WRITE | (addr << 5) | reg);
 145         if (ret < 0)
 146                 return ret;
 147
 148         /* Wait for the write command to complete. */
 149         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 150         if (ret < 0)
 151                 return ret;
 152
 153         return 0;
 154 }
 155
 156 static int _mv88e6xxx_reg_write(struct mv88e6xxx_priv_state *ps, int addr,
 157                                 int reg, u16 val)
 158 {
 159         assert_smi_lock(ps);
 160
 161         dev_dbg(ps->dev, "-> addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
 162                 addr, reg, val);
 163
 164         return __mv88e6xxx_reg_write(ps->bus, ps->sw_addr, addr, reg, val);
 165 }
 166
 167 int mv88e6xxx_reg_write(struct mv88e6xxx_priv_state *ps, int addr,
 168                         int reg, u16 val)
 169 {
 170         int ret;
 171
 172         mutex_lock(&ps->smi_mutex);
 173         ret = _mv88e6xxx_reg_write(ps, addr, reg, val);
 174         mutex_unlock(&ps->smi_mutex);
 175
 176         return ret;
 177 }
 178
 179 static int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr)
 180 {
 181         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 182         int err;
 183
 184         err = mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_MAC_01,
 185                                   (addr[0] << 8) | addr[1]);
 186         if (err)
 187                 return err;
 188
 189         err = mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_MAC_23,
 190                                   (addr[2] << 8) | addr[3]);
 191         if (err)
 192                 return err;
 193
 194         return mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_MAC_45,
 195                                    (addr[4] << 8) | addr[5]);
 196 }
 197
 198 static int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 199 {
 200         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 201         int ret;
 202         int i;
 203
 204         for (i = 0; i < 6; i++) {
 205                 int j;
 206
 207                 /* Write the MAC address byte. */
 208                 ret = mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_SWITCH_MAC,
 209                                           GLOBAL2_SWITCH_MAC_BUSY |
 210                                           (i << 8) | addr[i]);
 211                 if (ret)
 212                         return ret;
 213
 214                 /* Wait for the write to complete. */
 215                 for (j = 0; j < 16; j++) {
 216                         ret = mv88e6xxx_reg_read(ps, REG_GLOBAL2,
 217                                                  GLOBAL2_SWITCH_MAC);
 218                         if (ret < 0)
 219                                 return ret;
 220
 221                         if ((ret & GLOBAL2_SWITCH_MAC_BUSY) == 0)
 222                                 break;
 223                 }
 224                 if (j == 16)
 225                         return -ETIMEDOUT;
 226         }
 227
 228         return 0;
 229 }
 230
 231 int mv88e6xxx_set_addr(struct dsa_switch *ds, u8 *addr)
 232 {
 233         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 234
 235         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_SWITCH_MAC))
 236                 return mv88e6xxx_set_addr_indirect(ds, addr);
 237         else
 238                 return mv88e6xxx_set_addr_direct(ds, addr);
 239 }
 240
 241 static int _mv88e6xxx_phy_read(struct mv88e6xxx_priv_state *ps, int addr,
 242                                int regnum)
 243 {
 244         if (addr >= 0)
 245                 return _mv88e6xxx_reg_read(ps, addr, regnum);
 246         return 0xffff;
 247 }
 248
 249 static int _mv88e6xxx_phy_write(struct mv88e6xxx_priv_state *ps, int addr,
 250                                 int regnum, u16 val)
 251 {
 252         if (addr >= 0)
 253                 return _mv88e6xxx_reg_write(ps, addr, regnum, val);
 254         return 0;
 255 }
 256
 257 static int mv88e6xxx_ppu_disable(struct mv88e6xxx_priv_state *ps)
 258 {
 259         int ret;
 260         unsigned long timeout;
 261
 262         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_CONTROL);
 263         if (ret < 0)
 264                 return ret;
 265
 266         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_CONTROL,
 267                                    ret & ~GLOBAL_CONTROL_PPU_ENABLE);
 268         if (ret)
 269                 return ret;
 270
 271         timeout = jiffies + 1 * HZ;
 272         while (time_before(jiffies, timeout)) {
 273                 ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_STATUS);
 274                 if (ret < 0)
 275                         return ret;
 276
 277                 usleep_range(1000, 2000);
 278                 if ((ret & GLOBAL_STATUS_PPU_MASK) !=
 279                     GLOBAL_STATUS_PPU_POLLING)
 280                         return 0;
 281         }
 282
 283         return -ETIMEDOUT;
 284 }
 285
 286 static int mv88e6xxx_ppu_enable(struct mv88e6xxx_priv_state *ps)
 287 {
 288         int ret, err;
 289         unsigned long timeout;
 290
 291         ret = mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_CONTROL);
 292         if (ret < 0)
 293                 return ret;
 294
 295         err = mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_CONTROL,
 296                                   ret | GLOBAL_CONTROL_PPU_ENABLE);
 297         if (err)
 298                 return err;
 299
 300         timeout = jiffies + 1 * HZ;
 301         while (time_before(jiffies, timeout)) {
 302                 ret = mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_STATUS);
 303                 if (ret < 0)
 304                         return ret;
 305
 306                 usleep_range(1000, 2000);
 307                 if ((ret & GLOBAL_STATUS_PPU_MASK) ==
 308                     GLOBAL_STATUS_PPU_POLLING)
 309                         return 0;
 310         }
 311
 312         return -ETIMEDOUT;
 313 }
 314
 315 static void mv88e6xxx_ppu_reenable_work(struct work_struct *ugly)
 316 {
 317         struct mv88e6xxx_priv_state *ps;
 318
 319         ps = container_of(ugly, struct mv88e6xxx_priv_state, ppu_work);
 320         if (mutex_trylock(&ps->ppu_mutex)) {
 321                 if (mv88e6xxx_ppu_enable(ps) == 0)
 322                         ps->ppu_disabled = 0;
 323                 mutex_unlock(&ps->ppu_mutex);
 324         }
 325 }
 326
 327 static void mv88e6xxx_ppu_reenable_timer(unsigned long _ps)
 328 {
 329         struct mv88e6xxx_priv_state *ps = (void *)_ps;
 330
 331         schedule_work(&ps->ppu_work);
 332 }
 333
 334 static int mv88e6xxx_ppu_access_get(struct mv88e6xxx_priv_state *ps)
 335 {
 336         int ret;
 337
 338         mutex_lock(&ps->ppu_mutex);
 339
 340         /* If the PHY polling unit is enabled, disable it so that
 341          * we can access the PHY registers.  If it was already
 342          * disabled, cancel the timer that is going to re-enable
 343          * it.
 344          */
 345         if (!ps->ppu_disabled) {
 346                 ret = mv88e6xxx_ppu_disable(ps);
 347                 if (ret < 0) {
 348                         mutex_unlock(&ps->ppu_mutex);
 349                         return ret;
 350                 }
 351                 ps->ppu_disabled = 1;
 352         } else {
 353                 del_timer(&ps->ppu_timer);
 354                 ret = 0;
 355         }
 356
 357         return ret;
 358 }
 359
 360 static void mv88e6xxx_ppu_access_put(struct mv88e6xxx_priv_state *ps)
 361 {
 362         /* Schedule a timer to re-enable the PHY polling unit. */
 363         mod_timer(&ps->ppu_timer, jiffies + msecs_to_jiffies(10));
 364         mutex_unlock(&ps->ppu_mutex);
 365 }
 366
 367 void mv88e6xxx_ppu_state_init(struct mv88e6xxx_priv_state *ps)
 368 {
 369         mutex_init(&ps->ppu_mutex);
 370         INIT_WORK(&ps->ppu_work, mv88e6xxx_ppu_reenable_work);
 371         init_timer(&ps->ppu_timer);
 372         ps->ppu_timer.data = (unsigned long)ps;
 373         ps->ppu_timer.function = mv88e6xxx_ppu_reenable_timer;
 374 }
 375
 376 static int mv88e6xxx_phy_read_ppu(struct mv88e6xxx_priv_state *ps, int addr,
 377                                   int regnum)
 378 {
 379         int ret;
 380
 381         ret = mv88e6xxx_ppu_access_get(ps);
 382         if (ret >= 0) {
 383                 ret = _mv88e6xxx_reg_read(ps, addr, regnum);
 384                 mv88e6xxx_ppu_access_put(ps);
 385         }
 386
 387         return ret;
 388 }
 389
 390 static int mv88e6xxx_phy_write_ppu(struct mv88e6xxx_priv_state *ps, int addr,
 391                                    int regnum, u16 val)
 392 {
 393         int ret;
 394
 395         ret = mv88e6xxx_ppu_access_get(ps);
 396         if (ret >= 0) {
 397                 ret = _mv88e6xxx_reg_write(ps, addr, regnum, val);
 398                 mv88e6xxx_ppu_access_put(ps);
 399         }
 400
 401         return ret;
 402 }
 403
 404 static bool mv88e6xxx_6065_family(struct mv88e6xxx_priv_state *ps)
 405 {
 406         return ps->info->family == MV88E6XXX_FAMILY_6065;
 407 }
 408
 409 static bool mv88e6xxx_6095_family(struct mv88e6xxx_priv_state *ps)
 410 {
 411         return ps->info->family == MV88E6XXX_FAMILY_6095;
 412 }
 413
 414 static bool mv88e6xxx_6097_family(struct mv88e6xxx_priv_state *ps)
 415 {
 416         return ps->info->family == MV88E6XXX_FAMILY_6097;
 417 }
 418
 419 static bool mv88e6xxx_6165_family(struct mv88e6xxx_priv_state *ps)
 420 {
 421         return ps->info->family == MV88E6XXX_FAMILY_6165;
 422 }
 423
 424 static bool mv88e6xxx_6185_family(struct mv88e6xxx_priv_state *ps)
 425 {
 426         return ps->info->family == MV88E6XXX_FAMILY_6185;
 427 }
 428
 429 static bool mv88e6xxx_6320_family(struct mv88e6xxx_priv_state *ps)
 430 {
 431         return ps->info->family == MV88E6XXX_FAMILY_6320;
 432 }
 433
 434 static bool mv88e6xxx_6351_family(struct mv88e6xxx_priv_state *ps)
 435 {
 436         return ps->info->family == MV88E6XXX_FAMILY_6351;
 437 }
 438
 439 static bool mv88e6xxx_6352_family(struct mv88e6xxx_priv_state *ps)
 440 {
 441         return ps->info->family == MV88E6XXX_FAMILY_6352;
 442 }
 443
 444 static unsigned int mv88e6xxx_num_databases(struct mv88e6xxx_priv_state *ps)
 445 {
 446         return ps->info->num_databases;
 447 }
 448
 449 static bool mv88e6xxx_has_fid_reg(struct mv88e6xxx_priv_state *ps)
 450 {
 451         /* Does the device have dedicated FID registers for ATU and VTU ops? */
 452         if (mv88e6xxx_6097_family(ps) || mv88e6xxx_6165_family(ps) ||
 453             mv88e6xxx_6351_family(ps) || mv88e6xxx_6352_family(ps))
 454                 return true;
 455
 456         return false;
 457 }
 458
 459 /* We expect the switch to perform auto negotiation if there is a real
 460  * phy. However, in the case of a fixed link phy, we force the port
 461  * settings from the fixed link settings.
 462  */
 463 static void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
 464                                   struct phy_device *phydev)
 465 {
 466         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 467         u32 reg;
 468         int ret;
 469
 470         if (!phy_is_pseudo_fixed_link(phydev))
 471                 return;
 472
 473         mutex_lock(&ps->smi_mutex);
 474
 475         ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_PCS_CTRL);
 476         if (ret < 0)
 477                 goto out;
 478
 479         reg = ret & ~(PORT_PCS_CTRL_LINK_UP |
 480                       PORT_PCS_CTRL_FORCE_LINK |
 481                       PORT_PCS_CTRL_DUPLEX_FULL |
 482                       PORT_PCS_CTRL_FORCE_DUPLEX |
 483                       PORT_PCS_CTRL_UNFORCED);
 484
 485         reg |= PORT_PCS_CTRL_FORCE_LINK;
 486         if (phydev->link)
 487                         reg |= PORT_PCS_CTRL_LINK_UP;
 488
 489         if (mv88e6xxx_6065_family(ps) && phydev->speed > SPEED_100)
 490                 goto out;
 491
 492         switch (phydev->speed) {
 493         case SPEED_1000:
 494                 reg |= PORT_PCS_CTRL_1000;
 495                 break;
 496         case SPEED_100:
 497                 reg |= PORT_PCS_CTRL_100;
 498                 break;
 499         case SPEED_10:
 500                 reg |= PORT_PCS_CTRL_10;
 501                 break;
 502         default:
 503                 pr_info("Unknown speed");
 504                 goto out;
 505         }
 506
 507         reg |= PORT_PCS_CTRL_FORCE_DUPLEX;
 508         if (phydev->duplex == DUPLEX_FULL)
 509                 reg |= PORT_PCS_CTRL_DUPLEX_FULL;
 510
 511         if ((mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps)) &&
 512             (port >= ps->info->num_ports - 2)) {
 513                 if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID)
 514                         reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK;
 515                 if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)
 516                         reg |= PORT_PCS_CTRL_RGMII_DELAY_TXCLK;
 517                 if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID)
 518                         reg |= (PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
 519                                 PORT_PCS_CTRL_RGMII_DELAY_TXCLK);
 520         }
 521         _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_PCS_CTRL, reg);
 522
 523 out:
 524         mutex_unlock(&ps->smi_mutex);
 525 }
 526
 527 static int _mv88e6xxx_stats_wait(struct mv88e6xxx_priv_state *ps)
 528 {
 529         int ret;
 530         int i;
 531
 532         for (i = 0; i < 10; i++) {
 533                 ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_STATS_OP);
 534                 if ((ret & GLOBAL_STATS_OP_BUSY) == 0)
 535                         return 0;
 536         }
 537
 538         return -ETIMEDOUT;
 539 }
 540
 541 static int _mv88e6xxx_stats_snapshot(struct mv88e6xxx_priv_state *ps,
 542                                      int port)
 543 {
 544         int ret;
 545
 546         if (mv88e6xxx_6320_family(ps) || mv88e6xxx_6352_family(ps))
 547                 port = (port + 1) << 5;
 548
 549         /* Snapshot the hardware statistics counters for this port. */
 550         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_STATS_OP,
 551                                    GLOBAL_STATS_OP_CAPTURE_PORT |
 552                                    GLOBAL_STATS_OP_HIST_RX_TX | port);
 553         if (ret < 0)
 554                 return ret;
 555
 556         /* Wait for the snapshotting to complete. */
 557         ret = _mv88e6xxx_stats_wait(ps);
 558         if (ret < 0)
 559                 return ret;
 560
 561         return 0;
 562 }
 563
 564 static void _mv88e6xxx_stats_read(struct mv88e6xxx_priv_state *ps,
 565                                   int stat, u32 *val)
 566 {
 567         u32 _val;
 568         int ret;
 569
 570         *val = 0;
 571
 572         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_STATS_OP,
 573                                    GLOBAL_STATS_OP_READ_CAPTURED |
 574                                    GLOBAL_STATS_OP_HIST_RX_TX | stat);
 575         if (ret < 0)
 576                 return;
 577
 578         ret = _mv88e6xxx_stats_wait(ps);
 579         if (ret < 0)
 580                 return;
 581
 582         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_STATS_COUNTER_32);
 583         if (ret < 0)
 584                 return;
 585
 586         _val = ret << 16;
 587
 588         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_STATS_COUNTER_01);
 589         if (ret < 0)
 590                 return;
 591
 592         *val = _val | ret;
 593 }
 594
 595 static struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
 596         { "in_good_octets",     8, 0x00, BANK0, },
 597         { "in_bad_octets",      4, 0x02, BANK0, },
 598         { "in_unicast",         4, 0x04, BANK0, },
 599         { "in_broadcasts",      4, 0x06, BANK0, },
 600         { "in_multicasts",      4, 0x07, BANK0, },
 601         { "in_pause",           4, 0x16, BANK0, },
 602         { "in_undersize",       4, 0x18, BANK0, },
 603         { "in_fragments",       4, 0x19, BANK0, },
 604         { "in_oversize",        4, 0x1a, BANK0, },
 605         { "in_jabber",          4, 0x1b, BANK0, },
 606         { "in_rx_error",        4, 0x1c, BANK0, },
 607         { "in_fcs_error",       4, 0x1d, BANK0, },
 608         { "out_octets",         8, 0x0e, BANK0, },
 609         { "out_unicast",        4, 0x10, BANK0, },
 610         { "out_broadcasts",     4, 0x13, BANK0, },
 611         { "out_multicasts",     4, 0x12, BANK0, },
 612         { "out_pause",          4, 0x15, BANK0, },
 613         { "excessive",          4, 0x11, BANK0, },
 614         { "collisions",         4, 0x1e, BANK0, },
 615         { "deferred",           4, 0x05, BANK0, },
 616         { "single",             4, 0x14, BANK0, },
 617         { "multiple",           4, 0x17, BANK0, },
 618         { "out_fcs_error",      4, 0x03, BANK0, },
 619         { "late",               4, 0x1f, BANK0, },
 620         { "hist_64bytes",       4, 0x08, BANK0, },
 621         { "hist_65_127bytes",   4, 0x09, BANK0, },
 622         { "hist_128_255bytes",  4, 0x0a, BANK0, },
 623         { "hist_256_511bytes",  4, 0x0b, BANK0, },
 624         { "hist_512_1023bytes", 4, 0x0c, BANK0, },
 625         { "hist_1024_max_bytes", 4, 0x0d, BANK0, },
 626         { "sw_in_discards",     4, 0x10, PORT, },
 627         { "sw_in_filtered",     2, 0x12, PORT, },
 628         { "sw_out_filtered",    2, 0x13, PORT, },
 629         { "in_discards",        4, 0x00 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 630         { "in_filtered",        4, 0x01 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 631         { "in_accepted",        4, 0x02 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 632         { "in_bad_accepted",    4, 0x03 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 633         { "in_good_avb_class_a", 4, 0x04 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 634         { "in_good_avb_class_b", 4, 0x05 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 635         { "in_bad_avb_class_a", 4, 0x06 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 636         { "in_bad_avb_class_b", 4, 0x07 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 637         { "tcam_counter_0",     4, 0x08 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 638         { "tcam_counter_1",     4, 0x09 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 639         { "tcam_counter_2",     4, 0x0a | GLOBAL_STATS_OP_BANK_1, BANK1, },
 640         { "tcam_counter_3",     4, 0x0b | GLOBAL_STATS_OP_BANK_1, BANK1, },
 641         { "in_da_unknown",      4, 0x0e | GLOBAL_STATS_OP_BANK_1, BANK1, },
 642         { "in_management",      4, 0x0f | GLOBAL_STATS_OP_BANK_1, BANK1, },
 643         { "out_queue_0",        4, 0x10 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 644         { "out_queue_1",        4, 0x11 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 645         { "out_queue_2",        4, 0x12 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 646         { "out_queue_3",        4, 0x13 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 647         { "out_queue_4",        4, 0x14 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 648         { "out_queue_5",        4, 0x15 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 649         { "out_queue_6",        4, 0x16 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 650         { "out_queue_7",        4, 0x17 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 651         { "out_cut_through",    4, 0x18 | GLOBAL_STATS_OP_BANK_1, BANK1, },
 652         { "out_octets_a",       4, 0x1a | GLOBAL_STATS_OP_BANK_1, BANK1, },
 653         { "out_octets_b",       4, 0x1b | GLOBAL_STATS_OP_BANK_1, BANK1, },
 654         { "out_management",     4, 0x1f | GLOBAL_STATS_OP_BANK_1, BANK1, },
 655 };
 656
 657 static bool mv88e6xxx_has_stat(struct mv88e6xxx_priv_state *ps,
 658                                struct mv88e6xxx_hw_stat *stat)
 659 {
 660         switch (stat->type) {
 661         case BANK0:
 662                 return true;
 663         case BANK1:
 664                 return mv88e6xxx_6320_family(ps);
 665         case PORT:
 666                 return mv88e6xxx_6095_family(ps) ||
 667                         mv88e6xxx_6185_family(ps) ||
 668                         mv88e6xxx_6097_family(ps) ||
 669                         mv88e6xxx_6165_family(ps) ||
 670                         mv88e6xxx_6351_family(ps) ||
 671                         mv88e6xxx_6352_family(ps);
 672         }
 673         return false;
 674 }
 675
 676 static uint64_t _mv88e6xxx_get_ethtool_stat(struct mv88e6xxx_priv_state *ps,
 677                                             struct mv88e6xxx_hw_stat *s,
 678                                             int port)
 679 {
 680         u32 low;
 681         u32 high = 0;
 682         int ret;
 683         u64 value;
 684
 685         switch (s->type) {
 686         case PORT:
 687                 ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), s->reg);
 688                 if (ret < 0)
 689                         return UINT64_MAX;
 690
 691                 low = ret;
 692                 if (s->sizeof_stat == 4) {
 693                         ret = _mv88e6xxx_reg_read(ps, REG_PORT(port),
 694                                                   s->reg + 1);
 695                         if (ret < 0)
 696                                 return UINT64_MAX;
 697                         high = ret;
 698                 }
 699                 break;
 700         case BANK0:
 701         case BANK1:
 702                 _mv88e6xxx_stats_read(ps, s->reg, &low);
 703                 if (s->sizeof_stat == 8)
 704                         _mv88e6xxx_stats_read(ps, s->reg + 1, &high);
 705         }
 706         value = (((u64)high) << 16) | low;
 707         return value;
 708 }
 709
 710 static void mv88e6xxx_get_strings(struct dsa_switch *ds, int port,
 711                                   uint8_t *data)
 712 {
 713         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 714         struct mv88e6xxx_hw_stat *stat;
 715         int i, j;
 716
 717         for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
 718                 stat = &mv88e6xxx_hw_stats[i];
 719                 if (mv88e6xxx_has_stat(ps, stat)) {
 720                         memcpy(data + j * ETH_GSTRING_LEN, stat->string,
 721                                ETH_GSTRING_LEN);
 722                         j++;
 723                 }
 724         }
 725 }
 726
 727 static int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
 728 {
 729         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 730         struct mv88e6xxx_hw_stat *stat;
 731         int i, j;
 732
 733         for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
 734                 stat = &mv88e6xxx_hw_stats[i];
 735                 if (mv88e6xxx_has_stat(ps, stat))
 736                         j++;
 737         }
 738         return j;
 739 }
 740
 741 static void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds, int port,
 742                                         uint64_t *data)
 743 {
 744         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 745         struct mv88e6xxx_hw_stat *stat;
 746         int ret;
 747         int i, j;
 748
 749         mutex_lock(&ps->smi_mutex);
 750
 751         ret = _mv88e6xxx_stats_snapshot(ps, port);
 752         if (ret < 0) {
 753                 mutex_unlock(&ps->smi_mutex);
 754                 return;
 755         }
 756         for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
 757                 stat = &mv88e6xxx_hw_stats[i];
 758                 if (mv88e6xxx_has_stat(ps, stat)) {
 759                         data[j] = _mv88e6xxx_get_ethtool_stat(ps, stat, port);
 760                         j++;
 761                 }
 762         }
 763
 764         mutex_unlock(&ps->smi_mutex);
 765 }
 766
 767 static int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
 768 {
 769         return 32 * sizeof(u16);
 770 }
 771
 772 static void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 773                                struct ethtool_regs *regs, void *_p)
 774 {
 775         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 776         u16 *p = _p;
 777         int i;
 778
 779         regs->version = 0;
 780
 781         memset(p, 0xff, 32 * sizeof(u16));
 782
 783         mutex_lock(&ps->smi_mutex);
 784
 785         for (i = 0; i < 32; i++) {
 786                 int ret;
 787
 788                 ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), i);
 789                 if (ret >= 0)
 790                         p[i] = ret;
 791         }
 792
 793         mutex_unlock(&ps->smi_mutex);
 794 }
 795
 796 static int _mv88e6xxx_wait(struct mv88e6xxx_priv_state *ps, int reg, int offset,
 797                            u16 mask)
 798 {
 799         unsigned long timeout = jiffies + HZ / 10;
 800
 801         while (time_before(jiffies, timeout)) {
 802                 int ret;
 803
 804                 ret = _mv88e6xxx_reg_read(ps, reg, offset);
 805                 if (ret < 0)
 806                         return ret;
 807                 if (!(ret & mask))
 808                         return 0;
 809
 810                 usleep_range(1000, 2000);
 811         }
 812         return -ETIMEDOUT;
 813 }
 814
 815 static int mv88e6xxx_wait(struct mv88e6xxx_priv_state *ps, int reg,
 816                           int offset, u16 mask)
 817 {
 818         int ret;
 819
 820         mutex_lock(&ps->smi_mutex);
 821         ret = _mv88e6xxx_wait(ps, reg, offset, mask);
 822         mutex_unlock(&ps->smi_mutex);
 823
 824         return ret;
 825 }
 826
 827 static int _mv88e6xxx_phy_wait(struct mv88e6xxx_priv_state *ps)
 828 {
 829         return _mv88e6xxx_wait(ps, REG_GLOBAL2, GLOBAL2_SMI_OP,
 830                                GLOBAL2_SMI_OP_BUSY);
 831 }
 832
 833 static int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds)
 834 {
 835         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 836
 837         return mv88e6xxx_wait(ps, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
 838                               GLOBAL2_EEPROM_OP_LOAD);
 839 }
 840
 841 static int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds)
 842 {
 843         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 844
 845         return mv88e6xxx_wait(ps, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
 846                               GLOBAL2_EEPROM_OP_BUSY);
 847 }
 848
 849 static int mv88e6xxx_read_eeprom_word(struct dsa_switch *ds, int addr)
 850 {
 851         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 852         int ret;
 853
 854         mutex_lock(&ps->eeprom_mutex);
 855
 856         ret = mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
 857                                   GLOBAL2_EEPROM_OP_READ |
 858                                   (addr & GLOBAL2_EEPROM_OP_ADDR_MASK));
 859         if (ret < 0)
 860                 goto error;
 861
 862         ret = mv88e6xxx_eeprom_busy_wait(ds);
 863         if (ret < 0)
 864                 goto error;
 865
 866         ret = mv88e6xxx_reg_read(ps, REG_GLOBAL2, GLOBAL2_EEPROM_DATA);
 867 error:
 868         mutex_unlock(&ps->eeprom_mutex);
 869         return ret;
 870 }
 871
 872 static int mv88e6xxx_get_eeprom_len(struct dsa_switch *ds)
 873 {
 874         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 875
 876         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEPROM))
 877                 return ps->eeprom_len;
 878
 879         return 0;
 880 }
 881
 882 static int mv88e6xxx_get_eeprom(struct dsa_switch *ds,
 883                                 struct ethtool_eeprom *eeprom, u8 *data)
 884 {
 885         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 886         int offset;
 887         int len;
 888         int ret;
 889
 890         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEPROM))
 891                 return -EOPNOTSUPP;
 892
 893         offset = eeprom->offset;
 894         len = eeprom->len;
 895         eeprom->len = 0;
 896
 897         eeprom->magic = 0xc3ec4951;
 898
 899         ret = mv88e6xxx_eeprom_load_wait(ds);
 900         if (ret < 0)
 901                 return ret;
 902
 903         if (offset & 1) {
 904                 int word;
 905
 906                 word = mv88e6xxx_read_eeprom_word(ds, offset >> 1);
 907                 if (word < 0)
 908                         return word;
 909
 910                 *data++ = (word >> 8) & 0xff;
 911
 912                 offset++;
 913                 len--;
 914                 eeprom->len++;
 915         }
 916
 917         while (len >= 2) {
 918                 int word;
 919
 920                 word = mv88e6xxx_read_eeprom_word(ds, offset >> 1);
 921                 if (word < 0)
 922                         return word;
 923
 924                 *data++ = word & 0xff;
 925                 *data++ = (word >> 8) & 0xff;
 926
 927                 offset += 2;
 928                 len -= 2;
 929                 eeprom->len += 2;
 930         }
 931
 932         if (len) {
 933                 int word;
 934
 935                 word = mv88e6xxx_read_eeprom_word(ds, offset >> 1);
 936                 if (word < 0)
 937                         return word;
 938
 939                 *data++ = word & 0xff;
 940
 941                 offset++;
 942                 len--;
 943                 eeprom->len++;
 944         }
 945
 946         return 0;
 947 }
 948
 949 static int mv88e6xxx_eeprom_is_readonly(struct dsa_switch *ds)
 950 {
 951         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 952         int ret;
 953
 954         ret = mv88e6xxx_reg_read(ps, REG_GLOBAL2, GLOBAL2_EEPROM_OP);
 955         if (ret < 0)
 956                 return ret;
 957
 958         if (!(ret & GLOBAL2_EEPROM_OP_WRITE_EN))
 959                 return -EROFS;
 960
 961         return 0;
 962 }
 963
 964 static int mv88e6xxx_write_eeprom_word(struct dsa_switch *ds, int addr,
 965                                        u16 data)
 966 {
 967         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 968         int ret;
 969
 970         mutex_lock(&ps->eeprom_mutex);
 971
 972         ret = mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_EEPROM_DATA, data);
 973         if (ret < 0)
 974                 goto error;
 975
 976         ret = mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_EEPROM_OP,
 977                                   GLOBAL2_EEPROM_OP_WRITE |
 978                                   (addr & GLOBAL2_EEPROM_OP_ADDR_MASK));
 979         if (ret < 0)
 980                 goto error;
 981
 982         ret = mv88e6xxx_eeprom_busy_wait(ds);
 983 error:
 984         mutex_unlock(&ps->eeprom_mutex);
 985         return ret;
 986 }
 987
 988 static int mv88e6xxx_set_eeprom(struct dsa_switch *ds,
 989                                 struct ethtool_eeprom *eeprom, u8 *data)
 990 {
 991         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 992         int offset;
 993         int ret;
 994         int len;
 995
 996         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEPROM))
 997                 return -EOPNOTSUPP;
 998
 999         if (eeprom->magic != 0xc3ec4951)
1000                 return -EINVAL;
1001
1002         ret = mv88e6xxx_eeprom_is_readonly(ds);
1003         if (ret)
1004                 return ret;
1005
1006         offset = eeprom->offset;
1007         len = eeprom->len;
1008         eeprom->len = 0;
1009
1010         ret = mv88e6xxx_eeprom_load_wait(ds);
1011         if (ret < 0)
1012                 return ret;
1013
1014         if (offset & 1) {
1015                 int word;
1016
1017                 word = mv88e6xxx_read_eeprom_word(ds, offset >> 1);
1018                 if (word < 0)
1019                         return word;
1020
1021                 word = (*data++ << 8) | (word & 0xff);
1022
1023                 ret = mv88e6xxx_write_eeprom_word(ds, offset >> 1, word);
1024                 if (ret < 0)
1025                         return ret;
1026
1027                 offset++;
1028                 len--;
1029                 eeprom->len++;
1030         }
1031
1032         while (len >= 2) {
1033                 int word;
1034
1035                 word = *data++;
1036                 word |= *data++ << 8;
1037
1038                 ret = mv88e6xxx_write_eeprom_word(ds, offset >> 1, word);
1039                 if (ret < 0)
1040                         return ret;
1041
1042                 offset += 2;
1043                 len -= 2;
1044                 eeprom->len += 2;
1045         }
1046
1047         if (len) {
1048                 int word;
1049
1050                 word = mv88e6xxx_read_eeprom_word(ds, offset >> 1);
1051                 if (word < 0)
1052                         return word;
1053
1054                 word = (word & 0xff00) | *data++;
1055
1056                 ret = mv88e6xxx_write_eeprom_word(ds, offset >> 1, word);
1057                 if (ret < 0)
1058                         return ret;
1059
1060                 offset++;
1061                 len--;
1062                 eeprom->len++;
1063         }
1064
1065         return 0;
1066 }
1067
1068 static int _mv88e6xxx_atu_wait(struct mv88e6xxx_priv_state *ps)
1069 {
1070         return _mv88e6xxx_wait(ps, REG_GLOBAL, GLOBAL_ATU_OP,
1071                                GLOBAL_ATU_OP_BUSY);
1072 }
1073
1074 static int _mv88e6xxx_phy_read_indirect(struct mv88e6xxx_priv_state *ps,
1075                                         int addr, int regnum)
1076 {
1077         int ret;
1078
1079         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_SMI_OP,
1080                                    GLOBAL2_SMI_OP_22_READ | (addr << 5) |
1081                                    regnum);
1082         if (ret < 0)
1083                 return ret;
1084
1085         ret = _mv88e6xxx_phy_wait(ps);
1086         if (ret < 0)
1087                 return ret;
1088
1089         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL2, GLOBAL2_SMI_DATA);
1090
1091         return ret;
1092 }
1093
1094 static int _mv88e6xxx_phy_write_indirect(struct mv88e6xxx_priv_state *ps,
1095                                          int addr, int regnum, u16 val)
1096 {
1097         int ret;
1098
1099         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_SMI_DATA, val);
1100         if (ret < 0)
1101                 return ret;
1102
1103         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_SMI_OP,
1104                                    GLOBAL2_SMI_OP_22_WRITE | (addr << 5) |
1105                                    regnum);
1106
1107         return _mv88e6xxx_phy_wait(ps);
1108 }
1109
1110 static int mv88e6xxx_get_eee(struct dsa_switch *ds, int port,
1111                              struct ethtool_eee *e)
1112 {
1113         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1114         int reg;
1115
1116         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEE))
1117                 return -EOPNOTSUPP;
1118
1119         mutex_lock(&ps->smi_mutex);
1120
1121         reg = _mv88e6xxx_phy_read_indirect(ps, port, 16);
1122         if (reg < 0)
1123                 goto out;
1124
1125         e->eee_enabled = !!(reg & 0x0200);
1126         e->tx_lpi_enabled = !!(reg & 0x0100);
1127
1128         reg = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_STATUS);
1129         if (reg < 0)
1130                 goto out;
1131
1132         e->eee_active = !!(reg & PORT_STATUS_EEE);
1133         reg = 0;
1134
1135 out:
1136         mutex_unlock(&ps->smi_mutex);
1137         return reg;
1138 }
1139
1140 static int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
1141                              struct phy_device *phydev, struct ethtool_eee *e)
1142 {
1143         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1144         int reg;
1145         int ret;
1146
1147         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEE))
1148                 return -EOPNOTSUPP;
1149
1150         mutex_lock(&ps->smi_mutex);
1151
1152         ret = _mv88e6xxx_phy_read_indirect(ps, port, 16);
1153         if (ret < 0)
1154                 goto out;
1155
1156         reg = ret & ~0x0300;
1157         if (e->eee_enabled)
1158                 reg |= 0x0200;
1159         if (e->tx_lpi_enabled)
1160                 reg |= 0x0100;
1161
1162         ret = _mv88e6xxx_phy_write_indirect(ps, port, 16, reg);
1163 out:
1164         mutex_unlock(&ps->smi_mutex);
1165
1166         return ret;
1167 }
1168
1169 static int _mv88e6xxx_atu_cmd(struct mv88e6xxx_priv_state *ps, u16 fid, u16 cmd)
1170 {
1171         int ret;
1172
1173         if (mv88e6xxx_has_fid_reg(ps)) {
1174                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_ATU_FID, fid);
1175                 if (ret < 0)
1176                         return ret;
1177         } else if (mv88e6xxx_num_databases(ps) == 256) {
1178                 /* ATU DBNum[7:4] are located in ATU Control 15:12 */
1179                 ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_ATU_CONTROL);
1180                 if (ret < 0)
1181                         return ret;
1182
1183                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_ATU_CONTROL,
1184                                            (ret & 0xfff) |
1185                                            ((fid << 8) & 0xf000));
1186                 if (ret < 0)
1187                         return ret;
1188
1189                 /* ATU DBNum[3:0] are located in ATU Operation 3:0 */
1190                 cmd |= fid & 0xf;
1191         }
1192
1193         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_ATU_OP, cmd);
1194         if (ret < 0)
1195                 return ret;
1196
1197         return _mv88e6xxx_atu_wait(ps);
1198 }
1199
1200 static int _mv88e6xxx_atu_data_write(struct mv88e6xxx_priv_state *ps,
1201                                      struct mv88e6xxx_atu_entry *entry)
1202 {
1203         u16 data = entry->state & GLOBAL_ATU_DATA_STATE_MASK;
1204
1205         if (entry->state != GLOBAL_ATU_DATA_STATE_UNUSED) {
1206                 unsigned int mask, shift;
1207
1208                 if (entry->trunk) {
1209                         data |= GLOBAL_ATU_DATA_TRUNK;
1210                         mask = GLOBAL_ATU_DATA_TRUNK_ID_MASK;
1211                         shift = GLOBAL_ATU_DATA_TRUNK_ID_SHIFT;
1212                 } else {
1213                         mask = GLOBAL_ATU_DATA_PORT_VECTOR_MASK;
1214                         shift = GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT;
1215                 }
1216
1217                 data |= (entry->portv_trunkid << shift) & mask;
1218         }
1219
1220         return _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_ATU_DATA, data);
1221 }
1222
1223 static int _mv88e6xxx_atu_flush_move(struct mv88e6xxx_priv_state *ps,
1224                                      struct mv88e6xxx_atu_entry *entry,
1225                                      bool static_too)
1226 {
1227         int op;
1228         int err;
1229
1230         err = _mv88e6xxx_atu_wait(ps);
1231         if (err)
1232                 return err;
1233
1234         err = _mv88e6xxx_atu_data_write(ps, entry);
1235         if (err)
1236                 return err;
1237
1238         if (entry->fid) {
1239                 op = static_too ? GLOBAL_ATU_OP_FLUSH_MOVE_ALL_DB :
1240                         GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC_DB;
1241         } else {
1242                 op = static_too ? GLOBAL_ATU_OP_FLUSH_MOVE_ALL :
1243                         GLOBAL_ATU_OP_FLUSH_MOVE_NON_STATIC;
1244         }
1245
1246         return _mv88e6xxx_atu_cmd(ps, entry->fid, op);
1247 }
1248
1249 static int _mv88e6xxx_atu_flush(struct mv88e6xxx_priv_state *ps,
1250                                 u16 fid, bool static_too)
1251 {
1252         struct mv88e6xxx_atu_entry entry = {
1253                 .fid = fid,
1254                 .state = 0, /* EntryState bits must be 0 */
1255         };
1256
1257         return _mv88e6xxx_atu_flush_move(ps, &entry, static_too);
1258 }
1259
1260 static int _mv88e6xxx_atu_move(struct mv88e6xxx_priv_state *ps, u16 fid,
1261                                int from_port, int to_port, bool static_too)
1262 {
1263         struct mv88e6xxx_atu_entry entry = {
1264                 .trunk = false,
1265                 .fid = fid,
1266         };
1267
1268         /* EntryState bits must be 0xF */
1269         entry.state = GLOBAL_ATU_DATA_STATE_MASK;
1270
1271         /* ToPort and FromPort are respectively in PortVec bits 7:4 and 3:0 */
1272         entry.portv_trunkid = (to_port & 0x0f) << 4;
1273         entry.portv_trunkid |= from_port & 0x0f;
1274
1275         return _mv88e6xxx_atu_flush_move(ps, &entry, static_too);
1276 }
1277
1278 static int _mv88e6xxx_atu_remove(struct mv88e6xxx_priv_state *ps, u16 fid,
1279                                  int port, bool static_too)
1280 {
1281         /* Destination port 0xF means remove the entries */
1282         return _mv88e6xxx_atu_move(ps, fid, port, 0x0f, static_too);
1283 }
1284
1285 static const char * const mv88e6xxx_port_state_names[] = {
1286         [PORT_CONTROL_STATE_DISABLED] = "Disabled",
1287         [PORT_CONTROL_STATE_BLOCKING] = "Blocking/Listening",
1288         [PORT_CONTROL_STATE_LEARNING] = "Learning",
1289         [PORT_CONTROL_STATE_FORWARDING] = "Forwarding",
1290 };
1291
1292 static int _mv88e6xxx_port_state(struct mv88e6xxx_priv_state *ps, int port,
1293                                  u8 state)
1294 {
1295         struct dsa_switch *ds = ps->ds;
1296         int reg, ret = 0;
1297         u8 oldstate;
1298
1299         reg = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_CONTROL);
1300         if (reg < 0)
1301                 return reg;
1302
1303         oldstate = reg & PORT_CONTROL_STATE_MASK;
1304
1305         if (oldstate != state) {
1306                 /* Flush forwarding database if we're moving a port
1307                  * from Learning or Forwarding state to Disabled or
1308                  * Blocking or Listening state.
1309                  */
1310                 if ((oldstate == PORT_CONTROL_STATE_LEARNING ||
1311                      oldstate == PORT_CONTROL_STATE_FORWARDING)
1312                     && (state == PORT_CONTROL_STATE_DISABLED ||
1313                         state == PORT_CONTROL_STATE_BLOCKING)) {
1314                         ret = _mv88e6xxx_atu_remove(ps, 0, port, false);
1315                         if (ret)
1316                                 return ret;
1317                 }
1318
1319                 reg = (reg & ~PORT_CONTROL_STATE_MASK) | state;
1320                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_CONTROL,
1321                                            reg);
1322                 if (ret)
1323                         return ret;
1324
1325                 netdev_dbg(ds->ports[port], "PortState %s (was %s)\n",
1326                            mv88e6xxx_port_state_names[state],
1327                            mv88e6xxx_port_state_names[oldstate]);
1328         }
1329
1330         return ret;
1331 }
1332
1333 static int _mv88e6xxx_port_based_vlan_map(struct mv88e6xxx_priv_state *ps,
1334                                           int port)
1335 {
1336         struct net_device *bridge = ps->ports[port].bridge_dev;
1337         const u16 mask = (1 << ps->info->num_ports) - 1;
1338         struct dsa_switch *ds = ps->ds;
1339         u16 output_ports = 0;
1340         int reg;
1341         int i;
1342
1343         /* allow CPU port or DSA link(s) to send frames to every port */
1344         if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
1345                 output_ports = mask;
1346         } else {
1347                 for (i = 0; i < ps->info->num_ports; ++i) {
1348                         /* allow sending frames to every group member */
1349                         if (bridge && ps->ports[i].bridge_dev == bridge)
1350                                 output_ports |= BIT(i);
1351
1352                         /* allow sending frames to CPU port and DSA link(s) */
1353                         if (dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i))
1354                                 output_ports |= BIT(i);
1355                 }
1356         }
1357
1358         /* prevent frames from going back out of the port they came in on */
1359         output_ports &= ~BIT(port);
1360
1361         reg = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_BASE_VLAN);
1362         if (reg < 0)
1363                 return reg;
1364
1365         reg &= ~mask;
1366         reg |= output_ports & mask;
1367
1368         return _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_BASE_VLAN, reg);
1369 }
1370
1371 static void mv88e6xxx_port_stp_state_set(struct dsa_switch *ds, int port,
1372                                          u8 state)
1373 {
1374         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1375         int stp_state;
1376         int err;
1377
1378         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_PORTSTATE))
1379                 return;
1380
1381         switch (state) {
1382         case BR_STATE_DISABLED:
1383                 stp_state = PORT_CONTROL_STATE_DISABLED;
1384                 break;
1385         case BR_STATE_BLOCKING:
1386         case BR_STATE_LISTENING:
1387                 stp_state = PORT_CONTROL_STATE_BLOCKING;
1388                 break;
1389         case BR_STATE_LEARNING:
1390                 stp_state = PORT_CONTROL_STATE_LEARNING;
1391                 break;
1392         case BR_STATE_FORWARDING:
1393         default:
1394                 stp_state = PORT_CONTROL_STATE_FORWARDING;
1395                 break;
1396         }
1397
1398         mutex_lock(&ps->smi_mutex);
1399         err = _mv88e6xxx_port_state(ps, port, stp_state);
1400         mutex_unlock(&ps->smi_mutex);
1401
1402         if (err)
1403                 netdev_err(ds->ports[port], "failed to update state to %s\n",
1404                            mv88e6xxx_port_state_names[stp_state]);
1405 }
1406
1407 static int _mv88e6xxx_port_pvid(struct mv88e6xxx_priv_state *ps, int port,
1408                                 u16 *new, u16 *old)
1409 {
1410         struct dsa_switch *ds = ps->ds;
1411         u16 pvid;
1412         int ret;
1413
1414         ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_DEFAULT_VLAN);
1415         if (ret < 0)
1416                 return ret;
1417
1418         pvid = ret & PORT_DEFAULT_VLAN_MASK;
1419
1420         if (new) {
1421                 ret &= ~PORT_DEFAULT_VLAN_MASK;
1422                 ret |= *new & PORT_DEFAULT_VLAN_MASK;
1423
1424                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
1425                                            PORT_DEFAULT_VLAN, ret);
1426                 if (ret < 0)
1427                         return ret;
1428
1429                 netdev_dbg(ds->ports[port], "DefaultVID %d (was %d)\n", *new,
1430                            pvid);
1431         }
1432
1433         if (old)
1434                 *old = pvid;
1435
1436         return 0;
1437 }
1438
1439 static int _mv88e6xxx_port_pvid_get(struct mv88e6xxx_priv_state *ps,
1440                                     int port, u16 *pvid)
1441 {
1442         return _mv88e6xxx_port_pvid(ps, port, NULL, pvid);
1443 }
1444
1445 static int _mv88e6xxx_port_pvid_set(struct mv88e6xxx_priv_state *ps,
1446                                     int port, u16 pvid)
1447 {
1448         return _mv88e6xxx_port_pvid(ps, port, &pvid, NULL);
1449 }
1450
1451 static int _mv88e6xxx_vtu_wait(struct mv88e6xxx_priv_state *ps)
1452 {
1453         return _mv88e6xxx_wait(ps, REG_GLOBAL, GLOBAL_VTU_OP,
1454                                GLOBAL_VTU_OP_BUSY);
1455 }
1456
1457 static int _mv88e6xxx_vtu_cmd(struct mv88e6xxx_priv_state *ps, u16 op)
1458 {
1459         int ret;
1460
1461         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_OP, op);
1462         if (ret < 0)
1463                 return ret;
1464
1465         return _mv88e6xxx_vtu_wait(ps);
1466 }
1467
1468 static int _mv88e6xxx_vtu_stu_flush(struct mv88e6xxx_priv_state *ps)
1469 {
1470         int ret;
1471
1472         ret = _mv88e6xxx_vtu_wait(ps);
1473         if (ret < 0)
1474                 return ret;
1475
1476         return _mv88e6xxx_vtu_cmd(ps, GLOBAL_VTU_OP_FLUSH_ALL);
1477 }
1478
1479 static int _mv88e6xxx_vtu_stu_data_read(struct mv88e6xxx_priv_state *ps,
1480                                         struct mv88e6xxx_vtu_stu_entry *entry,
1481                                         unsigned int nibble_offset)
1482 {
1483         u16 regs[3];
1484         int i;
1485         int ret;
1486
1487         for (i = 0; i < 3; ++i) {
1488                 ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL,
1489                                           GLOBAL_VTU_DATA_0_3 + i);
1490                 if (ret < 0)
1491                         return ret;
1492
1493                 regs[i] = ret;
1494         }
1495
1496         for (i = 0; i < ps->info->num_ports; ++i) {
1497                 unsigned int shift = (i % 4) * 4 + nibble_offset;
1498                 u16 reg = regs[i / 4];
1499
1500                 entry->data[i] = (reg >> shift) & GLOBAL_VTU_STU_DATA_MASK;
1501         }
1502
1503         return 0;
1504 }
1505
1506 static int mv88e6xxx_vtu_data_read(struct mv88e6xxx_priv_state *ps,
1507                                    struct mv88e6xxx_vtu_stu_entry *entry)
1508 {
1509         return _mv88e6xxx_vtu_stu_data_read(ps, entry, 0);
1510 }
1511
1512 static int mv88e6xxx_stu_data_read(struct mv88e6xxx_priv_state *ps,
1513                                    struct mv88e6xxx_vtu_stu_entry *entry)
1514 {
1515         return _mv88e6xxx_vtu_stu_data_read(ps, entry, 2);
1516 }
1517
1518 static int _mv88e6xxx_vtu_stu_data_write(struct mv88e6xxx_priv_state *ps,
1519                                          struct mv88e6xxx_vtu_stu_entry *entry,
1520                                          unsigned int nibble_offset)
1521 {
1522         u16 regs[3] = { 0 };
1523         int i;
1524         int ret;
1525
1526         for (i = 0; i < ps->info->num_ports; ++i) {
1527                 unsigned int shift = (i % 4) * 4 + nibble_offset;
1528                 u8 data = entry->data[i];
1529
1530                 regs[i / 4] |= (data & GLOBAL_VTU_STU_DATA_MASK) << shift;
1531         }
1532
1533         for (i = 0; i < 3; ++i) {
1534                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL,
1535                                            GLOBAL_VTU_DATA_0_3 + i, regs[i]);
1536                 if (ret < 0)
1537                         return ret;
1538         }
1539
1540         return 0;
1541 }
1542
1543 static int mv88e6xxx_vtu_data_write(struct mv88e6xxx_priv_state *ps,
1544                                     struct mv88e6xxx_vtu_stu_entry *entry)
1545 {
1546         return _mv88e6xxx_vtu_stu_data_write(ps, entry, 0);
1547 }
1548
1549 static int mv88e6xxx_stu_data_write(struct mv88e6xxx_priv_state *ps,
1550                                     struct mv88e6xxx_vtu_stu_entry *entry)
1551 {
1552         return _mv88e6xxx_vtu_stu_data_write(ps, entry, 2);
1553 }
1554
1555 static int _mv88e6xxx_vtu_vid_write(struct mv88e6xxx_priv_state *ps, u16 vid)
1556 {
1557         return _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_VID,
1558                                     vid & GLOBAL_VTU_VID_MASK);
1559 }
1560
1561 static int _mv88e6xxx_vtu_getnext(struct mv88e6xxx_priv_state *ps,
1562                                   struct mv88e6xxx_vtu_stu_entry *entry)
1563 {
1564         struct mv88e6xxx_vtu_stu_entry next = { 0 };
1565         int ret;
1566
1567         ret = _mv88e6xxx_vtu_wait(ps);
1568         if (ret < 0)
1569                 return ret;
1570
1571         ret = _mv88e6xxx_vtu_cmd(ps, GLOBAL_VTU_OP_VTU_GET_NEXT);
1572         if (ret < 0)
1573                 return ret;
1574
1575         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_VTU_VID);
1576         if (ret < 0)
1577                 return ret;
1578
1579         next.vid = ret & GLOBAL_VTU_VID_MASK;
1580         next.valid = !!(ret & GLOBAL_VTU_VID_VALID);
1581
1582         if (next.valid) {
1583                 ret = mv88e6xxx_vtu_data_read(ps, &next);
1584                 if (ret < 0)
1585                         return ret;
1586
1587                 if (mv88e6xxx_has_fid_reg(ps)) {
1588                         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL,
1589                                                   GLOBAL_VTU_FID);
1590                         if (ret < 0)
1591                                 return ret;
1592
1593                         next.fid = ret & GLOBAL_VTU_FID_MASK;
1594                 } else if (mv88e6xxx_num_databases(ps) == 256) {
1595                         /* VTU DBNum[7:4] are located in VTU Operation 11:8, and
1596                          * VTU DBNum[3:0] are located in VTU Operation 3:0
1597                          */
1598                         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL,
1599                                                   GLOBAL_VTU_OP);
1600                         if (ret < 0)
1601                                 return ret;
1602
1603                         next.fid = (ret & 0xf00) >> 4;
1604                         next.fid |= ret & 0xf;
1605                 }
1606
1607                 if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_STU)) {
1608                         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL,
1609                                                   GLOBAL_VTU_SID);
1610                         if (ret < 0)
1611                                 return ret;
1612
1613                         next.sid = ret & GLOBAL_VTU_SID_MASK;
1614                 }
1615         }
1616
1617         *entry = next;
1618         return 0;
1619 }
1620
1621 static int mv88e6xxx_port_vlan_dump(struct dsa_switch *ds, int port,
1622                                     struct switchdev_obj_port_vlan *vlan,
1623                                     int (*cb)(struct switchdev_obj *obj))
1624 {
1625         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1626         struct mv88e6xxx_vtu_stu_entry next;
1627         u16 pvid;
1628         int err;
1629
1630         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VTU))
1631                 return -EOPNOTSUPP;
1632
1633         mutex_lock(&ps->smi_mutex);
1634
1635         err = _mv88e6xxx_port_pvid_get(ps, port, &pvid);
1636         if (err)
1637                 goto unlock;
1638
1639         err = _mv88e6xxx_vtu_vid_write(ps, GLOBAL_VTU_VID_MASK);
1640         if (err)
1641                 goto unlock;
1642
1643         do {
1644                 err = _mv88e6xxx_vtu_getnext(ps, &next);
1645                 if (err)
1646                         break;
1647
1648                 if (!next.valid)
1649                         break;
1650
1651                 if (next.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
1652                         continue;
1653
1654                 /* reinit and dump this VLAN obj */
1655                 vlan->vid_begin = vlan->vid_end = next.vid;
1656                 vlan->flags = 0;
1657
1658                 if (next.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED)
1659                         vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
1660
1661                 if (next.vid == pvid)
1662                         vlan->flags |= BRIDGE_VLAN_INFO_PVID;
1663
1664                 err = cb(&vlan->obj);
1665                 if (err)
1666                         break;
1667         } while (next.vid < GLOBAL_VTU_VID_MASK);
1668
1669 unlock:
1670         mutex_unlock(&ps->smi_mutex);
1671
1672         return err;
1673 }
1674
1675 static int _mv88e6xxx_vtu_loadpurge(struct mv88e6xxx_priv_state *ps,
1676                                     struct mv88e6xxx_vtu_stu_entry *entry)
1677 {
1678         u16 op = GLOBAL_VTU_OP_VTU_LOAD_PURGE;
1679         u16 reg = 0;
1680         int ret;
1681
1682         ret = _mv88e6xxx_vtu_wait(ps);
1683         if (ret < 0)
1684                 return ret;
1685
1686         if (!entry->valid)
1687                 goto loadpurge;
1688
1689         /* Write port member tags */
1690         ret = mv88e6xxx_vtu_data_write(ps, entry);
1691         if (ret < 0)
1692                 return ret;
1693
1694         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_STU)) {
1695                 reg = entry->sid & GLOBAL_VTU_SID_MASK;
1696                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_SID, reg);
1697                 if (ret < 0)
1698                         return ret;
1699         }
1700
1701         if (mv88e6xxx_has_fid_reg(ps)) {
1702                 reg = entry->fid & GLOBAL_VTU_FID_MASK;
1703                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_FID, reg);
1704                 if (ret < 0)
1705                         return ret;
1706         } else if (mv88e6xxx_num_databases(ps) == 256) {
1707                 /* VTU DBNum[7:4] are located in VTU Operation 11:8, and
1708                  * VTU DBNum[3:0] are located in VTU Operation 3:0
1709                  */
1710                 op |= (entry->fid & 0xf0) << 8;
1711                 op |= entry->fid & 0xf;
1712         }
1713
1714         reg = GLOBAL_VTU_VID_VALID;
1715 loadpurge:
1716         reg |= entry->vid & GLOBAL_VTU_VID_MASK;
1717         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_VID, reg);
1718         if (ret < 0)
1719                 return ret;
1720
1721         return _mv88e6xxx_vtu_cmd(ps, op);
1722 }
1723
1724 static int _mv88e6xxx_stu_getnext(struct mv88e6xxx_priv_state *ps, u8 sid,
1725                                   struct mv88e6xxx_vtu_stu_entry *entry)
1726 {
1727         struct mv88e6xxx_vtu_stu_entry next = { 0 };
1728         int ret;
1729
1730         ret = _mv88e6xxx_vtu_wait(ps);
1731         if (ret < 0)
1732                 return ret;
1733
1734         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_SID,
1735                                    sid & GLOBAL_VTU_SID_MASK);
1736         if (ret < 0)
1737                 return ret;
1738
1739         ret = _mv88e6xxx_vtu_cmd(ps, GLOBAL_VTU_OP_STU_GET_NEXT);
1740         if (ret < 0)
1741                 return ret;
1742
1743         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_VTU_SID);
1744         if (ret < 0)
1745                 return ret;
1746
1747         next.sid = ret & GLOBAL_VTU_SID_MASK;
1748
1749         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_VTU_VID);
1750         if (ret < 0)
1751                 return ret;
1752
1753         next.valid = !!(ret & GLOBAL_VTU_VID_VALID);
1754
1755         if (next.valid) {
1756                 ret = mv88e6xxx_stu_data_read(ps, &next);
1757                 if (ret < 0)
1758                         return ret;
1759         }
1760
1761         *entry = next;
1762         return 0;
1763 }
1764
1765 static int _mv88e6xxx_stu_loadpurge(struct mv88e6xxx_priv_state *ps,
1766                                     struct mv88e6xxx_vtu_stu_entry *entry)
1767 {
1768         u16 reg = 0;
1769         int ret;
1770
1771         ret = _mv88e6xxx_vtu_wait(ps);
1772         if (ret < 0)
1773                 return ret;
1774
1775         if (!entry->valid)
1776                 goto loadpurge;
1777
1778         /* Write port states */
1779         ret = mv88e6xxx_stu_data_write(ps, entry);
1780         if (ret < 0)
1781                 return ret;
1782
1783         reg = GLOBAL_VTU_VID_VALID;
1784 loadpurge:
1785         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_VID, reg);
1786         if (ret < 0)
1787                 return ret;
1788
1789         reg = entry->sid & GLOBAL_VTU_SID_MASK;
1790         ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_VTU_SID, reg);
1791         if (ret < 0)
1792                 return ret;
1793
1794         return _mv88e6xxx_vtu_cmd(ps, GLOBAL_VTU_OP_STU_LOAD_PURGE);
1795 }
1796
1797 static int _mv88e6xxx_port_fid(struct mv88e6xxx_priv_state *ps, int port,
1798                                u16 *new, u16 *old)
1799 {
1800         struct dsa_switch *ds = ps->ds;
1801         u16 upper_mask;
1802         u16 fid;
1803         int ret;
1804
1805         if (mv88e6xxx_num_databases(ps) == 4096)
1806                 upper_mask = 0xff;
1807         else if (mv88e6xxx_num_databases(ps) == 256)
1808                 upper_mask = 0xf;
1809         else
1810                 return -EOPNOTSUPP;
1811
1812         /* Port's default FID bits 3:0 are located in reg 0x06, offset 12 */
1813         ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_BASE_VLAN);
1814         if (ret < 0)
1815                 return ret;
1816
1817         fid = (ret & PORT_BASE_VLAN_FID_3_0_MASK) >> 12;
1818
1819         if (new) {
1820                 ret &= ~PORT_BASE_VLAN_FID_3_0_MASK;
1821                 ret |= (*new << 12) & PORT_BASE_VLAN_FID_3_0_MASK;
1822
1823                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_BASE_VLAN,
1824                                            ret);
1825                 if (ret < 0)
1826                         return ret;
1827         }
1828
1829         /* Port's default FID bits 11:4 are located in reg 0x05, offset 0 */
1830         ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_CONTROL_1);
1831         if (ret < 0)
1832                 return ret;
1833
1834         fid |= (ret & upper_mask) << 4;
1835
1836         if (new) {
1837                 ret &= ~upper_mask;
1838                 ret |= (*new >> 4) & upper_mask;
1839
1840                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_CONTROL_1,
1841                                            ret);
1842                 if (ret < 0)
1843                         return ret;
1844
1845                 netdev_dbg(ds->ports[port], "FID %d (was %d)\n", *new, fid);
1846         }
1847
1848         if (old)
1849                 *old = fid;
1850
1851         return 0;
1852 }
1853
1854 static int _mv88e6xxx_port_fid_get(struct mv88e6xxx_priv_state *ps,
1855                                    int port, u16 *fid)
1856 {
1857         return _mv88e6xxx_port_fid(ps, port, NULL, fid);
1858 }
1859
1860 static int _mv88e6xxx_port_fid_set(struct mv88e6xxx_priv_state *ps,
1861                                    int port, u16 fid)
1862 {
1863         return _mv88e6xxx_port_fid(ps, port, &fid, NULL);
1864 }
1865
1866 static int _mv88e6xxx_fid_new(struct mv88e6xxx_priv_state *ps, u16 *fid)
1867 {
1868         DECLARE_BITMAP(fid_bitmap, MV88E6XXX_N_FID);
1869         struct mv88e6xxx_vtu_stu_entry vlan;
1870         int i, err;
1871
1872         bitmap_zero(fid_bitmap, MV88E6XXX_N_FID);
1873
1874         /* Set every FID bit used by the (un)bridged ports */
1875         for (i = 0; i < ps->info->num_ports; ++i) {
1876                 err = _mv88e6xxx_port_fid_get(ps, i, fid);
1877                 if (err)
1878                         return err;
1879
1880                 set_bit(*fid, fid_bitmap);
1881         }
1882
1883         /* Set every FID bit used by the VLAN entries */
1884         err = _mv88e6xxx_vtu_vid_write(ps, GLOBAL_VTU_VID_MASK);
1885         if (err)
1886                 return err;
1887
1888         do {
1889                 err = _mv88e6xxx_vtu_getnext(ps, &vlan);
1890                 if (err)
1891                         return err;
1892
1893                 if (!vlan.valid)
1894                         break;
1895
1896                 set_bit(vlan.fid, fid_bitmap);
1897         } while (vlan.vid < GLOBAL_VTU_VID_MASK);
1898
1899         /* The reset value 0x000 is used to indicate that multiple address
1900          * databases are not needed. Return the next positive available.
1901          */
1902         *fid = find_next_zero_bit(fid_bitmap, MV88E6XXX_N_FID, 1);
1903         if (unlikely(*fid >= mv88e6xxx_num_databases(ps)))
1904                 return -ENOSPC;
1905
1906         /* Clear the database */
1907         return _mv88e6xxx_atu_flush(ps, *fid, true);
1908 }
1909
1910 static int _mv88e6xxx_vtu_new(struct mv88e6xxx_priv_state *ps, u16 vid,
1911                               struct mv88e6xxx_vtu_stu_entry *entry)
1912 {
1913         struct dsa_switch *ds = ps->ds;
1914         struct mv88e6xxx_vtu_stu_entry vlan = {
1915                 .valid = true,
1916                 .vid = vid,
1917         };
1918         int i, err;
1919
1920         err = _mv88e6xxx_fid_new(ps, &vlan.fid);
1921         if (err)
1922                 return err;
1923
1924         /* exclude all ports except the CPU and DSA ports */
1925         for (i = 0; i < ps->info->num_ports; ++i)
1926                 vlan.data[i] = dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i)
1927                         ? GLOBAL_VTU_DATA_MEMBER_TAG_UNMODIFIED
1928                         : GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
1929
1930         if (mv88e6xxx_6097_family(ps) || mv88e6xxx_6165_family(ps) ||
1931             mv88e6xxx_6351_family(ps) || mv88e6xxx_6352_family(ps)) {
1932                 struct mv88e6xxx_vtu_stu_entry vstp;
1933
1934                 /* Adding a VTU entry requires a valid STU entry. As VSTP is not
1935                  * implemented, only one STU entry is needed to cover all VTU
1936                  * entries. Thus, validate the SID 0.
1937                  */
1938                 vlan.sid = 0;
1939                 err = _mv88e6xxx_stu_getnext(ps, GLOBAL_VTU_SID_MASK, &vstp);
1940                 if (err)
1941                         return err;
1942
1943                 if (vstp.sid != vlan.sid || !vstp.valid) {
1944                         memset(&vstp, 0, sizeof(vstp));
1945                         vstp.valid = true;
1946                         vstp.sid = vlan.sid;
1947
1948                         err = _mv88e6xxx_stu_loadpurge(ps, &vstp);
1949                         if (err)
1950                                 return err;
1951                 }
1952         }
1953
1954         *entry = vlan;
1955         return 0;
1956 }
1957
1958 static int _mv88e6xxx_vtu_get(struct mv88e6xxx_priv_state *ps, u16 vid,
1959                               struct mv88e6xxx_vtu_stu_entry *entry, bool creat)
1960 {
1961         int err;
1962
1963         if (!vid)
1964                 return -EINVAL;
1965
1966         err = _mv88e6xxx_vtu_vid_write(ps, vid - 1);
1967         if (err)
1968                 return err;
1969
1970         err = _mv88e6xxx_vtu_getnext(ps, entry);
1971         if (err)
1972                 return err;
1973
1974         if (entry->vid != vid || !entry->valid) {
1975                 if (!creat)
1976                         return -EOPNOTSUPP;
1977                 /* -ENOENT would've been more appropriate, but switchdev expects
1978                  * -EOPNOTSUPP to inform bridge about an eventual software VLAN.
1979                  */
1980
1981                 err = _mv88e6xxx_vtu_new(ps, vid, entry);
1982         }
1983
1984         return err;
1985 }
1986
1987 static int mv88e6xxx_port_check_hw_vlan(struct dsa_switch *ds, int port,
1988                                         u16 vid_begin, u16 vid_end)
1989 {
1990         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1991         struct mv88e6xxx_vtu_stu_entry vlan;
1992         int i, err;
1993
1994         if (!vid_begin)
1995                 return -EOPNOTSUPP;
1996
1997         mutex_lock(&ps->smi_mutex);
1998
1999         err = _mv88e6xxx_vtu_vid_write(ps, vid_begin - 1);
2000         if (err)
2001                 goto unlock;
2002
2003         do {
2004                 err = _mv88e6xxx_vtu_getnext(ps, &vlan);
2005                 if (err)
2006                         goto unlock;
2007
2008                 if (!vlan.valid)
2009                         break;
2010
2011                 if (vlan.vid > vid_end)
2012                         break;
2013
2014                 for (i = 0; i < ps->info->num_ports; ++i) {
2015                         if (dsa_is_dsa_port(ds, i) || dsa_is_cpu_port(ds, i))
2016                                 continue;
2017
2018                         if (vlan.data[i] ==
2019                             GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
2020                                 continue;
2021
2022                         if (ps->ports[i].bridge_dev ==
2023                             ps->ports[port].bridge_dev)
2024                                 break; /* same bridge, check next VLAN */
2025
2026                         netdev_warn(ds->ports[port],
2027                                     "hardware VLAN %d already used by %s\n",
2028                                     vlan.vid,
2029                                     netdev_name(ps->ports[i].bridge_dev));
2030                         err = -EOPNOTSUPP;
2031                         goto unlock;
2032                 }
2033         } while (vlan.vid < vid_end);
2034
2035 unlock:
2036         mutex_unlock(&ps->smi_mutex);
2037
2038         return err;
2039 }
2040
2041 static const char * const mv88e6xxx_port_8021q_mode_names[] = {
2042         [PORT_CONTROL_2_8021Q_DISABLED] = "Disabled",
2043         [PORT_CONTROL_2_8021Q_FALLBACK] = "Fallback",
2044         [PORT_CONTROL_2_8021Q_CHECK] = "Check",
2045         [PORT_CONTROL_2_8021Q_SECURE] = "Secure",
2046 };
2047
2048 static int mv88e6xxx_port_vlan_filtering(struct dsa_switch *ds, int port,
2049                                          bool vlan_filtering)
2050 {
2051         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2052         u16 old, new = vlan_filtering ? PORT_CONTROL_2_8021Q_SECURE :
2053                 PORT_CONTROL_2_8021Q_DISABLED;
2054         int ret;
2055
2056         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VTU))
2057                 return -EOPNOTSUPP;
2058
2059         mutex_lock(&ps->smi_mutex);
2060
2061         ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_CONTROL_2);
2062         if (ret < 0)
2063                 goto unlock;
2064
2065         old = ret & PORT_CONTROL_2_8021Q_MASK;
2066
2067         if (new != old) {
2068                 ret &= ~PORT_CONTROL_2_8021Q_MASK;
2069                 ret |= new & PORT_CONTROL_2_8021Q_MASK;
2070
2071                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_CONTROL_2,
2072                                            ret);
2073                 if (ret < 0)
2074                         goto unlock;
2075
2076                 netdev_dbg(ds->ports[port], "802.1Q Mode %s (was %s)\n",
2077                            mv88e6xxx_port_8021q_mode_names[new],
2078                            mv88e6xxx_port_8021q_mode_names[old]);
2079         }
2080
2081         ret = 0;
2082 unlock:
2083         mutex_unlock(&ps->smi_mutex);
2084
2085         return ret;
2086 }
2087
2088 static int mv88e6xxx_port_vlan_prepare(struct dsa_switch *ds, int port,
2089                                        const struct switchdev_obj_port_vlan *vlan,
2090                                        struct switchdev_trans *trans)
2091 {
2092         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2093         int err;
2094
2095         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VTU))
2096                 return -EOPNOTSUPP;
2097
2098         /* If the requested port doesn't belong to the same bridge as the VLAN
2099          * members, do not support it (yet) and fallback to software VLAN.
2100          */
2101         err = mv88e6xxx_port_check_hw_vlan(ds, port, vlan->vid_begin,
2102                                            vlan->vid_end);
2103         if (err)
2104                 return err;
2105
2106         /* We don't need any dynamic resource from the kernel (yet),
2107          * so skip the prepare phase.
2108          */
2109         return 0;
2110 }
2111
2112 static int _mv88e6xxx_port_vlan_add(struct mv88e6xxx_priv_state *ps, int port,
2113                                     u16 vid, bool untagged)
2114 {
2115         struct mv88e6xxx_vtu_stu_entry vlan;
2116         int err;
2117
2118         err = _mv88e6xxx_vtu_get(ps, vid, &vlan, true);
2119         if (err)
2120                 return err;
2121
2122         vlan.data[port] = untagged ?
2123                 GLOBAL_VTU_DATA_MEMBER_TAG_UNTAGGED :
2124                 GLOBAL_VTU_DATA_MEMBER_TAG_TAGGED;
2125
2126         return _mv88e6xxx_vtu_loadpurge(ps, &vlan);
2127 }
2128
2129 static void mv88e6xxx_port_vlan_add(struct dsa_switch *ds, int port,
2130                                     const struct switchdev_obj_port_vlan *vlan,
2131                                     struct switchdev_trans *trans)
2132 {
2133         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2134         bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
2135         bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
2136         u16 vid;
2137
2138         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VTU))
2139                 return;
2140
2141         mutex_lock(&ps->smi_mutex);
2142
2143         for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid)
2144                 if (_mv88e6xxx_port_vlan_add(ps, port, vid, untagged))
2145                         netdev_err(ds->ports[port], "failed to add VLAN %d%c\n",
2146                                    vid, untagged ? 'u' : 't');
2147
2148         if (pvid && _mv88e6xxx_port_pvid_set(ps, port, vlan->vid_end))
2149                 netdev_err(ds->ports[port], "failed to set PVID %d\n",
2150                            vlan->vid_end);
2151
2152         mutex_unlock(&ps->smi_mutex);
2153 }
2154
2155 static int _mv88e6xxx_port_vlan_del(struct mv88e6xxx_priv_state *ps,
2156                                     int port, u16 vid)
2157 {
2158         struct dsa_switch *ds = ps->ds;
2159         struct mv88e6xxx_vtu_stu_entry vlan;
2160         int i, err;
2161
2162         err = _mv88e6xxx_vtu_get(ps, vid, &vlan, false);
2163         if (err)
2164                 return err;
2165
2166         /* Tell switchdev if this VLAN is handled in software */
2167         if (vlan.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
2168                 return -EOPNOTSUPP;
2169
2170         vlan.data[port] = GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
2171
2172         /* keep the VLAN unless all ports are excluded */
2173         vlan.valid = false;
2174         for (i = 0; i < ps->info->num_ports; ++i) {
2175                 if (dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i))
2176                         continue;
2177
2178                 if (vlan.data[i] != GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER) {
2179                         vlan.valid = true;
2180                         break;
2181                 }
2182         }
2183
2184         err = _mv88e6xxx_vtu_loadpurge(ps, &vlan);
2185         if (err)
2186                 return err;
2187
2188         return _mv88e6xxx_atu_remove(ps, vlan.fid, port, false);
2189 }
2190
2191 static int mv88e6xxx_port_vlan_del(struct dsa_switch *ds, int port,
2192                                    const struct switchdev_obj_port_vlan *vlan)
2193 {
2194         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2195         u16 pvid, vid;
2196         int err = 0;
2197
2198         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VTU))
2199                 return -EOPNOTSUPP;
2200
2201         mutex_lock(&ps->smi_mutex);
2202
2203         err = _mv88e6xxx_port_pvid_get(ps, port, &pvid);
2204         if (err)
2205                 goto unlock;
2206
2207         for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
2208                 err = _mv88e6xxx_port_vlan_del(ps, port, vid);
2209                 if (err)
2210                         goto unlock;
2211
2212                 if (vid == pvid) {
2213                         err = _mv88e6xxx_port_pvid_set(ps, port, 0);
2214                         if (err)
2215                                 goto unlock;
2216                 }
2217         }
2218
2219 unlock:
2220         mutex_unlock(&ps->smi_mutex);
2221
2222         return err;
2223 }
2224
2225 static int _mv88e6xxx_atu_mac_write(struct mv88e6xxx_priv_state *ps,
2226                                     const unsigned char *addr)
2227 {
2228         int i, ret;
2229
2230         for (i = 0; i < 3; i++) {
2231                 ret = _mv88e6xxx_reg_write(
2232                         ps, REG_GLOBAL, GLOBAL_ATU_MAC_01 + i,
2233                         (addr[i * 2] << 8) | addr[i * 2 + 1]);
2234                 if (ret < 0)
2235                         return ret;
2236         }
2237
2238         return 0;
2239 }
2240
2241 static int _mv88e6xxx_atu_mac_read(struct mv88e6xxx_priv_state *ps,
2242                                    unsigned char *addr)
2243 {
2244         int i, ret;
2245
2246         for (i = 0; i < 3; i++) {
2247                 ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL,
2248                                           GLOBAL_ATU_MAC_01 + i);
2249                 if (ret < 0)
2250                         return ret;
2251                 addr[i * 2] = ret >> 8;
2252                 addr[i * 2 + 1] = ret & 0xff;
2253         }
2254
2255         return 0;
2256 }
2257
2258 static int _mv88e6xxx_atu_load(struct mv88e6xxx_priv_state *ps,
2259                                struct mv88e6xxx_atu_entry *entry)
2260 {
2261         int ret;
2262
2263         ret = _mv88e6xxx_atu_wait(ps);
2264         if (ret < 0)
2265                 return ret;
2266
2267         ret = _mv88e6xxx_atu_mac_write(ps, entry->mac);
2268         if (ret < 0)
2269                 return ret;
2270
2271         ret = _mv88e6xxx_atu_data_write(ps, entry);
2272         if (ret < 0)
2273                 return ret;
2274
2275         return _mv88e6xxx_atu_cmd(ps, entry->fid, GLOBAL_ATU_OP_LOAD_DB);
2276 }
2277
2278 static int _mv88e6xxx_port_fdb_load(struct mv88e6xxx_priv_state *ps, int port,
2279                                     const unsigned char *addr, u16 vid,
2280                                     u8 state)
2281 {
2282         struct mv88e6xxx_atu_entry entry = { 0 };
2283         struct mv88e6xxx_vtu_stu_entry vlan;
2284         int err;
2285
2286         /* Null VLAN ID corresponds to the port private database */
2287         if (vid == 0)
2288                 err = _mv88e6xxx_port_fid_get(ps, port, &vlan.fid);
2289         else
2290                 err = _mv88e6xxx_vtu_get(ps, vid, &vlan, false);
2291         if (err)
2292                 return err;
2293
2294         entry.fid = vlan.fid;
2295         entry.state = state;
2296         ether_addr_copy(entry.mac, addr);
2297         if (state != GLOBAL_ATU_DATA_STATE_UNUSED) {
2298                 entry.trunk = false;
2299                 entry.portv_trunkid = BIT(port);
2300         }
2301
2302         return _mv88e6xxx_atu_load(ps, &entry);
2303 }
2304
2305 static int mv88e6xxx_port_fdb_prepare(struct dsa_switch *ds, int port,
2306                                       const struct switchdev_obj_port_fdb *fdb,
2307                                       struct switchdev_trans *trans)
2308 {
2309         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2310
2311         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_ATU))
2312                 return -EOPNOTSUPP;
2313
2314         /* We don't need any dynamic resource from the kernel (yet),
2315          * so skip the prepare phase.
2316          */
2317         return 0;
2318 }
2319
2320 static void mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
2321                                    const struct switchdev_obj_port_fdb *fdb,
2322                                    struct switchdev_trans *trans)
2323 {
2324         int state = is_multicast_ether_addr(fdb->addr) ?
2325                 GLOBAL_ATU_DATA_STATE_MC_STATIC :
2326                 GLOBAL_ATU_DATA_STATE_UC_STATIC;
2327         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2328
2329         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_ATU))
2330                 return;
2331
2332         mutex_lock(&ps->smi_mutex);
2333         if (_mv88e6xxx_port_fdb_load(ps, port, fdb->addr, fdb->vid, state))
2334                 netdev_err(ds->ports[port], "failed to load MAC address\n");
2335         mutex_unlock(&ps->smi_mutex);
2336 }
2337
2338 static int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
2339                                   const struct switchdev_obj_port_fdb *fdb)
2340 {
2341         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2342         int ret;
2343
2344         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_ATU))
2345                 return -EOPNOTSUPP;
2346
2347         mutex_lock(&ps->smi_mutex);
2348         ret = _mv88e6xxx_port_fdb_load(ps, port, fdb->addr, fdb->vid,
2349                                        GLOBAL_ATU_DATA_STATE_UNUSED);
2350         mutex_unlock(&ps->smi_mutex);
2351
2352         return ret;
2353 }
2354
2355 static int _mv88e6xxx_atu_getnext(struct mv88e6xxx_priv_state *ps, u16 fid,
2356                                   struct mv88e6xxx_atu_entry *entry)
2357 {
2358         struct mv88e6xxx_atu_entry next = { 0 };
2359         int ret;
2360
2361         next.fid = fid;
2362
2363         ret = _mv88e6xxx_atu_wait(ps);
2364         if (ret < 0)
2365                 return ret;
2366
2367         ret = _mv88e6xxx_atu_cmd(ps, fid, GLOBAL_ATU_OP_GET_NEXT_DB);
2368         if (ret < 0)
2369                 return ret;
2370
2371         ret = _mv88e6xxx_atu_mac_read(ps, next.mac);
2372         if (ret < 0)
2373                 return ret;
2374
2375         ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, GLOBAL_ATU_DATA);
2376         if (ret < 0)
2377                 return ret;
2378
2379         next.state = ret & GLOBAL_ATU_DATA_STATE_MASK;
2380         if (next.state != GLOBAL_ATU_DATA_STATE_UNUSED) {
2381                 unsigned int mask, shift;
2382
2383                 if (ret & GLOBAL_ATU_DATA_TRUNK) {
2384                         next.trunk = true;
2385                         mask = GLOBAL_ATU_DATA_TRUNK_ID_MASK;
2386                         shift = GLOBAL_ATU_DATA_TRUNK_ID_SHIFT;
2387                 } else {
2388                         next.trunk = false;
2389                         mask = GLOBAL_ATU_DATA_PORT_VECTOR_MASK;
2390                         shift = GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT;
2391                 }
2392
2393                 next.portv_trunkid = (ret & mask) >> shift;
2394         }
2395
2396         *entry = next;
2397         return 0;
2398 }
2399
2400 static int _mv88e6xxx_port_fdb_dump_one(struct mv88e6xxx_priv_state *ps,
2401                                         u16 fid, u16 vid, int port,
2402                                         struct switchdev_obj_port_fdb *fdb,
2403                                         int (*cb)(struct switchdev_obj *obj))
2404 {
2405         struct mv88e6xxx_atu_entry addr = {
2406                 .mac = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff },
2407         };
2408         int err;
2409
2410         err = _mv88e6xxx_atu_mac_write(ps, addr.mac);
2411         if (err)
2412                 return err;
2413
2414         do {
2415                 err = _mv88e6xxx_atu_getnext(ps, fid, &addr);
2416                 if (err)
2417                         break;
2418
2419                 if (addr.state == GLOBAL_ATU_DATA_STATE_UNUSED)
2420                         break;
2421
2422                 if (!addr.trunk && addr.portv_trunkid & BIT(port)) {
2423                         bool is_static = addr.state ==
2424                                 (is_multicast_ether_addr(addr.mac) ?
2425                                  GLOBAL_ATU_DATA_STATE_MC_STATIC :
2426                                  GLOBAL_ATU_DATA_STATE_UC_STATIC);
2427
2428                         fdb->vid = vid;
2429                         ether_addr_copy(fdb->addr, addr.mac);
2430                         fdb->ndm_state = is_static ? NUD_NOARP : NUD_REACHABLE;
2431
2432                         err = cb(&fdb->obj);
2433                         if (err)
2434                                 break;
2435                 }
2436         } while (!is_broadcast_ether_addr(addr.mac));
2437
2438         return err;
2439 }
2440
2441 static int mv88e6xxx_port_fdb_dump(struct dsa_switch *ds, int port,
2442                                    struct switchdev_obj_port_fdb *fdb,
2443                                    int (*cb)(struct switchdev_obj *obj))
2444 {
2445         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2446         struct mv88e6xxx_vtu_stu_entry vlan = {
2447                 .vid = GLOBAL_VTU_VID_MASK, /* all ones */
2448         };
2449         u16 fid;
2450         int err;
2451
2452         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_ATU))
2453                 return -EOPNOTSUPP;
2454
2455         mutex_lock(&ps->smi_mutex);
2456
2457         /* Dump port's default Filtering Information Database (VLAN ID 0) */
2458         err = _mv88e6xxx_port_fid_get(ps, port, &fid);
2459         if (err)
2460                 goto unlock;
2461
2462         err = _mv88e6xxx_port_fdb_dump_one(ps, fid, 0, port, fdb, cb);
2463         if (err)
2464                 goto unlock;
2465
2466         /* Dump VLANs' Filtering Information Databases */
2467         err = _mv88e6xxx_vtu_vid_write(ps, vlan.vid);
2468         if (err)
2469                 goto unlock;
2470
2471         do {
2472                 err = _mv88e6xxx_vtu_getnext(ps, &vlan);
2473                 if (err)
2474                         break;
2475
2476                 if (!vlan.valid)
2477                         break;
2478
2479                 err = _mv88e6xxx_port_fdb_dump_one(ps, vlan.fid, vlan.vid, port,
2480                                                    fdb, cb);
2481                 if (err)
2482                         break;
2483         } while (vlan.vid < GLOBAL_VTU_VID_MASK);
2484
2485 unlock:
2486         mutex_unlock(&ps->smi_mutex);
2487
2488         return err;
2489 }
2490
2491 static int mv88e6xxx_port_bridge_join(struct dsa_switch *ds, int port,
2492                                       struct net_device *bridge)
2493 {
2494         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2495         int i, err = 0;
2496
2497         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VLANTABLE))
2498                 return -EOPNOTSUPP;
2499
2500         mutex_lock(&ps->smi_mutex);
2501
2502         /* Assign the bridge and remap each port's VLANTable */
2503         ps->ports[port].bridge_dev = bridge;
2504
2505         for (i = 0; i < ps->info->num_ports; ++i) {
2506                 if (ps->ports[i].bridge_dev == bridge) {
2507                         err = _mv88e6xxx_port_based_vlan_map(ps, i);
2508                         if (err)
2509                                 break;
2510                 }
2511         }
2512
2513         mutex_unlock(&ps->smi_mutex);
2514
2515         return err;
2516 }
2517
2518 static void mv88e6xxx_port_bridge_leave(struct dsa_switch *ds, int port)
2519 {
2520         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
2521         struct net_device *bridge = ps->ports[port].bridge_dev;
2522         int i;
2523
2524         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_VLANTABLE))
2525                 return;
2526
2527         mutex_lock(&ps->smi_mutex);
2528
2529         /* Unassign the bridge and remap each port's VLANTable */
2530         ps->ports[port].bridge_dev = NULL;
2531
2532         for (i = 0; i < ps->info->num_ports; ++i)
2533                 if (i == port || ps->ports[i].bridge_dev == bridge)
2534                         if (_mv88e6xxx_port_based_vlan_map(ps, i))
2535                                 netdev_warn(ds->ports[i], "failed to remap\n");
2536
2537         mutex_unlock(&ps->smi_mutex);
2538 }
2539
2540 static int _mv88e6xxx_phy_page_write(struct mv88e6xxx_priv_state *ps,
2541                                      int port, int page, int reg, int val)
2542 {
2543         int ret;
2544
2545         ret = _mv88e6xxx_phy_write_indirect(ps, port, 0x16, page);
2546         if (ret < 0)
2547                 goto restore_page_0;
2548
2549         ret = _mv88e6xxx_phy_write_indirect(ps, port, reg, val);
2550 restore_page_0:
2551         _mv88e6xxx_phy_write_indirect(ps, port, 0x16, 0x0);
2552
2553         return ret;
2554 }
2555
2556 static int _mv88e6xxx_phy_page_read(struct mv88e6xxx_priv_state *ps,
2557                                     int port, int page, int reg)
2558 {
2559         int ret;
2560
2561         ret = _mv88e6xxx_phy_write_indirect(ps, port, 0x16, page);
2562         if (ret < 0)
2563                 goto restore_page_0;
2564
2565         ret = _mv88e6xxx_phy_read_indirect(ps, port, reg);
2566 restore_page_0:
2567         _mv88e6xxx_phy_write_indirect(ps, port, 0x16, 0x0);
2568
2569         return ret;
2570 }
2571
2572 static int mv88e6xxx_switch_reset(struct mv88e6xxx_priv_state *ps)
2573 {
2574         bool ppu_active = mv88e6xxx_has(ps, MV88E6XXX_FLAG_PPU_ACTIVE);
2575         u16 is_reset = (ppu_active ? 0x8800 : 0xc800);
2576         struct gpio_desc *gpiod = ps->reset;
2577         unsigned long timeout;
2578         int ret;
2579         int i;
2580
2581         /* Set all ports to the disabled state. */
2582         for (i = 0; i < ps->info->num_ports; i++) {
2583                 ret = _mv88e6xxx_reg_read(ps, REG_PORT(i), PORT_CONTROL);
2584                 if (ret < 0)
2585                         return ret;
2586
2587                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(i), PORT_CONTROL,
2588                                            ret & 0xfffc);
2589                 if (ret)
2590                         return ret;
2591         }
2592
2593         /* Wait for transmit queues to drain. */
2594         usleep_range(2000, 4000);
2595
2596         /* If there is a gpio connected to the reset pin, toggle it */
2597         if (gpiod) {
2598                 gpiod_set_value_cansleep(gpiod, 1);
2599                 usleep_range(10000, 20000);
2600                 gpiod_set_value_cansleep(gpiod, 0);
2601                 usleep_range(10000, 20000);
2602         }
2603
2604         /* Reset the switch. Keep the PPU active if requested. The PPU
2605          * needs to be active to support indirect phy register access
2606          * through global registers 0x18 and 0x19.
2607          */
2608         if (ppu_active)
2609                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, 0x04, 0xc000);
2610         else
2611                 ret = _mv88e6xxx_reg_write(ps, REG_GLOBAL, 0x04, 0xc400);
2612         if (ret)
2613                 return ret;
2614
2615         /* Wait up to one second for reset to complete. */
2616         timeout = jiffies + 1 * HZ;
2617         while (time_before(jiffies, timeout)) {
2618                 ret = _mv88e6xxx_reg_read(ps, REG_GLOBAL, 0x00);
2619                 if (ret < 0)
2620                         return ret;
2621
2622                 if ((ret & is_reset) == is_reset)
2623                         break;
2624                 usleep_range(1000, 2000);
2625         }
2626         if (time_after(jiffies, timeout))
2627                 ret = -ETIMEDOUT;
2628         else
2629                 ret = 0;
2630
2631         return ret;
2632 }
2633
2634 static int mv88e6xxx_power_on_serdes(struct mv88e6xxx_priv_state *ps)
2635 {
2636         int ret;
2637
2638         ret = _mv88e6xxx_phy_page_read(ps, REG_FIBER_SERDES, PAGE_FIBER_SERDES,
2639                                        MII_BMCR);
2640         if (ret < 0)
2641                 return ret;
2642
2643         if (ret & BMCR_PDOWN) {
2644                 ret &= ~BMCR_PDOWN;
2645                 ret = _mv88e6xxx_phy_page_write(ps, REG_FIBER_SERDES,
2646                                                 PAGE_FIBER_SERDES, MII_BMCR,
2647                                                 ret);
2648         }
2649
2650         return ret;
2651 }
2652
2653 static int mv88e6xxx_setup_port(struct mv88e6xxx_priv_state *ps, int port)
2654 {
2655         struct dsa_switch *ds = ps->ds;
2656         int ret;
2657         u16 reg;
2658
2659         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2660             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2661             mv88e6xxx_6185_family(ps) || mv88e6xxx_6095_family(ps) ||
2662             mv88e6xxx_6065_family(ps) || mv88e6xxx_6320_family(ps)) {
2663                 /* MAC Forcing register: don't force link, speed,
2664                  * duplex or flow control state to any particular
2665                  * values on physical ports, but force the CPU port
2666                  * and all DSA ports to their maximum bandwidth and
2667                  * full duplex.
2668                  */
2669                 reg = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_PCS_CTRL);
2670                 if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
2671                         reg &= ~PORT_PCS_CTRL_UNFORCED;
2672                         reg |= PORT_PCS_CTRL_FORCE_LINK |
2673                                 PORT_PCS_CTRL_LINK_UP |
2674                                 PORT_PCS_CTRL_DUPLEX_FULL |
2675                                 PORT_PCS_CTRL_FORCE_DUPLEX;
2676                         if (mv88e6xxx_6065_family(ps))
2677                                 reg |= PORT_PCS_CTRL_100;
2678                         else
2679                                 reg |= PORT_PCS_CTRL_1000;
2680                 } else {
2681                         reg |= PORT_PCS_CTRL_UNFORCED;
2682                 }
2683
2684                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2685                                            PORT_PCS_CTRL, reg);
2686                 if (ret)
2687                         return ret;
2688         }
2689
2690         /* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
2691          * disable Header mode, enable IGMP/MLD snooping, disable VLAN
2692          * tunneling, determine priority by looking at 802.1p and IP
2693          * priority fields (IP prio has precedence), and set STP state
2694          * to Forwarding.
2695          *
2696          * If this is the CPU link, use DSA or EDSA tagging depending
2697          * on which tagging mode was configured.
2698          *
2699          * If this is a link to another switch, use DSA tagging mode.
2700          *
2701          * If this is the upstream port for this switch, enable
2702          * forwarding of unknown unicasts and multicasts.
2703          */
2704         reg = 0;
2705         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2706             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2707             mv88e6xxx_6095_family(ps) || mv88e6xxx_6065_family(ps) ||
2708             mv88e6xxx_6185_family(ps) || mv88e6xxx_6320_family(ps))
2709                 reg = PORT_CONTROL_IGMP_MLD_SNOOP |
2710                 PORT_CONTROL_USE_TAG | PORT_CONTROL_USE_IP |
2711                 PORT_CONTROL_STATE_FORWARDING;
2712         if (dsa_is_cpu_port(ds, port)) {
2713                 if (mv88e6xxx_6095_family(ps) || mv88e6xxx_6185_family(ps))
2714                         reg |= PORT_CONTROL_DSA_TAG;
2715                 if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2716                     mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2717                     mv88e6xxx_6320_family(ps)) {
2718                         if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
2719                                 reg |= PORT_CONTROL_FRAME_ETHER_TYPE_DSA;
2720                         else
2721                                 reg |= PORT_CONTROL_FRAME_MODE_DSA;
2722                         reg |= PORT_CONTROL_FORWARD_UNKNOWN |
2723                                 PORT_CONTROL_FORWARD_UNKNOWN_MC;
2724                 }
2725
2726                 if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2727                     mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2728                     mv88e6xxx_6095_family(ps) || mv88e6xxx_6065_family(ps) ||
2729                     mv88e6xxx_6185_family(ps) || mv88e6xxx_6320_family(ps)) {
2730                         if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
2731                                 reg |= PORT_CONTROL_EGRESS_ADD_TAG;
2732                 }
2733         }
2734         if (dsa_is_dsa_port(ds, port)) {
2735                 if (mv88e6xxx_6095_family(ps) || mv88e6xxx_6185_family(ps))
2736                         reg |= PORT_CONTROL_DSA_TAG;
2737                 if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2738                     mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2739                     mv88e6xxx_6320_family(ps)) {
2740                         reg |= PORT_CONTROL_FRAME_MODE_DSA;
2741                 }
2742
2743                 if (port == dsa_upstream_port(ds))
2744                         reg |= PORT_CONTROL_FORWARD_UNKNOWN |
2745                                 PORT_CONTROL_FORWARD_UNKNOWN_MC;
2746         }
2747         if (reg) {
2748                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2749                                            PORT_CONTROL, reg);
2750                 if (ret)
2751                         return ret;
2752         }
2753
2754         /* If this port is connected to a SerDes, make sure the SerDes is not
2755          * powered down.
2756          */
2757         if (mv88e6xxx_6352_family(ps)) {
2758                 ret = _mv88e6xxx_reg_read(ps, REG_PORT(port), PORT_STATUS);
2759                 if (ret < 0)
2760                         return ret;
2761                 ret &= PORT_STATUS_CMODE_MASK;
2762                 if ((ret == PORT_STATUS_CMODE_100BASE_X) ||
2763                     (ret == PORT_STATUS_CMODE_1000BASE_X) ||
2764                     (ret == PORT_STATUS_CMODE_SGMII)) {
2765                         ret = mv88e6xxx_power_on_serdes(ps);
2766                         if (ret < 0)
2767                                 return ret;
2768                 }
2769         }
2770
2771         /* Port Control 2: don't force a good FCS, set the maximum frame size to
2772          * 10240 bytes, disable 802.1q tags checking, don't discard tagged or
2773          * untagged frames on this port, do a destination address lookup on all
2774          * received packets as usual, disable ARP mirroring and don't send a
2775          * copy of all transmitted/received frames on this port to the CPU.
2776          */
2777         reg = 0;
2778         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2779             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2780             mv88e6xxx_6095_family(ps) || mv88e6xxx_6320_family(ps) ||
2781             mv88e6xxx_6185_family(ps))
2782                 reg = PORT_CONTROL_2_MAP_DA;
2783
2784         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2785             mv88e6xxx_6165_family(ps) || mv88e6xxx_6320_family(ps))
2786                 reg |= PORT_CONTROL_2_JUMBO_10240;
2787
2788         if (mv88e6xxx_6095_family(ps) || mv88e6xxx_6185_family(ps)) {
2789                 /* Set the upstream port this port should use */
2790                 reg |= dsa_upstream_port(ds);
2791                 /* enable forwarding of unknown multicast addresses to
2792                  * the upstream port
2793                  */
2794                 if (port == dsa_upstream_port(ds))
2795                         reg |= PORT_CONTROL_2_FORWARD_UNKNOWN;
2796         }
2797
2798         reg |= PORT_CONTROL_2_8021Q_DISABLED;
2799
2800         if (reg) {
2801                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2802                                            PORT_CONTROL_2, reg);
2803                 if (ret)
2804                         return ret;
2805         }
2806
2807         /* Port Association Vector: when learning source addresses
2808          * of packets, add the address to the address database using
2809          * a port bitmap that has only the bit for this port set and
2810          * the other bits clear.
2811          */
2812         reg = 1 << port;
2813         /* Disable learning for CPU port */
2814         if (dsa_is_cpu_port(ds, port))
2815                 reg = 0;
2816
2817         ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_ASSOC_VECTOR, reg);
2818         if (ret)
2819                 return ret;
2820
2821         /* Egress rate control 2: disable egress rate control. */
2822         ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_RATE_CONTROL_2,
2823                                    0x0000);
2824         if (ret)
2825                 return ret;
2826
2827         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2828             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2829             mv88e6xxx_6320_family(ps)) {
2830                 /* Do not limit the period of time that this port can
2831                  * be paused for by the remote end or the period of
2832                  * time that this port can pause the remote end.
2833                  */
2834                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2835                                            PORT_PAUSE_CTRL, 0x0000);
2836                 if (ret)
2837                         return ret;
2838
2839                 /* Port ATU control: disable limiting the number of
2840                  * address database entries that this port is allowed
2841                  * to use.
2842                  */
2843                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2844                                            PORT_ATU_CONTROL, 0x0000);
2845                 /* Priority Override: disable DA, SA and VTU priority
2846                  * override.
2847                  */
2848                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2849                                            PORT_PRI_OVERRIDE, 0x0000);
2850                 if (ret)
2851                         return ret;
2852
2853                 /* Port Ethertype: use the Ethertype DSA Ethertype
2854                  * value.
2855                  */
2856                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2857                                            PORT_ETH_TYPE, ETH_P_EDSA);
2858                 if (ret)
2859                         return ret;
2860                 /* Tag Remap: use an identity 802.1p prio -> switch
2861                  * prio mapping.
2862                  */
2863                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2864                                            PORT_TAG_REGMAP_0123, 0x3210);
2865                 if (ret)
2866                         return ret;
2867
2868                 /* Tag Remap 2: use an identity 802.1p prio -> switch
2869                  * prio mapping.
2870                  */
2871                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2872                                            PORT_TAG_REGMAP_4567, 0x7654);
2873                 if (ret)
2874                         return ret;
2875         }
2876
2877         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
2878             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
2879             mv88e6xxx_6185_family(ps) || mv88e6xxx_6095_family(ps) ||
2880             mv88e6xxx_6320_family(ps)) {
2881                 /* Rate Control: disable ingress rate limiting. */
2882                 ret = _mv88e6xxx_reg_write(ps, REG_PORT(port),
2883                                            PORT_RATE_CONTROL, 0x0001);
2884                 if (ret)
2885                         return ret;
2886         }
2887
2888         /* Port Control 1: disable trunking, disable sending
2889          * learning messages to this port.
2890          */
2891         ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_CONTROL_1, 0x0000);
2892         if (ret)
2893                 return ret;
2894
2895         /* Port based VLAN map: give each port the same default address
2896          * database, and allow bidirectional communication between the
2897          * CPU and DSA port(s), and the other ports.
2898          */
2899         ret = _mv88e6xxx_port_fid_set(ps, port, 0);
2900         if (ret)
2901                 return ret;
2902
2903         ret = _mv88e6xxx_port_based_vlan_map(ps, port);
2904         if (ret)
2905                 return ret;
2906
2907         /* Default VLAN ID and priority: don't set a default VLAN
2908          * ID, and set the default packet priority to zero.
2909          */
2910         ret = _mv88e6xxx_reg_write(ps, REG_PORT(port), PORT_DEFAULT_VLAN,
2911                                    0x0000);
2912         if (ret)
2913                 return ret;
2914
2915         return 0;
2916 }
2917
2918 static int mv88e6xxx_setup_global(struct mv88e6xxx_priv_state *ps)
2919 {
2920         struct dsa_switch *ds = ps->ds;
2921         u32 upstream_port = dsa_upstream_port(ds);
2922         u16 reg;
2923         int err;
2924         int i;
2925
2926         /* Enable the PHY Polling Unit if present, don't discard any packets,
2927          * and mask all interrupt sources.
2928          */
2929         reg = 0;
2930         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_PPU) ||
2931             mv88e6xxx_has(ps, MV88E6XXX_FLAG_PPU_ACTIVE))
2932                 reg |= GLOBAL_CONTROL_PPU_ENABLE;
2933
2934         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_CONTROL, reg);
2935         if (err)
2936                 return err;
2937
2938         /* Configure the upstream port, and configure it as the port to which
2939          * ingress and egress and ARP monitor frames are to be sent.
2940          */
2941         reg = upstream_port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
2942                 upstream_port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT |
2943                 upstream_port << GLOBAL_MONITOR_CONTROL_ARP_SHIFT;
2944         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_MONITOR_CONTROL, reg);
2945         if (err)
2946                 return err;
2947
2948         /* Disable remote management, and set the switch's DSA device number. */
2949         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_CONTROL_2,
2950                                    GLOBAL_CONTROL_2_MULTIPLE_CASCADE |
2951                                    (ds->index & 0x1f));
2952         if (err)
2953                 return err;
2954
2955         /* Set the default address aging time to 5 minutes, and
2956          * enable address learn messages to be sent to all message
2957          * ports.
2958          */
2959         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_ATU_CONTROL,
2960                                    0x0140 | GLOBAL_ATU_CONTROL_LEARN2ALL);
2961         if (err)
2962                 return err;
2963
2964         /* Configure the IP ToS mapping registers. */
2965         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_0, 0x0000);
2966         if (err)
2967                 return err;
2968         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_1, 0x0000);
2969         if (err)
2970                 return err;
2971         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_2, 0x5555);
2972         if (err)
2973                 return err;
2974         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_3, 0x5555);
2975         if (err)
2976                 return err;
2977         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_4, 0xaaaa);
2978         if (err)
2979                 return err;
2980         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_5, 0xaaaa);
2981         if (err)
2982                 return err;
2983         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_6, 0xffff);
2984         if (err)
2985                 return err;
2986         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IP_PRI_7, 0xffff);
2987         if (err)
2988                 return err;
2989
2990         /* Configure the IEEE 802.1p priority mapping register. */
2991         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_IEEE_PRI, 0xfa41);
2992         if (err)
2993                 return err;
2994
2995         /* Send all frames with destination addresses matching
2996          * 01:80:c2:00:00:0x to the CPU port.
2997          */
2998         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_MGMT_EN_0X, 0xffff);
2999         if (err)
3000                 return err;
3001
3002         /* Ignore removed tag data on doubly tagged packets, disable
3003          * flow control messages, force flow control priority to the
3004          * highest, and send all special multicast frames to the CPU
3005          * port at the highest priority.
3006          */
3007         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_SWITCH_MGMT,
3008                                    0x7 | GLOBAL2_SWITCH_MGMT_RSVD2CPU | 0x70 |
3009                                    GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI);
3010         if (err)
3011                 return err;
3012
3013         /* Program the DSA routing table. */
3014         for (i = 0; i < 32; i++) {
3015                 int nexthop = 0x1f;
3016
3017                 if (ps->ds->cd->rtable &&
3018                     i != ps->ds->index && i < ps->ds->dst->pd->nr_chips)
3019                         nexthop = ps->ds->cd->rtable[i] & 0x1f;
3020
3021                 err = _mv88e6xxx_reg_write(
3022                         ps, REG_GLOBAL2,
3023                         GLOBAL2_DEVICE_MAPPING,
3024                         GLOBAL2_DEVICE_MAPPING_UPDATE |
3025                         (i << GLOBAL2_DEVICE_MAPPING_TARGET_SHIFT) | nexthop);
3026                 if (err)
3027                         return err;
3028         }
3029
3030         /* Clear all trunk masks. */
3031         for (i = 0; i < 8; i++) {
3032                 err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2, GLOBAL2_TRUNK_MASK,
3033                                            0x8000 |
3034                                            (i << GLOBAL2_TRUNK_MASK_NUM_SHIFT) |
3035                                            ((1 << ps->info->num_ports) - 1));
3036                 if (err)
3037                         return err;
3038         }
3039
3040         /* Clear all trunk mappings. */
3041         for (i = 0; i < 16; i++) {
3042                 err = _mv88e6xxx_reg_write(
3043                         ps, REG_GLOBAL2,
3044                         GLOBAL2_TRUNK_MAPPING,
3045                         GLOBAL2_TRUNK_MAPPING_UPDATE |
3046                         (i << GLOBAL2_TRUNK_MAPPING_ID_SHIFT));
3047                 if (err)
3048                         return err;
3049         }
3050
3051         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
3052             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
3053             mv88e6xxx_6320_family(ps)) {
3054                 /* Send all frames with destination addresses matching
3055                  * 01:80:c2:00:00:2x to the CPU port.
3056                  */
3057                 err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2,
3058                                            GLOBAL2_MGMT_EN_2X, 0xffff);
3059                 if (err)
3060                         return err;
3061
3062                 /* Initialise cross-chip port VLAN table to reset
3063                  * defaults.
3064                  */
3065                 err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2,
3066                                            GLOBAL2_PVT_ADDR, 0x9000);
3067                 if (err)
3068                         return err;
3069
3070                 /* Clear the priority override table. */
3071                 for (i = 0; i < 16; i++) {
3072                         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2,
3073                                                    GLOBAL2_PRIO_OVERRIDE,
3074                                                    0x8000 | (i << 8));
3075                         if (err)
3076                                 return err;
3077                 }
3078         }
3079
3080         if (mv88e6xxx_6352_family(ps) || mv88e6xxx_6351_family(ps) ||
3081             mv88e6xxx_6165_family(ps) || mv88e6xxx_6097_family(ps) ||
3082             mv88e6xxx_6185_family(ps) || mv88e6xxx_6095_family(ps) ||
3083             mv88e6xxx_6320_family(ps)) {
3084                 /* Disable ingress rate limiting by resetting all
3085                  * ingress rate limit registers to their initial
3086                  * state.
3087                  */
3088                 for (i = 0; i < ps->info->num_ports; i++) {
3089                         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL2,
3090                                                    GLOBAL2_INGRESS_OP,
3091                                                    0x9000 | (i << 8));
3092                         if (err)
3093                                 return err;
3094                 }
3095         }
3096
3097         /* Clear the statistics counters for all ports */
3098         err = _mv88e6xxx_reg_write(ps, REG_GLOBAL, GLOBAL_STATS_OP,
3099                                    GLOBAL_STATS_OP_FLUSH_ALL);
3100         if (err)
3101                 return err;
3102
3103         /* Wait for the flush to complete. */
3104         err = _mv88e6xxx_stats_wait(ps);
3105         if (err)
3106                 return err;
3107
3108         /* Clear all ATU entries */
3109         err = _mv88e6xxx_atu_flush(ps, 0, true);
3110         if (err)
3111                 return err;
3112
3113         /* Clear all the VTU and STU entries */
3114         err = _mv88e6xxx_vtu_stu_flush(ps);
3115         if (err < 0)
3116                 return err;
3117
3118         return err;
3119 }
3120
3121 static int mv88e6xxx_setup(struct dsa_switch *ds)
3122 {
3123         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3124         int err;
3125         int i;
3126
3127         ps->ds = ds;
3128
3129         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEPROM))
3130                 mutex_init(&ps->eeprom_mutex);
3131
3132         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_PPU))
3133                 mv88e6xxx_ppu_state_init(ps);
3134
3135         mutex_lock(&ps->smi_mutex);
3136
3137         err = mv88e6xxx_switch_reset(ps);
3138         if (err)
3139                 goto unlock;
3140
3141         err = mv88e6xxx_setup_global(ps);
3142         if (err)
3143                 goto unlock;
3144
3145         for (i = 0; i < ps->info->num_ports; i++) {
3146                 err = mv88e6xxx_setup_port(ps, i);
3147                 if (err)
3148                         goto unlock;
3149         }
3150
3151 unlock:
3152         mutex_unlock(&ps->smi_mutex);
3153
3154         return err;
3155 }
3156
3157 int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg)
3158 {
3159         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3160         int ret;
3161
3162         mutex_lock(&ps->smi_mutex);
3163         ret = _mv88e6xxx_phy_page_read(ps, port, page, reg);
3164         mutex_unlock(&ps->smi_mutex);
3165
3166         return ret;
3167 }
3168
3169 int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
3170                              int reg, int val)
3171 {
3172         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3173         int ret;
3174
3175         mutex_lock(&ps->smi_mutex);
3176         ret = _mv88e6xxx_phy_page_write(ps, port, page, reg, val);
3177         mutex_unlock(&ps->smi_mutex);
3178
3179         return ret;
3180 }
3181
3182 static int mv88e6xxx_port_to_phy_addr(struct mv88e6xxx_priv_state *ps,
3183                                       int port)
3184 {
3185         if (port >= 0 && port < ps->info->num_ports)
3186                 return port;
3187         return -EINVAL;
3188 }
3189
3190 static int mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum)
3191 {
3192         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3193         int addr = mv88e6xxx_port_to_phy_addr(ps, port);
3194         int ret;
3195
3196         if (addr < 0)
3197                 return 0xffff;
3198
3199         mutex_lock(&ps->smi_mutex);
3200
3201         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_PPU))
3202                 ret = mv88e6xxx_phy_read_ppu(ps, addr, regnum);
3203         else if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_SMI_PHY))
3204                 ret = _mv88e6xxx_phy_read_indirect(ps, addr, regnum);
3205         else
3206                 ret = _mv88e6xxx_phy_read(ps, addr, regnum);
3207
3208         mutex_unlock(&ps->smi_mutex);
3209         return ret;
3210 }
3211
3212 static int mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum,
3213                                u16 val)
3214 {
3215         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3216         int addr = mv88e6xxx_port_to_phy_addr(ps, port);
3217         int ret;
3218
3219         if (addr < 0)
3220                 return 0xffff;
3221
3222         mutex_lock(&ps->smi_mutex);
3223
3224         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_PPU))
3225                 ret = mv88e6xxx_phy_write_ppu(ps, addr, regnum, val);
3226         else if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_SMI_PHY))
3227                 ret = _mv88e6xxx_phy_write_indirect(ps, addr, regnum, val);
3228         else
3229                 ret = _mv88e6xxx_phy_write(ps, addr, regnum, val);
3230
3231         mutex_unlock(&ps->smi_mutex);
3232         return ret;
3233 }
3234
3235 #ifdef CONFIG_NET_DSA_HWMON
3236
3237 static int mv88e61xx_get_temp(struct dsa_switch *ds, int *temp)
3238 {
3239         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3240         int ret;
3241         int val;
3242
3243         *temp = 0;
3244
3245         mutex_lock(&ps->smi_mutex);
3246
3247         ret = _mv88e6xxx_phy_write(ps, 0x0, 0x16, 0x6);
3248         if (ret < 0)
3249                 goto error;
3250
3251         /* Enable temperature sensor */
3252         ret = _mv88e6xxx_phy_read(ps, 0x0, 0x1a);
3253         if (ret < 0)
3254                 goto error;
3255
3256         ret = _mv88e6xxx_phy_write(ps, 0x0, 0x1a, ret | (1 << 5));
3257         if (ret < 0)
3258                 goto error;
3259
3260         /* Wait for temperature to stabilize */
3261         usleep_range(10000, 12000);
3262
3263         val = _mv88e6xxx_phy_read(ps, 0x0, 0x1a);
3264         if (val < 0) {
3265                 ret = val;
3266                 goto error;
3267         }
3268
3269         /* Disable temperature sensor */
3270         ret = _mv88e6xxx_phy_write(ps, 0x0, 0x1a, ret & ~(1 << 5));
3271         if (ret < 0)
3272                 goto error;
3273
3274         *temp = ((val & 0x1f) - 5) * 5;
3275
3276 error:
3277         _mv88e6xxx_phy_write(ps, 0x0, 0x16, 0x0);
3278         mutex_unlock(&ps->smi_mutex);
3279         return ret;
3280 }
3281
3282 static int mv88e63xx_get_temp(struct dsa_switch *ds, int *temp)
3283 {
3284         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3285         int phy = mv88e6xxx_6320_family(ps) ? 3 : 0;
3286         int ret;
3287
3288         *temp = 0;
3289
3290         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 27);
3291         if (ret < 0)
3292                 return ret;
3293
3294         *temp = (ret & 0xff) - 25;
3295
3296         return 0;
3297 }
3298
3299 static int mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
3300 {
3301         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3302
3303         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_TEMP))
3304                 return -EOPNOTSUPP;
3305
3306         if (mv88e6xxx_6320_family(ps) || mv88e6xxx_6352_family(ps))
3307                 return mv88e63xx_get_temp(ds, temp);
3308
3309         return mv88e61xx_get_temp(ds, temp);
3310 }
3311
3312 static int mv88e6xxx_get_temp_limit(struct dsa_switch *ds, int *temp)
3313 {
3314         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3315         int phy = mv88e6xxx_6320_family(ps) ? 3 : 0;
3316         int ret;
3317
3318         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_TEMP_LIMIT))
3319                 return -EOPNOTSUPP;
3320
3321         *temp = 0;
3322
3323         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 26);
3324         if (ret < 0)
3325                 return ret;
3326
3327         *temp = (((ret >> 8) & 0x1f) * 5) - 25;
3328
3329         return 0;
3330 }
3331
3332 static int mv88e6xxx_set_temp_limit(struct dsa_switch *ds, int temp)
3333 {
3334         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3335         int phy = mv88e6xxx_6320_family(ps) ? 3 : 0;
3336         int ret;
3337
3338         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_TEMP_LIMIT))
3339                 return -EOPNOTSUPP;
3340
3341         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 26);
3342         if (ret < 0)
3343                 return ret;
3344         temp = clamp_val(DIV_ROUND_CLOSEST(temp, 5) + 5, 0, 0x1f);
3345         return mv88e6xxx_phy_page_write(ds, phy, 6, 26,
3346                                         (ret & 0xe0ff) | (temp << 8));
3347 }
3348
3349 static int mv88e6xxx_get_temp_alarm(struct dsa_switch *ds, bool *alarm)
3350 {
3351         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3352         int phy = mv88e6xxx_6320_family(ps) ? 3 : 0;
3353         int ret;
3354
3355         if (!mv88e6xxx_has(ps, MV88E6XXX_FLAG_TEMP_LIMIT))
3356                 return -EOPNOTSUPP;
3357
3358         *alarm = false;
3359
3360         ret = mv88e6xxx_phy_page_read(ds, phy, 6, 26);
3361         if (ret < 0)
3362                 return ret;
3363
3364         *alarm = !!(ret & 0x40);
3365
3366         return 0;
3367 }
3368 #endif /* CONFIG_NET_DSA_HWMON */
3369
3370 static const struct mv88e6xxx_info mv88e6xxx_table[] = {
3371         [MV88E6085] = {
3372                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6085,
3373                 .family = MV88E6XXX_FAMILY_6097,
3374                 .name = "Marvell 88E6085",
3375                 .num_databases = 4096,
3376                 .num_ports = 10,
3377                 .flags = MV88E6XXX_FLAGS_FAMILY_6097,
3378         },
3379
3380         [MV88E6095] = {
3381                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6095,
3382                 .family = MV88E6XXX_FAMILY_6095,
3383                 .name = "Marvell 88E6095/88E6095F",
3384                 .num_databases = 256,
3385                 .num_ports = 11,
3386                 .flags = MV88E6XXX_FLAGS_FAMILY_6095,
3387         },
3388
3389         [MV88E6123] = {
3390                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6123,
3391                 .family = MV88E6XXX_FAMILY_6165,
3392                 .name = "Marvell 88E6123",
3393                 .num_databases = 4096,
3394                 .num_ports = 3,
3395                 .flags = MV88E6XXX_FLAGS_FAMILY_6165,
3396         },
3397
3398         [MV88E6131] = {
3399                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6131,
3400                 .family = MV88E6XXX_FAMILY_6185,
3401                 .name = "Marvell 88E6131",
3402                 .num_databases = 256,
3403                 .num_ports = 8,
3404                 .flags = MV88E6XXX_FLAGS_FAMILY_6185,
3405         },
3406
3407         [MV88E6161] = {
3408                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6161,
3409                 .family = MV88E6XXX_FAMILY_6165,
3410                 .name = "Marvell 88E6161",
3411                 .num_databases = 4096,
3412                 .num_ports = 6,
3413                 .flags = MV88E6XXX_FLAGS_FAMILY_6165,
3414         },
3415
3416         [MV88E6165] = {
3417                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6165,
3418                 .family = MV88E6XXX_FAMILY_6165,
3419                 .name = "Marvell 88E6165",
3420                 .num_databases = 4096,
3421                 .num_ports = 6,
3422                 .flags = MV88E6XXX_FLAGS_FAMILY_6165,
3423         },
3424
3425         [MV88E6171] = {
3426                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6171,
3427                 .family = MV88E6XXX_FAMILY_6351,
3428                 .name = "Marvell 88E6171",
3429                 .num_databases = 4096,
3430                 .num_ports = 7,
3431                 .flags = MV88E6XXX_FLAGS_FAMILY_6351,
3432         },
3433
3434         [MV88E6172] = {
3435                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6172,
3436                 .family = MV88E6XXX_FAMILY_6352,
3437                 .name = "Marvell 88E6172",
3438                 .num_databases = 4096,
3439                 .num_ports = 7,
3440                 .flags = MV88E6XXX_FLAGS_FAMILY_6352,
3441         },
3442
3443         [MV88E6175] = {
3444                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6175,
3445                 .family = MV88E6XXX_FAMILY_6351,
3446                 .name = "Marvell 88E6175",
3447                 .num_databases = 4096,
3448                 .num_ports = 7,
3449                 .flags = MV88E6XXX_FLAGS_FAMILY_6351,
3450         },
3451
3452         [MV88E6176] = {
3453                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6176,
3454                 .family = MV88E6XXX_FAMILY_6352,
3455                 .name = "Marvell 88E6176",
3456                 .num_databases = 4096,
3457                 .num_ports = 7,
3458                 .flags = MV88E6XXX_FLAGS_FAMILY_6352,
3459         },
3460
3461         [MV88E6185] = {
3462                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6185,
3463                 .family = MV88E6XXX_FAMILY_6185,
3464                 .name = "Marvell 88E6185",
3465                 .num_databases = 256,
3466                 .num_ports = 10,
3467                 .flags = MV88E6XXX_FLAGS_FAMILY_6185,
3468         },
3469
3470         [MV88E6240] = {
3471                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6240,
3472                 .family = MV88E6XXX_FAMILY_6352,
3473                 .name = "Marvell 88E6240",
3474                 .num_databases = 4096,
3475                 .num_ports = 7,
3476                 .flags = MV88E6XXX_FLAGS_FAMILY_6352,
3477         },
3478
3479         [MV88E6320] = {
3480                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6320,
3481                 .family = MV88E6XXX_FAMILY_6320,
3482                 .name = "Marvell 88E6320",
3483                 .num_databases = 4096,
3484                 .num_ports = 7,
3485                 .flags = MV88E6XXX_FLAGS_FAMILY_6320,
3486         },
3487
3488         [MV88E6321] = {
3489                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6321,
3490                 .family = MV88E6XXX_FAMILY_6320,
3491                 .name = "Marvell 88E6321",
3492                 .num_databases = 4096,
3493                 .num_ports = 7,
3494                 .flags = MV88E6XXX_FLAGS_FAMILY_6320,
3495         },
3496
3497         [MV88E6350] = {
3498                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6350,
3499                 .family = MV88E6XXX_FAMILY_6351,
3500                 .name = "Marvell 88E6350",
3501                 .num_databases = 4096,
3502                 .num_ports = 7,
3503                 .flags = MV88E6XXX_FLAGS_FAMILY_6351,
3504         },
3505
3506         [MV88E6351] = {
3507                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6351,
3508                 .family = MV88E6XXX_FAMILY_6351,
3509                 .name = "Marvell 88E6351",
3510                 .num_databases = 4096,
3511                 .num_ports = 7,
3512                 .flags = MV88E6XXX_FLAGS_FAMILY_6351,
3513         },
3514
3515         [MV88E6352] = {
3516                 .prod_num = PORT_SWITCH_ID_PROD_NUM_6352,
3517                 .family = MV88E6XXX_FAMILY_6352,
3518                 .name = "Marvell 88E6352",
3519                 .num_databases = 4096,
3520                 .num_ports = 7,
3521                 .flags = MV88E6XXX_FLAGS_FAMILY_6352,
3522         },
3523 };
3524
3525 static const struct mv88e6xxx_info *
3526 mv88e6xxx_lookup_info(unsigned int prod_num, const struct mv88e6xxx_info *table,
3527                       unsigned int num)
3528 {
3529         int i;
3530
3531         for (i = 0; i < num; ++i)
3532                 if (table[i].prod_num == prod_num)
3533                         return &table[i];
3534
3535         return NULL;
3536 }
3537
3538 static const char *mv88e6xxx_drv_probe(struct device *dsa_dev,
3539                                        struct device *host_dev, int sw_addr,
3540                                        void **priv)
3541 {
3542         const struct mv88e6xxx_info *info;
3543         struct mv88e6xxx_priv_state *ps;
3544         struct mii_bus *bus;
3545         const char *name;
3546         int id, prod_num, rev;
3547
3548         bus = dsa_host_dev_to_mii_bus(host_dev);
3549         if (!bus)
3550                 return NULL;
3551
3552         id = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
3553         if (id < 0)
3554                 return NULL;
3555
3556         prod_num = (id & 0xfff0) >> 4;
3557         rev = id & 0x000f;
3558
3559         info = mv88e6xxx_lookup_info(prod_num, mv88e6xxx_table,
3560                                      ARRAY_SIZE(mv88e6xxx_table));
3561         if (!info)
3562                 return NULL;
3563
3564         name = info->name;
3565
3566         ps = devm_kzalloc(dsa_dev, sizeof(*ps), GFP_KERNEL);
3567         if (!ps)
3568                 return NULL;
3569
3570         ps->bus = bus;
3571         ps->sw_addr = sw_addr;
3572         ps->info = info;
3573         mutex_init(&ps->smi_mutex);
3574
3575         *priv = ps;
3576
3577         dev_info(&ps->bus->dev, "switch 0x%x probed: %s, revision %u\n",
3578                  prod_num, name, rev);
3579
3580         return name;
3581 }
3582
3583 struct dsa_switch_driver mv88e6xxx_switch_driver = {
3584         .tag_protocol           = DSA_TAG_PROTO_EDSA,
3585         .probe                  = mv88e6xxx_drv_probe,
3586         .setup                  = mv88e6xxx_setup,
3587         .set_addr               = mv88e6xxx_set_addr,
3588         .phy_read               = mv88e6xxx_phy_read,
3589         .phy_write              = mv88e6xxx_phy_write,
3590         .adjust_link            = mv88e6xxx_adjust_link,
3591         .get_strings            = mv88e6xxx_get_strings,
3592         .get_ethtool_stats      = mv88e6xxx_get_ethtool_stats,
3593         .get_sset_count         = mv88e6xxx_get_sset_count,
3594         .set_eee                = mv88e6xxx_set_eee,
3595         .get_eee                = mv88e6xxx_get_eee,
3596 #ifdef CONFIG_NET_DSA_HWMON
3597         .get_temp               = mv88e6xxx_get_temp,
3598         .get_temp_limit         = mv88e6xxx_get_temp_limit,
3599         .set_temp_limit         = mv88e6xxx_set_temp_limit,
3600         .get_temp_alarm         = mv88e6xxx_get_temp_alarm,
3601 #endif
3602         .get_eeprom_len         = mv88e6xxx_get_eeprom_len,
3603         .get_eeprom             = mv88e6xxx_get_eeprom,
3604         .set_eeprom             = mv88e6xxx_set_eeprom,
3605         .get_regs_len           = mv88e6xxx_get_regs_len,
3606         .get_regs               = mv88e6xxx_get_regs,
3607         .port_bridge_join       = mv88e6xxx_port_bridge_join,
3608         .port_bridge_leave      = mv88e6xxx_port_bridge_leave,
3609         .port_stp_state_set     = mv88e6xxx_port_stp_state_set,
3610         .port_vlan_filtering    = mv88e6xxx_port_vlan_filtering,
3611         .port_vlan_prepare      = mv88e6xxx_port_vlan_prepare,
3612         .port_vlan_add          = mv88e6xxx_port_vlan_add,
3613         .port_vlan_del          = mv88e6xxx_port_vlan_del,
3614         .port_vlan_dump         = mv88e6xxx_port_vlan_dump,
3615         .port_fdb_prepare       = mv88e6xxx_port_fdb_prepare,
3616         .port_fdb_add           = mv88e6xxx_port_fdb_add,
3617         .port_fdb_del           = mv88e6xxx_port_fdb_del,
3618         .port_fdb_dump          = mv88e6xxx_port_fdb_dump,
3619 };
3620
3621 int mv88e6xxx_probe(struct mdio_device *mdiodev)
3622 {
3623         struct device *dev = &mdiodev->dev;
3624         struct device_node *np = dev->of_node;
3625         struct mv88e6xxx_priv_state *ps;
3626         int id, prod_num, rev;
3627         struct dsa_switch *ds;
3628         u32 eeprom_len;
3629         int err;
3630
3631         ds = devm_kzalloc(dev, sizeof(*ds) + sizeof(*ps), GFP_KERNEL);
3632         if (!ds)
3633                 return -ENOMEM;
3634
3635         ps = (struct mv88e6xxx_priv_state *)(ds + 1);
3636         ds->priv = ps;
3637         ds->dev = dev;
3638         ps->dev = dev;
3639         ps->ds = ds;
3640         ps->bus = mdiodev->bus;
3641         ps->sw_addr = mdiodev->addr;
3642         mutex_init(&ps->smi_mutex);
3643
3644         get_device(&ps->bus->dev);
3645
3646         ds->drv = &mv88e6xxx_switch_driver;
3647
3648         id = mv88e6xxx_reg_read(ps, REG_PORT(0), PORT_SWITCH_ID);
3649         if (id < 0)
3650                 return id;
3651
3652         prod_num = (id & 0xfff0) >> 4;
3653         rev = id & 0x000f;
3654
3655         ps->info = mv88e6xxx_lookup_info(prod_num, mv88e6xxx_table,
3656                                          ARRAY_SIZE(mv88e6xxx_table));
3657         if (!ps->info)
3658                 return -ENODEV;
3659
3660         ps->reset = devm_gpiod_get(&mdiodev->dev, "reset", GPIOD_ASIS);
3661         if (IS_ERR(ps->reset)) {
3662                 err = PTR_ERR(ps->reset);
3663                 if (err == -ENOENT) {
3664                         /* Optional, so not an error */
3665                         ps->reset = NULL;
3666                 } else {
3667                         return err;
3668                 }
3669         }
3670
3671         if (mv88e6xxx_has(ps, MV88E6XXX_FLAG_EEPROM) &&
3672             !of_property_read_u32(np, "eeprom-length", &eeprom_len))
3673                 ps->eeprom_len = eeprom_len;
3674
3675         dev_set_drvdata(dev, ds);
3676
3677         dev_info(dev, "switch 0x%x probed: %s, revision %u\n",
3678                  prod_num, ps->info->name, rev);
3679
3680         return 0;
3681 }
3682
3683 static void mv88e6xxx_remove(struct mdio_device *mdiodev)
3684 {
3685         struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
3686         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
3687
3688         put_device(&ps->bus->dev);
3689 }
3690
3691 static const struct of_device_id mv88e6xxx_of_match[] = {
3692         { .compatible = "marvell,mv88e6085" },
3693         { /* sentinel */ },
3694 };
3695
3696 MODULE_DEVICE_TABLE(of, mv88e6xxx_of_match);
3697
3698 static struct mdio_driver mv88e6xxx_driver = {
3699         .probe  = mv88e6xxx_probe,
3700         .remove = mv88e6xxx_remove,
3701         .mdiodrv.driver = {
3702                 .name = "mv88e6085",
3703                 .of_match_table = mv88e6xxx_of_match,
3704         },
3705 };
3706
3707 static int __init mv88e6xxx_init(void)
3708 {
3709         register_switch_driver(&mv88e6xxx_switch_driver);
3710         return mdio_driver_register(&mv88e6xxx_driver);
3711 }
3712 module_init(mv88e6xxx_init);
3713
3714 static void __exit mv88e6xxx_cleanup(void)
3715 {
3716         mdio_driver_unregister(&mv88e6xxx_driver);
3717         unregister_switch_driver(&mv88e6xxx_switch_driver);
3718 }
3719 module_exit(mv88e6xxx_cleanup);
3720
3721 MODULE_AUTHOR("Lennert Buytenhek <buytenh@wantstofly.org>");
3722 MODULE_DESCRIPTION("Driver for Marvell 88E6XXX ethernet switch chips");
3723 MODULE_LICENSE("GPL");