drivers/net/dsa/mv88e6xxx.c

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