drivers/net/dsa/mv88e6xxx/port.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Marvell 88E6xxx Switch Port Registers support
   4  *
   5  * Copyright (c) 2008 Marvell Semiconductor
   6  *
   7  * Copyright (c) 2016-2017 Savoir-faire Linux Inc.
   8  *      Vivien Didelot <vivien.didelot@savoirfairelinux.com>
   9  */
  10
  11 #include <linux/bitfield.h>
  12 #include <linux/if_bridge.h>
  13 #include <linux/phy.h>
  14 #include <linux/phylink.h>
  15
  16 #include "chip.h"
  17 #include "port.h"
  18 #include "serdes.h"
  19
  20 int mv88e6xxx_port_read(struct mv88e6xxx_chip *chip, int port, int reg,
  21                         u16 *val)
  22 {
  23         int addr = chip->info->port_base_addr + port;
  24
  25         return mv88e6xxx_read(chip, addr, reg, val);
  26 }
  27
  28 int mv88e6xxx_port_write(struct mv88e6xxx_chip *chip, int port, int reg,
  29                          u16 val)
  30 {
  31         int addr = chip->info->port_base_addr + port;
  32
  33         return mv88e6xxx_write(chip, addr, reg, val);
  34 }
  35
  36 /* Offset 0x00: MAC (or PCS or Physical) Status Register
  37  *
  38  * For most devices, this is read only. However the 6185 has the MyPause
  39  * bit read/write.
  40  */
  41 int mv88e6185_port_set_pause(struct mv88e6xxx_chip *chip, int port,
  42                              int pause)
  43 {
  44         u16 reg;
  45         int err;
  46
  47         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
  48         if (err)
  49                 return err;
  50
  51         if (pause)
  52                 reg |= MV88E6XXX_PORT_STS_MY_PAUSE;
  53         else
  54                 reg &= ~MV88E6XXX_PORT_STS_MY_PAUSE;
  55
  56         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_STS, reg);
  57 }
  58
  59 /* Offset 0x01: MAC (or PCS or Physical) Control Register
  60  *
  61  * Link, Duplex and Flow Control have one force bit, one value bit.
  62  *
  63  * For port's MAC speed, ForceSpd (or SpdValue) bits 1:0 program the value.
  64  * Alternative values require the 200BASE (or AltSpeed) bit 12 set.
  65  * Newer chips need a ForcedSpd bit 13 set to consider the value.
  66  */
  67
  68 static int mv88e6xxx_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
  69                                           phy_interface_t mode)
  70 {
  71         u16 reg;
  72         int err;
  73
  74         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
  75         if (err)
  76                 return err;
  77
  78         reg &= ~(MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK |
  79                  MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK);
  80
  81         switch (mode) {
  82         case PHY_INTERFACE_MODE_RGMII_RXID:
  83                 reg |= MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK;
  84                 break;
  85         case PHY_INTERFACE_MODE_RGMII_TXID:
  86                 reg |= MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK;
  87                 break;
  88         case PHY_INTERFACE_MODE_RGMII_ID:
  89                 reg |= MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK |
  90                         MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK;
  91                 break;
  92         case PHY_INTERFACE_MODE_RGMII:
  93                 break;
  94         default:
  95                 return 0;
  96         }
  97
  98         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
  99         if (err)
 100                 return err;
 101
 102         dev_dbg(chip->dev, "p%d: delay RXCLK %s, TXCLK %s\n", port,
 103                 reg & MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_RXCLK ? "yes" : "no",
 104                 reg & MV88E6XXX_PORT_MAC_CTL_RGMII_DELAY_TXCLK ? "yes" : "no");
 105
 106         return 0;
 107 }
 108
 109 int mv88e6352_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
 110                                    phy_interface_t mode)
 111 {
 112         if (port < 5)
 113                 return -EOPNOTSUPP;
 114
 115         return mv88e6xxx_port_set_rgmii_delay(chip, port, mode);
 116 }
 117
 118 int mv88e6390_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
 119                                    phy_interface_t mode)
 120 {
 121         if (port != 0)
 122                 return -EOPNOTSUPP;
 123
 124         return mv88e6xxx_port_set_rgmii_delay(chip, port, mode);
 125 }
 126
 127 int mv88e6xxx_port_set_link(struct mv88e6xxx_chip *chip, int port, int link)
 128 {
 129         u16 reg;
 130         int err;
 131
 132         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
 133         if (err)
 134                 return err;
 135
 136         reg &= ~(MV88E6XXX_PORT_MAC_CTL_FORCE_LINK |
 137                  MV88E6XXX_PORT_MAC_CTL_LINK_UP);
 138
 139         switch (link) {
 140         case LINK_FORCED_DOWN:
 141                 reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_LINK;
 142                 break;
 143         case LINK_FORCED_UP:
 144                 reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_LINK |
 145                         MV88E6XXX_PORT_MAC_CTL_LINK_UP;
 146                 break;
 147         case LINK_UNFORCED:
 148                 /* normal link detection */
 149                 break;
 150         default:
 151                 return -EINVAL;
 152         }
 153
 154         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
 155         if (err)
 156                 return err;
 157
 158         dev_dbg(chip->dev, "p%d: %s link %s\n", port,
 159                 reg & MV88E6XXX_PORT_MAC_CTL_FORCE_LINK ? "Force" : "Unforce",
 160                 reg & MV88E6XXX_PORT_MAC_CTL_LINK_UP ? "up" : "down");
 161
 162         return 0;
 163 }
 164
 165 int mv88e6xxx_port_sync_link(struct mv88e6xxx_chip *chip, int port, unsigned int mode, bool isup)
 166 {
 167         const struct mv88e6xxx_ops *ops = chip->info->ops;
 168         int err = 0;
 169         int link;
 170
 171         if (isup)
 172                 link = LINK_FORCED_UP;
 173         else
 174                 link = LINK_FORCED_DOWN;
 175
 176         if (ops->port_set_link)
 177                 err = ops->port_set_link(chip, port, link);
 178
 179         return err;
 180 }
 181
 182 int mv88e6185_port_sync_link(struct mv88e6xxx_chip *chip, int port, unsigned int mode, bool isup)
 183 {
 184         const struct mv88e6xxx_ops *ops = chip->info->ops;
 185         int err = 0;
 186         int link;
 187
 188         if (mode == MLO_AN_INBAND)
 189                 link = LINK_UNFORCED;
 190         else if (isup)
 191                 link = LINK_FORCED_UP;
 192         else
 193                 link = LINK_FORCED_DOWN;
 194
 195         if (ops->port_set_link)
 196                 err = ops->port_set_link(chip, port, link);
 197
 198         return err;
 199 }
 200
 201 static int mv88e6xxx_port_set_speed_duplex(struct mv88e6xxx_chip *chip,
 202                                            int port, int speed, bool alt_bit,
 203                                            bool force_bit, int duplex)
 204 {
 205         u16 reg, ctrl;
 206         int err;
 207
 208         switch (speed) {
 209         case 10:
 210                 ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_10;
 211                 break;
 212         case 100:
 213                 ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_100;
 214                 break;
 215         case 200:
 216                 if (alt_bit)
 217                         ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_100 |
 218                                 MV88E6390_PORT_MAC_CTL_ALTSPEED;
 219                 else
 220                         ctrl = MV88E6065_PORT_MAC_CTL_SPEED_200;
 221                 break;
 222         case 1000:
 223                 ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_1000;
 224                 break;
 225         case 2500:
 226                 if (alt_bit)
 227                         ctrl = MV88E6390_PORT_MAC_CTL_SPEED_10000 |
 228                                 MV88E6390_PORT_MAC_CTL_ALTSPEED;
 229                 else
 230                         ctrl = MV88E6390_PORT_MAC_CTL_SPEED_10000;
 231                 break;
 232         case 10000:
 233                 /* all bits set, fall through... */
 234         case SPEED_UNFORCED:
 235                 ctrl = MV88E6XXX_PORT_MAC_CTL_SPEED_UNFORCED;
 236                 break;
 237         default:
 238                 return -EOPNOTSUPP;
 239         }
 240
 241         switch (duplex) {
 242         case DUPLEX_HALF:
 243                 ctrl |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX;
 244                 break;
 245         case DUPLEX_FULL:
 246                 ctrl |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
 247                         MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL;
 248                 break;
 249         case DUPLEX_UNFORCED:
 250                 /* normal duplex detection */
 251                 break;
 252         default:
 253                 return -EOPNOTSUPP;
 254         }
 255
 256         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
 257         if (err)
 258                 return err;
 259
 260         reg &= ~(MV88E6XXX_PORT_MAC_CTL_SPEED_MASK |
 261                  MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
 262                  MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL);
 263
 264         if (alt_bit)
 265                 reg &= ~MV88E6390_PORT_MAC_CTL_ALTSPEED;
 266         if (force_bit) {
 267                 reg &= ~MV88E6390_PORT_MAC_CTL_FORCE_SPEED;
 268                 if (speed != SPEED_UNFORCED)
 269                         ctrl |= MV88E6390_PORT_MAC_CTL_FORCE_SPEED;
 270         }
 271         reg |= ctrl;
 272
 273         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
 274         if (err)
 275                 return err;
 276
 277         if (speed)
 278                 dev_dbg(chip->dev, "p%d: Speed set to %d Mbps\n", port, speed);
 279         else
 280                 dev_dbg(chip->dev, "p%d: Speed unforced\n", port);
 281         dev_dbg(chip->dev, "p%d: %s %s duplex\n", port,
 282                 reg & MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX ? "Force" : "Unforce",
 283                 reg & MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL ? "full" : "half");
 284
 285         return 0;
 286 }
 287
 288 /* Support 10, 100, 200 Mbps (e.g. 88E6065 family) */
 289 int mv88e6065_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 290                                     int speed, int duplex)
 291 {
 292         if (speed == SPEED_MAX)
 293                 speed = 200;
 294
 295         if (speed > 200)
 296                 return -EOPNOTSUPP;
 297
 298         /* Setting 200 Mbps on port 0 to 3 selects 100 Mbps */
 299         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, false, false,
 300                                                duplex);
 301 }
 302
 303 /* Support 10, 100, 1000 Mbps (e.g. 88E6185 family) */
 304 int mv88e6185_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 305                                     int speed, int duplex)
 306 {
 307         if (speed == SPEED_MAX)
 308                 speed = 1000;
 309
 310         if (speed == 200 || speed > 1000)
 311                 return -EOPNOTSUPP;
 312
 313         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, false, false,
 314                                                duplex);
 315 }
 316
 317 /* Support 10, 100 Mbps (e.g. 88E6250 family) */
 318 int mv88e6250_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 319                                     int speed, int duplex)
 320 {
 321         if (speed == SPEED_MAX)
 322                 speed = 100;
 323
 324         if (speed > 100)
 325                 return -EOPNOTSUPP;
 326
 327         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, false, false,
 328                                                duplex);
 329 }
 330
 331 /* Support 10, 100, 200, 1000, 2500 Mbps (e.g. 88E6341) */
 332 int mv88e6341_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 333                                     int speed, int duplex)
 334 {
 335         if (speed == SPEED_MAX)
 336                 speed = port < 5 ? 1000 : 2500;
 337
 338         if (speed > 2500)
 339                 return -EOPNOTSUPP;
 340
 341         if (speed == 200 && port != 0)
 342                 return -EOPNOTSUPP;
 343
 344         if (speed == 2500 && port < 5)
 345                 return -EOPNOTSUPP;
 346
 347         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, !port, true,
 348                                                duplex);
 349 }
 350
 351 phy_interface_t mv88e6341_port_max_speed_mode(int port)
 352 {
 353         if (port == 5)
 354                 return PHY_INTERFACE_MODE_2500BASEX;
 355
 356         return PHY_INTERFACE_MODE_NA;
 357 }
 358
 359 /* Support 10, 100, 200, 1000 Mbps (e.g. 88E6352 family) */
 360 int mv88e6352_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 361                                     int speed, int duplex)
 362 {
 363         if (speed == SPEED_MAX)
 364                 speed = 1000;
 365
 366         if (speed > 1000)
 367                 return -EOPNOTSUPP;
 368
 369         if (speed == 200 && port < 5)
 370                 return -EOPNOTSUPP;
 371
 372         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, true, false,
 373                                                duplex);
 374 }
 375
 376 /* Support 10, 100, 200, 1000, 2500 Mbps (e.g. 88E6390) */
 377 int mv88e6390_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 378                                     int speed, int duplex)
 379 {
 380         if (speed == SPEED_MAX)
 381                 speed = port < 9 ? 1000 : 2500;
 382
 383         if (speed > 2500)
 384                 return -EOPNOTSUPP;
 385
 386         if (speed == 200 && port != 0)
 387                 return -EOPNOTSUPP;
 388
 389         if (speed == 2500 && port < 9)
 390                 return -EOPNOTSUPP;
 391
 392         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, true, true,
 393                                                duplex);
 394 }
 395
 396 phy_interface_t mv88e6390_port_max_speed_mode(int port)
 397 {
 398         if (port == 9 || port == 10)
 399                 return PHY_INTERFACE_MODE_2500BASEX;
 400
 401         return PHY_INTERFACE_MODE_NA;
 402 }
 403
 404 /* Support 10, 100, 200, 1000, 2500, 10000 Mbps (e.g. 88E6190X) */
 405 int mv88e6390x_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
 406                                      int speed, int duplex)
 407 {
 408         if (speed == SPEED_MAX)
 409                 speed = port < 9 ? 1000 : 10000;
 410
 411         if (speed == 200 && port != 0)
 412                 return -EOPNOTSUPP;
 413
 414         if (speed >= 2500 && port < 9)
 415                 return -EOPNOTSUPP;
 416
 417         return mv88e6xxx_port_set_speed_duplex(chip, port, speed, true, true,
 418                                                duplex);
 419 }
 420
 421 phy_interface_t mv88e6390x_port_max_speed_mode(int port)
 422 {
 423         if (port == 9 || port == 10)
 424                 return PHY_INTERFACE_MODE_XAUI;
 425
 426         return PHY_INTERFACE_MODE_NA;
 427 }
 428
 429 static int mv88e6xxx_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 430                                     phy_interface_t mode, bool force)
 431 {
 432         u16 cmode;
 433         int lane;
 434         u16 reg;
 435         int err;
 436
 437         /* Default to a slow mode, so freeing up SERDES interfaces for
 438          * other ports which might use them for SFPs.
 439          */
 440         if (mode == PHY_INTERFACE_MODE_NA)
 441                 mode = PHY_INTERFACE_MODE_1000BASEX;
 442
 443         switch (mode) {
 444         case PHY_INTERFACE_MODE_1000BASEX:
 445                 cmode = MV88E6XXX_PORT_STS_CMODE_1000BASEX;
 446                 break;
 447         case PHY_INTERFACE_MODE_SGMII:
 448                 cmode = MV88E6XXX_PORT_STS_CMODE_SGMII;
 449                 break;
 450         case PHY_INTERFACE_MODE_2500BASEX:
 451                 cmode = MV88E6XXX_PORT_STS_CMODE_2500BASEX;
 452                 break;
 453         case PHY_INTERFACE_MODE_XGMII:
 454         case PHY_INTERFACE_MODE_XAUI:
 455                 cmode = MV88E6XXX_PORT_STS_CMODE_XAUI;
 456                 break;
 457         case PHY_INTERFACE_MODE_RXAUI:
 458                 cmode = MV88E6XXX_PORT_STS_CMODE_RXAUI;
 459                 break;
 460         default:
 461                 cmode = 0;
 462         }
 463
 464         /* cmode doesn't change, nothing to do for us unless forced */
 465         if (cmode == chip->ports[port].cmode && !force)
 466                 return 0;
 467
 468         lane = mv88e6xxx_serdes_get_lane(chip, port);
 469         if (lane >= 0) {
 470                 if (chip->ports[port].serdes_irq) {
 471                         err = mv88e6xxx_serdes_irq_disable(chip, port, lane);
 472                         if (err)
 473                                 return err;
 474                 }
 475
 476                 err = mv88e6xxx_serdes_power_down(chip, port, lane);
 477                 if (err)
 478                         return err;
 479         }
 480
 481         chip->ports[port].cmode = 0;
 482
 483         if (cmode) {
 484                 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
 485                 if (err)
 486                         return err;
 487
 488                 reg &= ~MV88E6XXX_PORT_STS_CMODE_MASK;
 489                 reg |= cmode;
 490
 491                 err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_STS, reg);
 492                 if (err)
 493                         return err;
 494
 495                 chip->ports[port].cmode = cmode;
 496
 497                 lane = mv88e6xxx_serdes_get_lane(chip, port);
 498                 if (lane < 0)
 499                         return lane;
 500
 501                 err = mv88e6xxx_serdes_power_up(chip, port, lane);
 502                 if (err)
 503                         return err;
 504
 505                 if (chip->ports[port].serdes_irq) {
 506                         err = mv88e6xxx_serdes_irq_enable(chip, port, lane);
 507                         if (err)
 508                                 return err;
 509                 }
 510         }
 511
 512         return 0;
 513 }
 514
 515 int mv88e6390x_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 516                               phy_interface_t mode)
 517 {
 518         if (port != 9 && port != 10)
 519                 return -EOPNOTSUPP;
 520
 521         return mv88e6xxx_port_set_cmode(chip, port, mode, false);
 522 }
 523
 524 int mv88e6390_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 525                              phy_interface_t mode)
 526 {
 527         if (port != 9 && port != 10)
 528                 return -EOPNOTSUPP;
 529
 530         switch (mode) {
 531         case PHY_INTERFACE_MODE_NA:
 532                 return 0;
 533         case PHY_INTERFACE_MODE_XGMII:
 534         case PHY_INTERFACE_MODE_XAUI:
 535         case PHY_INTERFACE_MODE_RXAUI:
 536                 return -EINVAL;
 537         default:
 538                 break;
 539         }
 540
 541         return mv88e6xxx_port_set_cmode(chip, port, mode, false);
 542 }
 543
 544 static int mv88e6341_port_set_cmode_writable(struct mv88e6xxx_chip *chip,
 545                                              int port)
 546 {
 547         int err, addr;
 548         u16 reg, bits;
 549
 550         if (port != 5)
 551                 return -EOPNOTSUPP;
 552
 553         addr = chip->info->port_base_addr + port;
 554
 555         err = mv88e6xxx_port_hidden_read(chip, 0x7, addr, 0, &reg);
 556         if (err)
 557                 return err;
 558
 559         bits = MV88E6341_PORT_RESERVED_1A_FORCE_CMODE |
 560                MV88E6341_PORT_RESERVED_1A_SGMII_AN;
 561
 562         if ((reg & bits) == bits)
 563                 return 0;
 564
 565         reg |= bits;
 566         return mv88e6xxx_port_hidden_write(chip, 0x7, addr, 0, reg);
 567 }
 568
 569 int mv88e6341_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 570                              phy_interface_t mode)
 571 {
 572         int err;
 573
 574         if (port != 5)
 575                 return -EOPNOTSUPP;
 576
 577         switch (mode) {
 578         case PHY_INTERFACE_MODE_NA:
 579                 return 0;
 580         case PHY_INTERFACE_MODE_XGMII:
 581         case PHY_INTERFACE_MODE_XAUI:
 582         case PHY_INTERFACE_MODE_RXAUI:
 583                 return -EINVAL;
 584         default:
 585                 break;
 586         }
 587
 588         err = mv88e6341_port_set_cmode_writable(chip, port);
 589         if (err)
 590                 return err;
 591
 592         return mv88e6xxx_port_set_cmode(chip, port, mode, true);
 593 }
 594
 595 int mv88e6185_port_get_cmode(struct mv88e6xxx_chip *chip, int port, u8 *cmode)
 596 {
 597         int err;
 598         u16 reg;
 599
 600         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
 601         if (err)
 602                 return err;
 603
 604         *cmode = reg & MV88E6185_PORT_STS_CMODE_MASK;
 605
 606         return 0;
 607 }
 608
 609 int mv88e6352_port_get_cmode(struct mv88e6xxx_chip *chip, int port, u8 *cmode)
 610 {
 611         int err;
 612         u16 reg;
 613
 614         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
 615         if (err)
 616                 return err;
 617
 618         *cmode = reg & MV88E6XXX_PORT_STS_CMODE_MASK;
 619
 620         return 0;
 621 }
 622
 623 /* Offset 0x02: Jamming Control
 624  *
 625  * Do not limit the period of time that this port can be paused for by
 626  * the remote end or the period of time that this port can pause the
 627  * remote end.
 628  */
 629 int mv88e6097_port_pause_limit(struct mv88e6xxx_chip *chip, int port, u8 in,
 630                                u8 out)
 631 {
 632         return mv88e6xxx_port_write(chip, port, MV88E6097_PORT_JAM_CTL,
 633                                     out << 8 | in);
 634 }
 635
 636 int mv88e6390_port_pause_limit(struct mv88e6xxx_chip *chip, int port, u8 in,
 637                                u8 out)
 638 {
 639         int err;
 640
 641         err = mv88e6xxx_port_write(chip, port, MV88E6390_PORT_FLOW_CTL,
 642                                    MV88E6390_PORT_FLOW_CTL_UPDATE |
 643                                    MV88E6390_PORT_FLOW_CTL_LIMIT_IN | in);
 644         if (err)
 645                 return err;
 646
 647         return mv88e6xxx_port_write(chip, port, MV88E6390_PORT_FLOW_CTL,
 648                                     MV88E6390_PORT_FLOW_CTL_UPDATE |
 649                                     MV88E6390_PORT_FLOW_CTL_LIMIT_OUT | out);
 650 }
 651
 652 /* Offset 0x04: Port Control Register */
 653
 654 static const char * const mv88e6xxx_port_state_names[] = {
 655         [MV88E6XXX_PORT_CTL0_STATE_DISABLED] = "Disabled",
 656         [MV88E6XXX_PORT_CTL0_STATE_BLOCKING] = "Blocking/Listening",
 657         [MV88E6XXX_PORT_CTL0_STATE_LEARNING] = "Learning",
 658         [MV88E6XXX_PORT_CTL0_STATE_FORWARDING] = "Forwarding",
 659 };
 660
 661 int mv88e6xxx_port_set_state(struct mv88e6xxx_chip *chip, int port, u8 state)
 662 {
 663         u16 reg;
 664         int err;
 665
 666         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 667         if (err)
 668                 return err;
 669
 670         reg &= ~MV88E6XXX_PORT_CTL0_STATE_MASK;
 671
 672         switch (state) {
 673         case BR_STATE_DISABLED:
 674                 state = MV88E6XXX_PORT_CTL0_STATE_DISABLED;
 675                 break;
 676         case BR_STATE_BLOCKING:
 677         case BR_STATE_LISTENING:
 678                 state = MV88E6XXX_PORT_CTL0_STATE_BLOCKING;
 679                 break;
 680         case BR_STATE_LEARNING:
 681                 state = MV88E6XXX_PORT_CTL0_STATE_LEARNING;
 682                 break;
 683         case BR_STATE_FORWARDING:
 684                 state = MV88E6XXX_PORT_CTL0_STATE_FORWARDING;
 685                 break;
 686         default:
 687                 return -EINVAL;
 688         }
 689
 690         reg |= state;
 691
 692         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 693         if (err)
 694                 return err;
 695
 696         dev_dbg(chip->dev, "p%d: PortState set to %s\n", port,
 697                 mv88e6xxx_port_state_names[state]);
 698
 699         return 0;
 700 }
 701
 702 int mv88e6xxx_port_set_egress_mode(struct mv88e6xxx_chip *chip, int port,
 703                                    enum mv88e6xxx_egress_mode mode)
 704 {
 705         int err;
 706         u16 reg;
 707
 708         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 709         if (err)
 710                 return err;
 711
 712         reg &= ~MV88E6XXX_PORT_CTL0_EGRESS_MODE_MASK;
 713
 714         switch (mode) {
 715         case MV88E6XXX_EGRESS_MODE_UNMODIFIED:
 716                 reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_UNMODIFIED;
 717                 break;
 718         case MV88E6XXX_EGRESS_MODE_UNTAGGED:
 719                 reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_UNTAGGED;
 720                 break;
 721         case MV88E6XXX_EGRESS_MODE_TAGGED:
 722                 reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_TAGGED;
 723                 break;
 724         case MV88E6XXX_EGRESS_MODE_ETHERTYPE:
 725                 reg |= MV88E6XXX_PORT_CTL0_EGRESS_MODE_ETHER_TYPE_DSA;
 726                 break;
 727         default:
 728                 return -EINVAL;
 729         }
 730
 731         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 732 }
 733
 734 int mv88e6085_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 735                                   enum mv88e6xxx_frame_mode mode)
 736 {
 737         int err;
 738         u16 reg;
 739
 740         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 741         if (err)
 742                 return err;
 743
 744         reg &= ~MV88E6XXX_PORT_CTL0_FRAME_MODE_MASK;
 745
 746         switch (mode) {
 747         case MV88E6XXX_FRAME_MODE_NORMAL:
 748                 reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_NORMAL;
 749                 break;
 750         case MV88E6XXX_FRAME_MODE_DSA:
 751                 reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_DSA;
 752                 break;
 753         default:
 754                 return -EINVAL;
 755         }
 756
 757         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 758 }
 759
 760 int mv88e6351_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 761                                   enum mv88e6xxx_frame_mode mode)
 762 {
 763         int err;
 764         u16 reg;
 765
 766         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 767         if (err)
 768                 return err;
 769
 770         reg &= ~MV88E6XXX_PORT_CTL0_FRAME_MODE_MASK;
 771
 772         switch (mode) {
 773         case MV88E6XXX_FRAME_MODE_NORMAL:
 774                 reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_NORMAL;
 775                 break;
 776         case MV88E6XXX_FRAME_MODE_DSA:
 777                 reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_DSA;
 778                 break;
 779         case MV88E6XXX_FRAME_MODE_PROVIDER:
 780                 reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_PROVIDER;
 781                 break;
 782         case MV88E6XXX_FRAME_MODE_ETHERTYPE:
 783                 reg |= MV88E6XXX_PORT_CTL0_FRAME_MODE_ETHER_TYPE_DSA;
 784                 break;
 785         default:
 786                 return -EINVAL;
 787         }
 788
 789         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 790 }
 791
 792 int mv88e6185_port_set_forward_unknown(struct mv88e6xxx_chip *chip,
 793                                        int port, bool unicast)
 794 {
 795         int err;
 796         u16 reg;
 797
 798         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 799         if (err)
 800                 return err;
 801
 802         if (unicast)
 803                 reg |= MV88E6185_PORT_CTL0_FORWARD_UNKNOWN;
 804         else
 805                 reg &= ~MV88E6185_PORT_CTL0_FORWARD_UNKNOWN;
 806
 807         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 808 }
 809
 810 int mv88e6352_port_set_ucast_flood(struct mv88e6xxx_chip *chip, int port,
 811                                    bool unicast)
 812 {
 813         int err;
 814         u16 reg;
 815
 816         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 817         if (err)
 818                 return err;
 819
 820         if (unicast)
 821                 reg |= MV88E6352_PORT_CTL0_EGRESS_FLOODS_UC;
 822         else
 823                 reg &= ~MV88E6352_PORT_CTL0_EGRESS_FLOODS_UC;
 824
 825         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 826 }
 827
 828 int mv88e6352_port_set_mcast_flood(struct mv88e6xxx_chip *chip, int port,
 829                                    bool multicast)
 830 {
 831         int err;
 832         u16 reg;
 833
 834         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL0, &reg);
 835         if (err)
 836                 return err;
 837
 838         if (multicast)
 839                 reg |= MV88E6352_PORT_CTL0_EGRESS_FLOODS_MC;
 840         else
 841                 reg &= ~MV88E6352_PORT_CTL0_EGRESS_FLOODS_MC;
 842
 843         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL0, reg);
 844 }
 845
 846 /* Offset 0x05: Port Control 1 */
 847
 848 int mv88e6xxx_port_set_message_port(struct mv88e6xxx_chip *chip, int port,
 849                                     bool message_port)
 850 {
 851         u16 val;
 852         int err;
 853
 854         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1, &val);
 855         if (err)
 856                 return err;
 857
 858         if (message_port)
 859                 val |= MV88E6XXX_PORT_CTL1_MESSAGE_PORT;
 860         else
 861                 val &= ~MV88E6XXX_PORT_CTL1_MESSAGE_PORT;
 862
 863         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL1, val);
 864 }
 865
 866 int mv88e6xxx_port_set_trunk(struct mv88e6xxx_chip *chip, int port,
 867                              bool trunk, u8 id)
 868 {
 869         u16 val;
 870         int err;
 871
 872         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1, &val);
 873         if (err)
 874                 return err;
 875
 876         val &= ~MV88E6XXX_PORT_CTL1_TRUNK_ID_MASK;
 877
 878         if (trunk)
 879                 val |= MV88E6XXX_PORT_CTL1_TRUNK_PORT |
 880                         (id << MV88E6XXX_PORT_CTL1_TRUNK_ID_SHIFT);
 881         else
 882                 val &= ~MV88E6XXX_PORT_CTL1_TRUNK_PORT;
 883
 884         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL1, val);
 885 }
 886
 887 /* Offset 0x06: Port Based VLAN Map */
 888
 889 int mv88e6xxx_port_set_vlan_map(struct mv88e6xxx_chip *chip, int port, u16 map)
 890 {
 891         const u16 mask = mv88e6xxx_port_mask(chip);
 892         u16 reg;
 893         int err;
 894
 895         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_BASE_VLAN, &reg);
 896         if (err)
 897                 return err;
 898
 899         reg &= ~mask;
 900         reg |= map & mask;
 901
 902         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_BASE_VLAN, reg);
 903         if (err)
 904                 return err;
 905
 906         dev_dbg(chip->dev, "p%d: VLANTable set to %.3x\n", port, map);
 907
 908         return 0;
 909 }
 910
 911 int mv88e6xxx_port_get_fid(struct mv88e6xxx_chip *chip, int port, u16 *fid)
 912 {
 913         const u16 upper_mask = (mv88e6xxx_num_databases(chip) - 1) >> 4;
 914         u16 reg;
 915         int err;
 916
 917         /* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
 918         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_BASE_VLAN, &reg);
 919         if (err)
 920                 return err;
 921
 922         *fid = (reg & 0xf000) >> 12;
 923
 924         /* Port's default FID upper bits are located in reg 0x05, offset 0 */
 925         if (upper_mask) {
 926                 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1,
 927                                           &reg);
 928                 if (err)
 929                         return err;
 930
 931                 *fid |= (reg & upper_mask) << 4;
 932         }
 933
 934         return 0;
 935 }
 936
 937 int mv88e6xxx_port_set_fid(struct mv88e6xxx_chip *chip, int port, u16 fid)
 938 {
 939         const u16 upper_mask = (mv88e6xxx_num_databases(chip) - 1) >> 4;
 940         u16 reg;
 941         int err;
 942
 943         if (fid >= mv88e6xxx_num_databases(chip))
 944                 return -EINVAL;
 945
 946         /* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
 947         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_BASE_VLAN, &reg);
 948         if (err)
 949                 return err;
 950
 951         reg &= 0x0fff;
 952         reg |= (fid & 0x000f) << 12;
 953
 954         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_BASE_VLAN, reg);
 955         if (err)
 956                 return err;
 957
 958         /* Port's default FID upper bits are located in reg 0x05, offset 0 */
 959         if (upper_mask) {
 960                 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL1,
 961                                           &reg);
 962                 if (err)
 963                         return err;
 964
 965                 reg &= ~upper_mask;
 966                 reg |= (fid >> 4) & upper_mask;
 967
 968                 err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL1,
 969                                            reg);
 970                 if (err)
 971                         return err;
 972         }
 973
 974         dev_dbg(chip->dev, "p%d: FID set to %u\n", port, fid);
 975
 976         return 0;
 977 }
 978
 979 /* Offset 0x07: Default Port VLAN ID & Priority */
 980
 981 int mv88e6xxx_port_get_pvid(struct mv88e6xxx_chip *chip, int port, u16 *pvid)
 982 {
 983         u16 reg;
 984         int err;
 985
 986         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN,
 987                                   &reg);
 988         if (err)
 989                 return err;
 990
 991         *pvid = reg & MV88E6XXX_PORT_DEFAULT_VLAN_MASK;
 992
 993         return 0;
 994 }
 995
 996 int mv88e6xxx_port_set_pvid(struct mv88e6xxx_chip *chip, int port, u16 pvid)
 997 {
 998         u16 reg;
 999         int err;
1000
1001         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN,
1002                                   &reg);
1003         if (err)
1004                 return err;
1005
1006         reg &= ~MV88E6XXX_PORT_DEFAULT_VLAN_MASK;
1007         reg |= pvid & MV88E6XXX_PORT_DEFAULT_VLAN_MASK;
1008
1009         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_DEFAULT_VLAN,
1010                                    reg);
1011         if (err)
1012                 return err;
1013
1014         dev_dbg(chip->dev, "p%d: DefaultVID set to %u\n", port, pvid);
1015
1016         return 0;
1017 }
1018
1019 /* Offset 0x08: Port Control 2 Register */
1020
1021 static const char * const mv88e6xxx_port_8021q_mode_names[] = {
1022         [MV88E6XXX_PORT_CTL2_8021Q_MODE_DISABLED] = "Disabled",
1023         [MV88E6XXX_PORT_CTL2_8021Q_MODE_FALLBACK] = "Fallback",
1024         [MV88E6XXX_PORT_CTL2_8021Q_MODE_CHECK] = "Check",
1025         [MV88E6XXX_PORT_CTL2_8021Q_MODE_SECURE] = "Secure",
1026 };
1027
1028 int mv88e6185_port_set_default_forward(struct mv88e6xxx_chip *chip,
1029                                        int port, bool multicast)
1030 {
1031         int err;
1032         u16 reg;
1033
1034         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1035         if (err)
1036                 return err;
1037
1038         if (multicast)
1039                 reg |= MV88E6XXX_PORT_CTL2_DEFAULT_FORWARD;
1040         else
1041                 reg &= ~MV88E6XXX_PORT_CTL2_DEFAULT_FORWARD;
1042
1043         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1044 }
1045
1046 int mv88e6095_port_set_upstream_port(struct mv88e6xxx_chip *chip, int port,
1047                                      int upstream_port)
1048 {
1049         int err;
1050         u16 reg;
1051
1052         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1053         if (err)
1054                 return err;
1055
1056         reg &= ~MV88E6095_PORT_CTL2_CPU_PORT_MASK;
1057         reg |= upstream_port;
1058
1059         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1060 }
1061
1062 int mv88e6xxx_port_set_mirror(struct mv88e6xxx_chip *chip, int port,
1063                               enum mv88e6xxx_egress_direction direction,
1064                               bool mirror)
1065 {
1066         bool *mirror_port;
1067         u16 reg;
1068         u16 bit;
1069         int err;
1070
1071         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1072         if (err)
1073                 return err;
1074
1075         switch (direction) {
1076         case MV88E6XXX_EGRESS_DIR_INGRESS:
1077                 bit = MV88E6XXX_PORT_CTL2_INGRESS_MONITOR;
1078                 mirror_port = &chip->ports[port].mirror_ingress;
1079                 break;
1080         case MV88E6XXX_EGRESS_DIR_EGRESS:
1081                 bit = MV88E6XXX_PORT_CTL2_EGRESS_MONITOR;
1082                 mirror_port = &chip->ports[port].mirror_egress;
1083                 break;
1084         default:
1085                 return -EINVAL;
1086         }
1087
1088         reg &= ~bit;
1089         if (mirror)
1090                 reg |= bit;
1091
1092         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1093         if (!err)
1094                 *mirror_port = mirror;
1095
1096         return err;
1097 }
1098
1099 int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
1100                                   u16 mode)
1101 {
1102         u16 reg;
1103         int err;
1104
1105         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1106         if (err)
1107                 return err;
1108
1109         reg &= ~MV88E6XXX_PORT_CTL2_8021Q_MODE_MASK;
1110         reg |= mode & MV88E6XXX_PORT_CTL2_8021Q_MODE_MASK;
1111
1112         err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1113         if (err)
1114                 return err;
1115
1116         dev_dbg(chip->dev, "p%d: 802.1QMode set to %s\n", port,
1117                 mv88e6xxx_port_8021q_mode_names[mode]);
1118
1119         return 0;
1120 }
1121
1122 int mv88e6xxx_port_set_map_da(struct mv88e6xxx_chip *chip, int port)
1123 {
1124         u16 reg;
1125         int err;
1126
1127         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1128         if (err)
1129                 return err;
1130
1131         reg |= MV88E6XXX_PORT_CTL2_MAP_DA;
1132
1133         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1134 }
1135
1136 int mv88e6165_port_set_jumbo_size(struct mv88e6xxx_chip *chip, int port,
1137                                   size_t size)
1138 {
1139         u16 reg;
1140         int err;
1141
1142         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1143         if (err)
1144                 return err;
1145
1146         reg &= ~MV88E6XXX_PORT_CTL2_JUMBO_MODE_MASK;
1147
1148         if (size <= 1522)
1149                 reg |= MV88E6XXX_PORT_CTL2_JUMBO_MODE_1522;
1150         else if (size <= 2048)
1151                 reg |= MV88E6XXX_PORT_CTL2_JUMBO_MODE_2048;
1152         else if (size <= 10240)
1153                 reg |= MV88E6XXX_PORT_CTL2_JUMBO_MODE_10240;
1154         else
1155                 return -ERANGE;
1156
1157         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1158 }
1159
1160 /* Offset 0x09: Port Rate Control */
1161
1162 int mv88e6095_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port)
1163 {
1164         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_EGRESS_RATE_CTL1,
1165                                     0x0000);
1166 }
1167
1168 int mv88e6097_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port)
1169 {
1170         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_EGRESS_RATE_CTL1,
1171                                     0x0001);
1172 }
1173
1174 /* Offset 0x0C: Port ATU Control */
1175
1176 int mv88e6xxx_port_disable_learn_limit(struct mv88e6xxx_chip *chip, int port)
1177 {
1178         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_ATU_CTL, 0);
1179 }
1180
1181 /* Offset 0x0D: (Priority) Override Register */
1182
1183 int mv88e6xxx_port_disable_pri_override(struct mv88e6xxx_chip *chip, int port)
1184 {
1185         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_PRI_OVERRIDE, 0);
1186 }
1187
1188 /* Offset 0x0f: Port Ether type */
1189
1190 int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
1191                                   u16 etype)
1192 {
1193         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_ETH_TYPE, etype);
1194 }
1195
1196 /* Offset 0x18: Port IEEE Priority Remapping Registers [0-3]
1197  * Offset 0x19: Port IEEE Priority Remapping Registers [4-7]
1198  */
1199
1200 int mv88e6095_port_tag_remap(struct mv88e6xxx_chip *chip, int port)
1201 {
1202         int err;
1203
1204         /* Use a direct priority mapping for all IEEE tagged frames */
1205         err = mv88e6xxx_port_write(chip, port,
1206                                    MV88E6095_PORT_IEEE_PRIO_REMAP_0123,
1207                                    0x3210);
1208         if (err)
1209                 return err;
1210
1211         return mv88e6xxx_port_write(chip, port,
1212                                     MV88E6095_PORT_IEEE_PRIO_REMAP_4567,
1213                                     0x7654);
1214 }
1215
1216 static int mv88e6xxx_port_ieeepmt_write(struct mv88e6xxx_chip *chip,
1217                                         int port, u16 table, u8 ptr, u16 data)
1218 {
1219         u16 reg;
1220
1221         reg = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_UPDATE | table |
1222                 (ptr << __bf_shf(MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_PTR_MASK)) |
1223                 (data & MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_DATA_MASK);
1224
1225         return mv88e6xxx_port_write(chip, port,
1226                                     MV88E6390_PORT_IEEE_PRIO_MAP_TABLE, reg);
1227 }
1228
1229 int mv88e6390_port_tag_remap(struct mv88e6xxx_chip *chip, int port)
1230 {
1231         int err, i;
1232         u16 table;
1233
1234         for (i = 0; i <= 7; i++) {
1235                 table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_INGRESS_PCP;
1236                 err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i,
1237                                                    (i | i << 4));
1238                 if (err)
1239                         return err;
1240
1241                 table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_PCP;
1242                 err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i, i);
1243                 if (err)
1244                         return err;
1245
1246                 table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_PCP;
1247                 err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i, i);
1248                 if (err)
1249                         return err;
1250
1251                 table = MV88E6390_PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_PCP;
1252                 err = mv88e6xxx_port_ieeepmt_write(chip, port, table, i, i);
1253                 if (err)
1254                         return err;
1255         }
1256
1257         return 0;
1258 }
1259
1260 /* Offset 0x0E: Policy Control Register */
1261
1262 int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port,
1263                               enum mv88e6xxx_policy_mapping mapping,
1264                               enum mv88e6xxx_policy_action action)
1265 {
1266         u16 reg, mask, val;
1267         int shift;
1268         int err;
1269
1270         switch (mapping) {
1271         case MV88E6XXX_POLICY_MAPPING_DA:
1272                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_DA_MASK);
1273                 mask = MV88E6XXX_PORT_POLICY_CTL_DA_MASK;
1274                 break;
1275         case MV88E6XXX_POLICY_MAPPING_SA:
1276                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_SA_MASK);
1277                 mask = MV88E6XXX_PORT_POLICY_CTL_SA_MASK;
1278                 break;
1279         case MV88E6XXX_POLICY_MAPPING_VTU:
1280                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VTU_MASK);
1281                 mask = MV88E6XXX_PORT_POLICY_CTL_VTU_MASK;
1282                 break;
1283         case MV88E6XXX_POLICY_MAPPING_ETYPE:
1284                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK);
1285                 mask = MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK;
1286                 break;
1287         case MV88E6XXX_POLICY_MAPPING_PPPOE:
1288                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK);
1289                 mask = MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK;
1290                 break;
1291         case MV88E6XXX_POLICY_MAPPING_VBAS:
1292                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK);
1293                 mask = MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK;
1294                 break;
1295         case MV88E6XXX_POLICY_MAPPING_OPT82:
1296                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK);
1297                 mask = MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK;
1298                 break;
1299         case MV88E6XXX_POLICY_MAPPING_UDP:
1300                 shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_UDP_MASK);
1301                 mask = MV88E6XXX_PORT_POLICY_CTL_UDP_MASK;
1302                 break;
1303         default:
1304                 return -EOPNOTSUPP;
1305         }
1306
1307         switch (action) {
1308         case MV88E6XXX_POLICY_ACTION_NORMAL:
1309                 val = MV88E6XXX_PORT_POLICY_CTL_NORMAL;
1310                 break;
1311         case MV88E6XXX_POLICY_ACTION_MIRROR:
1312                 val = MV88E6XXX_PORT_POLICY_CTL_MIRROR;
1313                 break;
1314         case MV88E6XXX_POLICY_ACTION_TRAP:
1315                 val = MV88E6XXX_PORT_POLICY_CTL_TRAP;
1316                 break;
1317         case MV88E6XXX_POLICY_ACTION_DISCARD:
1318                 val = MV88E6XXX_PORT_POLICY_CTL_DISCARD;
1319                 break;
1320         default:
1321                 return -EOPNOTSUPP;
1322         }
1323
1324         err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_POLICY_CTL, &reg);
1325         if (err)
1326                 return err;
1327
1328         reg &= ~mask;
1329         reg |= (val << shift) & mask;
1330
1331         return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_POLICY_CTL, reg);
1332 }