drivers/net/dsa/mv88e6xxx/port.c

   1 /*
   2  * Marvell 88E6xxx Switch Port Registers support
   3  *
   4  * Copyright (c) 2008 Marvell Semiconductor
   5  *
   6  * Copyright (c) 2016-2017 Savoir-faire Linux Inc.
   7  *      Vivien Didelot <vivien.didelot@savoirfairelinux.com>
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation; either version 2 of the License, or
  12  * (at your option) any later version.
  13  */
  14
  15 #include <linux/phy.h>
  16 #include "mv88e6xxx.h"
  17 #include "port.h"
  18
  19 int mv88e6xxx_port_read(struct mv88e6xxx_chip *chip, int port, int reg,
  20                         u16 *val)
  21 {
  22         int addr = chip->info->port_base_addr + port;
  23
  24         return mv88e6xxx_read(chip, addr, reg, val);
  25 }
  26
  27 int mv88e6xxx_port_write(struct mv88e6xxx_chip *chip, int port, int reg,
  28                          u16 val)
  29 {
  30         int addr = chip->info->port_base_addr + port;
  31
  32         return mv88e6xxx_write(chip, addr, reg, val);
  33 }
  34
  35 /* Offset 0x01: MAC (or PCS or Physical) Control Register
  36  *
  37  * Link, Duplex and Flow Control have one force bit, one value bit.
  38  *
  39  * For port's MAC speed, ForceSpd (or SpdValue) bits 1:0 program the value.
  40  * Alternative values require the 200BASE (or AltSpeed) bit 12 set.
  41  * Newer chips need a ForcedSpd bit 13 set to consider the value.
  42  */
  43
  44 static int mv88e6xxx_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
  45                                           phy_interface_t mode)
  46 {
  47         u16 reg;
  48         int err;
  49
  50         err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
  51         if (err)
  52                 return err;
  53
  54         reg &= ~(PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
  55                  PORT_PCS_CTRL_RGMII_DELAY_TXCLK);
  56
  57         switch (mode) {
  58         case PHY_INTERFACE_MODE_RGMII_RXID:
  59                 reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK;
  60                 break;
  61         case PHY_INTERFACE_MODE_RGMII_TXID:
  62                 reg |= PORT_PCS_CTRL_RGMII_DELAY_TXCLK;
  63                 break;
  64         case PHY_INTERFACE_MODE_RGMII_ID:
  65                 reg |= PORT_PCS_CTRL_RGMII_DELAY_RXCLK |
  66                         PORT_PCS_CTRL_RGMII_DELAY_TXCLK;
  67                 break;
  68         case PHY_INTERFACE_MODE_RGMII:
  69                 break;
  70         default:
  71                 return 0;
  72         }
  73
  74         err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
  75         if (err)
  76                 return err;
  77
  78         netdev_dbg(chip->ds->ports[port].netdev, "delay RXCLK %s, TXCLK %s\n",
  79                    reg & PORT_PCS_CTRL_RGMII_DELAY_RXCLK ? "yes" : "no",
  80                    reg & PORT_PCS_CTRL_RGMII_DELAY_TXCLK ? "yes" : "no");
  81
  82         return 0;
  83 }
  84
  85 int mv88e6352_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
  86                                    phy_interface_t mode)
  87 {
  88         if (port < 5)
  89                 return -EOPNOTSUPP;
  90
  91         return mv88e6xxx_port_set_rgmii_delay(chip, port, mode);
  92 }
  93
  94 int mv88e6390_port_set_rgmii_delay(struct mv88e6xxx_chip *chip, int port,
  95                                    phy_interface_t mode)
  96 {
  97         if (port != 0)
  98                 return -EOPNOTSUPP;
  99
 100         return mv88e6xxx_port_set_rgmii_delay(chip, port, mode);
 101 }
 102
 103 int mv88e6xxx_port_set_link(struct mv88e6xxx_chip *chip, int port, int link)
 104 {
 105         u16 reg;
 106         int err;
 107
 108         err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
 109         if (err)
 110                 return err;
 111
 112         reg &= ~(PORT_PCS_CTRL_FORCE_LINK | PORT_PCS_CTRL_LINK_UP);
 113
 114         switch (link) {
 115         case LINK_FORCED_DOWN:
 116                 reg |= PORT_PCS_CTRL_FORCE_LINK;
 117                 break;
 118         case LINK_FORCED_UP:
 119                 reg |= PORT_PCS_CTRL_FORCE_LINK | PORT_PCS_CTRL_LINK_UP;
 120                 break;
 121         case LINK_UNFORCED:
 122                 /* normal link detection */
 123                 break;
 124         default:
 125                 return -EINVAL;
 126         }
 127
 128         err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
 129         if (err)
 130                 return err;
 131
 132         netdev_dbg(chip->ds->ports[port].netdev, "%s link %s\n",
 133                    reg & PORT_PCS_CTRL_FORCE_LINK ? "Force" : "Unforce",
 134                    reg & PORT_PCS_CTRL_LINK_UP ? "up" : "down");
 135
 136         return 0;
 137 }
 138
 139 int mv88e6xxx_port_set_duplex(struct mv88e6xxx_chip *chip, int port, int dup)
 140 {
 141         u16 reg;
 142         int err;
 143
 144         err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
 145         if (err)
 146                 return err;
 147
 148         reg &= ~(PORT_PCS_CTRL_FORCE_DUPLEX | PORT_PCS_CTRL_DUPLEX_FULL);
 149
 150         switch (dup) {
 151         case DUPLEX_HALF:
 152                 reg |= PORT_PCS_CTRL_FORCE_DUPLEX;
 153                 break;
 154         case DUPLEX_FULL:
 155                 reg |= PORT_PCS_CTRL_FORCE_DUPLEX | PORT_PCS_CTRL_DUPLEX_FULL;
 156                 break;
 157         case DUPLEX_UNFORCED:
 158                 /* normal duplex detection */
 159                 break;
 160         default:
 161                 return -EINVAL;
 162         }
 163
 164         err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
 165         if (err)
 166                 return err;
 167
 168         netdev_dbg(chip->ds->ports[port].netdev, "%s %s duplex\n",
 169                    reg & PORT_PCS_CTRL_FORCE_DUPLEX ? "Force" : "Unforce",
 170                    reg & PORT_PCS_CTRL_DUPLEX_FULL ? "full" : "half");
 171
 172         return 0;
 173 }
 174
 175 static int mv88e6xxx_port_set_speed(struct mv88e6xxx_chip *chip, int port,
 176                                     int speed, bool alt_bit, bool force_bit)
 177 {
 178         u16 reg, ctrl;
 179         int err;
 180
 181         switch (speed) {
 182         case 10:
 183                 ctrl = PORT_PCS_CTRL_SPEED_10;
 184                 break;
 185         case 100:
 186                 ctrl = PORT_PCS_CTRL_SPEED_100;
 187                 break;
 188         case 200:
 189                 if (alt_bit)
 190                         ctrl = PORT_PCS_CTRL_SPEED_100 | PORT_PCS_CTRL_ALTSPEED;
 191                 else
 192                         ctrl = PORT_PCS_CTRL_SPEED_200;
 193                 break;
 194         case 1000:
 195                 ctrl = PORT_PCS_CTRL_SPEED_1000;
 196                 break;
 197         case 2500:
 198                 ctrl = PORT_PCS_CTRL_SPEED_10000 | PORT_PCS_CTRL_ALTSPEED;
 199                 break;
 200         case 10000:
 201                 /* all bits set, fall through... */
 202         case SPEED_UNFORCED:
 203                 ctrl = PORT_PCS_CTRL_SPEED_UNFORCED;
 204                 break;
 205         default:
 206                 return -EOPNOTSUPP;
 207         }
 208
 209         err = mv88e6xxx_port_read(chip, port, PORT_PCS_CTRL, &reg);
 210         if (err)
 211                 return err;
 212
 213         reg &= ~PORT_PCS_CTRL_SPEED_MASK;
 214         if (alt_bit)
 215                 reg &= ~PORT_PCS_CTRL_ALTSPEED;
 216         if (force_bit) {
 217                 reg &= ~PORT_PCS_CTRL_FORCE_SPEED;
 218                 if (speed != SPEED_UNFORCED)
 219                         ctrl |= PORT_PCS_CTRL_FORCE_SPEED;
 220         }
 221         reg |= ctrl;
 222
 223         err = mv88e6xxx_port_write(chip, port, PORT_PCS_CTRL, reg);
 224         if (err)
 225                 return err;
 226
 227         if (speed)
 228                 netdev_dbg(chip->ds->ports[port].netdev,
 229                            "Speed set to %d Mbps\n", speed);
 230         else
 231                 netdev_dbg(chip->ds->ports[port].netdev, "Speed unforced\n");
 232
 233         return 0;
 234 }
 235
 236 /* Support 10, 100, 200 Mbps (e.g. 88E6065 family) */
 237 int mv88e6065_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
 238 {
 239         if (speed == SPEED_MAX)
 240                 speed = 200;
 241
 242         if (speed > 200)
 243                 return -EOPNOTSUPP;
 244
 245         /* Setting 200 Mbps on port 0 to 3 selects 100 Mbps */
 246         return mv88e6xxx_port_set_speed(chip, port, speed, false, false);
 247 }
 248
 249 /* Support 10, 100, 1000 Mbps (e.g. 88E6185 family) */
 250 int mv88e6185_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
 251 {
 252         if (speed == SPEED_MAX)
 253                 speed = 1000;
 254
 255         if (speed == 200 || speed > 1000)
 256                 return -EOPNOTSUPP;
 257
 258         return mv88e6xxx_port_set_speed(chip, port, speed, false, false);
 259 }
 260
 261 /* Support 10, 100, 200, 1000 Mbps (e.g. 88E6352 family) */
 262 int mv88e6352_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
 263 {
 264         if (speed == SPEED_MAX)
 265                 speed = 1000;
 266
 267         if (speed > 1000)
 268                 return -EOPNOTSUPP;
 269
 270         if (speed == 200 && port < 5)
 271                 return -EOPNOTSUPP;
 272
 273         return mv88e6xxx_port_set_speed(chip, port, speed, true, false);
 274 }
 275
 276 /* Support 10, 100, 200, 1000, 2500 Mbps (e.g. 88E6390) */
 277 int mv88e6390_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
 278 {
 279         if (speed == SPEED_MAX)
 280                 speed = port < 9 ? 1000 : 2500;
 281
 282         if (speed > 2500)
 283                 return -EOPNOTSUPP;
 284
 285         if (speed == 200 && port != 0)
 286                 return -EOPNOTSUPP;
 287
 288         if (speed == 2500 && port < 9)
 289                 return -EOPNOTSUPP;
 290
 291         return mv88e6xxx_port_set_speed(chip, port, speed, true, true);
 292 }
 293
 294 /* Support 10, 100, 200, 1000, 2500, 10000 Mbps (e.g. 88E6190X) */
 295 int mv88e6390x_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
 296 {
 297         if (speed == SPEED_MAX)
 298                 speed = port < 9 ? 1000 : 10000;
 299
 300         if (speed == 200 && port != 0)
 301                 return -EOPNOTSUPP;
 302
 303         if (speed >= 2500 && port < 9)
 304                 return -EOPNOTSUPP;
 305
 306         return mv88e6xxx_port_set_speed(chip, port, speed, true, true);
 307 }
 308
 309 int mv88e6390x_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 310                               phy_interface_t mode)
 311 {
 312         u16 reg;
 313         u16 cmode;
 314         int err;
 315
 316         if (mode == PHY_INTERFACE_MODE_NA)
 317                 return 0;
 318
 319         if (port != 9 && port != 10)
 320                 return -EOPNOTSUPP;
 321
 322         switch (mode) {
 323         case PHY_INTERFACE_MODE_1000BASEX:
 324                 cmode = PORT_STATUS_CMODE_1000BASE_X;
 325                 break;
 326         case PHY_INTERFACE_MODE_SGMII:
 327                 cmode = PORT_STATUS_CMODE_SGMII;
 328                 break;
 329         case PHY_INTERFACE_MODE_2500BASEX:
 330                 cmode = PORT_STATUS_CMODE_2500BASEX;
 331                 break;
 332         case PHY_INTERFACE_MODE_XGMII:
 333                 cmode = PORT_STATUS_CMODE_XAUI;
 334                 break;
 335         case PHY_INTERFACE_MODE_RXAUI:
 336                 cmode = PORT_STATUS_CMODE_RXAUI;
 337                 break;
 338         default:
 339                 cmode = 0;
 340         }
 341
 342         if (cmode) {
 343                 err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
 344                 if (err)
 345                         return err;
 346
 347                 reg &= ~PORT_STATUS_CMODE_MASK;
 348                 reg |= cmode;
 349
 350                 err = mv88e6xxx_port_write(chip, port, PORT_STATUS, reg);
 351                 if (err)
 352                         return err;
 353         }
 354
 355         return 0;
 356 }
 357
 358 int mv88e6xxx_port_get_cmode(struct mv88e6xxx_chip *chip, int port, u8 *cmode)
 359 {
 360         int err;
 361         u16 reg;
 362
 363         err = mv88e6xxx_port_read(chip, port, PORT_STATUS, &reg);
 364         if (err)
 365                 return err;
 366
 367         *cmode = reg & PORT_STATUS_CMODE_MASK;
 368
 369         return 0;
 370 }
 371
 372 /* Offset 0x02: Pause Control
 373  *
 374  * Do not limit the period of time that this port can be paused for by
 375  * the remote end or the period of time that this port can pause the
 376  * remote end.
 377  */
 378 int mv88e6097_port_pause_config(struct mv88e6xxx_chip *chip, int port)
 379 {
 380         return mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL, 0x0000);
 381 }
 382
 383 int mv88e6390_port_pause_config(struct mv88e6xxx_chip *chip, int port)
 384 {
 385         int err;
 386
 387         err = mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL,
 388                                    PORT_FLOW_CTRL_LIMIT_IN | 0);
 389         if (err)
 390                 return err;
 391
 392         return mv88e6xxx_port_write(chip, port, PORT_PAUSE_CTRL,
 393                                     PORT_FLOW_CTRL_LIMIT_OUT | 0);
 394 }
 395
 396 /* Offset 0x04: Port Control Register */
 397
 398 static const char * const mv88e6xxx_port_state_names[] = {
 399         [PORT_CONTROL_STATE_DISABLED] = "Disabled",
 400         [PORT_CONTROL_STATE_BLOCKING] = "Blocking/Listening",
 401         [PORT_CONTROL_STATE_LEARNING] = "Learning",
 402         [PORT_CONTROL_STATE_FORWARDING] = "Forwarding",
 403 };
 404
 405 int mv88e6xxx_port_set_state(struct mv88e6xxx_chip *chip, int port, u8 state)
 406 {
 407         u16 reg;
 408         int err;
 409
 410         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
 411         if (err)
 412                 return err;
 413
 414         reg &= ~PORT_CONTROL_STATE_MASK;
 415         reg |= state;
 416
 417         err = mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
 418         if (err)
 419                 return err;
 420
 421         netdev_dbg(chip->ds->ports[port].netdev, "PortState set to %s\n",
 422                    mv88e6xxx_port_state_names[state]);
 423
 424         return 0;
 425 }
 426
 427 int mv88e6xxx_port_set_egress_mode(struct mv88e6xxx_chip *chip, int port,
 428                                    u16 mode)
 429 {
 430         int err;
 431         u16 reg;
 432
 433         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
 434         if (err)
 435                 return err;
 436
 437         reg &= ~PORT_CONTROL_EGRESS_MASK;
 438         reg |= mode;
 439
 440         return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
 441 }
 442
 443 int mv88e6085_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 444                                   enum mv88e6xxx_frame_mode mode)
 445 {
 446         int err;
 447         u16 reg;
 448
 449         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
 450         if (err)
 451                 return err;
 452
 453         reg &= ~PORT_CONTROL_FRAME_MODE_DSA;
 454
 455         switch (mode) {
 456         case MV88E6XXX_FRAME_MODE_NORMAL:
 457                 reg |= PORT_CONTROL_FRAME_MODE_NORMAL;
 458                 break;
 459         case MV88E6XXX_FRAME_MODE_DSA:
 460                 reg |= PORT_CONTROL_FRAME_MODE_DSA;
 461                 break;
 462         default:
 463                 return -EINVAL;
 464         }
 465
 466         return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
 467 }
 468
 469 int mv88e6351_port_set_frame_mode(struct mv88e6xxx_chip *chip, int port,
 470                                   enum mv88e6xxx_frame_mode mode)
 471 {
 472         int err;
 473         u16 reg;
 474
 475         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
 476         if (err)
 477                 return err;
 478
 479         reg &= ~PORT_CONTROL_FRAME_MASK;
 480
 481         switch (mode) {
 482         case MV88E6XXX_FRAME_MODE_NORMAL:
 483                 reg |= PORT_CONTROL_FRAME_MODE_NORMAL;
 484                 break;
 485         case MV88E6XXX_FRAME_MODE_DSA:
 486                 reg |= PORT_CONTROL_FRAME_MODE_DSA;
 487                 break;
 488         case MV88E6XXX_FRAME_MODE_PROVIDER:
 489                 reg |= PORT_CONTROL_FRAME_MODE_PROVIDER;
 490                 break;
 491         case MV88E6XXX_FRAME_MODE_ETHERTYPE:
 492                 reg |= PORT_CONTROL_FRAME_ETHER_TYPE_DSA;
 493                 break;
 494         default:
 495                 return -EINVAL;
 496         }
 497
 498         return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
 499 }
 500
 501 static int mv88e6185_port_set_forward_unknown(struct mv88e6xxx_chip *chip,
 502                                               int port, bool unicast)
 503 {
 504         int err;
 505         u16 reg;
 506
 507         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
 508         if (err)
 509                 return err;
 510
 511         if (unicast)
 512                 reg |= PORT_CONTROL_FORWARD_UNKNOWN;
 513         else
 514                 reg &= ~PORT_CONTROL_FORWARD_UNKNOWN;
 515
 516         return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
 517 }
 518
 519 int mv88e6352_port_set_egress_floods(struct mv88e6xxx_chip *chip, int port,
 520                                      bool unicast, bool multicast)
 521 {
 522         int err;
 523         u16 reg;
 524
 525         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL, &reg);
 526         if (err)
 527                 return err;
 528
 529         reg &= ~PORT_CONTROL_EGRESS_FLOODS_MASK;
 530
 531         if (unicast && multicast)
 532                 reg |= PORT_CONTROL_EGRESS_FLOODS_ALL_UNKNOWN_DA;
 533         else if (unicast)
 534                 reg |= PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_MC_DA;
 535         else if (multicast)
 536                 reg |= PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_UC_DA;
 537         else
 538                 reg |= PORT_CONTROL_EGRESS_FLOODS_NO_UNKNOWN_DA;
 539
 540         return mv88e6xxx_port_write(chip, port, PORT_CONTROL, reg);
 541 }
 542
 543 /* Offset 0x05: Port Control 1 */
 544
 545 int mv88e6xxx_port_set_message_port(struct mv88e6xxx_chip *chip, int port,
 546                                     bool message_port)
 547 {
 548         u16 val;
 549         int err;
 550
 551         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &val);
 552         if (err)
 553                 return err;
 554
 555         if (message_port)
 556                 val |= PORT_CONTROL_1_MESSAGE_PORT;
 557         else
 558                 val &= ~PORT_CONTROL_1_MESSAGE_PORT;
 559
 560         return mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, val);
 561 }
 562
 563 /* Offset 0x06: Port Based VLAN Map */
 564
 565 int mv88e6xxx_port_set_vlan_map(struct mv88e6xxx_chip *chip, int port, u16 map)
 566 {
 567         const u16 mask = mv88e6xxx_port_mask(chip);
 568         u16 reg;
 569         int err;
 570
 571         err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
 572         if (err)
 573                 return err;
 574
 575         reg &= ~mask;
 576         reg |= map & mask;
 577
 578         err = mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
 579         if (err)
 580                 return err;
 581
 582         netdev_dbg(chip->ds->ports[port].netdev, "VLANTable set to %.3x\n",
 583                    map);
 584
 585         return 0;
 586 }
 587
 588 int mv88e6xxx_port_get_fid(struct mv88e6xxx_chip *chip, int port, u16 *fid)
 589 {
 590         const u16 upper_mask = (mv88e6xxx_num_databases(chip) - 1) >> 4;
 591         u16 reg;
 592         int err;
 593
 594         /* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
 595         err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
 596         if (err)
 597                 return err;
 598
 599         *fid = (reg & 0xf000) >> 12;
 600
 601         /* Port's default FID upper bits are located in reg 0x05, offset 0 */
 602         if (upper_mask) {
 603                 err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &reg);
 604                 if (err)
 605                         return err;
 606
 607                 *fid |= (reg & upper_mask) << 4;
 608         }
 609
 610         return 0;
 611 }
 612
 613 int mv88e6xxx_port_set_fid(struct mv88e6xxx_chip *chip, int port, u16 fid)
 614 {
 615         const u16 upper_mask = (mv88e6xxx_num_databases(chip) - 1) >> 4;
 616         u16 reg;
 617         int err;
 618
 619         if (fid >= mv88e6xxx_num_databases(chip))
 620                 return -EINVAL;
 621
 622         /* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
 623         err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, &reg);
 624         if (err)
 625                 return err;
 626
 627         reg &= 0x0fff;
 628         reg |= (fid & 0x000f) << 12;
 629
 630         err = mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
 631         if (err)
 632                 return err;
 633
 634         /* Port's default FID upper bits are located in reg 0x05, offset 0 */
 635         if (upper_mask) {
 636                 err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, &reg);
 637                 if (err)
 638                         return err;
 639
 640                 reg &= ~upper_mask;
 641                 reg |= (fid >> 4) & upper_mask;
 642
 643                 err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, reg);
 644                 if (err)
 645                         return err;
 646         }
 647
 648         netdev_dbg(chip->ds->ports[port].netdev, "FID set to %u\n", fid);
 649
 650         return 0;
 651 }
 652
 653 /* Offset 0x07: Default Port VLAN ID & Priority */
 654
 655 int mv88e6xxx_port_get_pvid(struct mv88e6xxx_chip *chip, int port, u16 *pvid)
 656 {
 657         u16 reg;
 658         int err;
 659
 660         err = mv88e6xxx_port_read(chip, port, PORT_DEFAULT_VLAN, &reg);
 661         if (err)
 662                 return err;
 663
 664         *pvid = reg & PORT_DEFAULT_VLAN_MASK;
 665
 666         return 0;
 667 }
 668
 669 int mv88e6xxx_port_set_pvid(struct mv88e6xxx_chip *chip, int port, u16 pvid)
 670 {
 671         u16 reg;
 672         int err;
 673
 674         err = mv88e6xxx_port_read(chip, port, PORT_DEFAULT_VLAN, &reg);
 675         if (err)
 676                 return err;
 677
 678         reg &= ~PORT_DEFAULT_VLAN_MASK;
 679         reg |= pvid & PORT_DEFAULT_VLAN_MASK;
 680
 681         err = mv88e6xxx_port_write(chip, port, PORT_DEFAULT_VLAN, reg);
 682         if (err)
 683                 return err;
 684
 685         netdev_dbg(chip->ds->ports[port].netdev, "DefaultVID set to %u\n",
 686                    pvid);
 687
 688         return 0;
 689 }
 690
 691 /* Offset 0x08: Port Control 2 Register */
 692
 693 static const char * const mv88e6xxx_port_8021q_mode_names[] = {
 694         [PORT_CONTROL_2_8021Q_DISABLED] = "Disabled",
 695         [PORT_CONTROL_2_8021Q_FALLBACK] = "Fallback",
 696         [PORT_CONTROL_2_8021Q_CHECK] = "Check",
 697         [PORT_CONTROL_2_8021Q_SECURE] = "Secure",
 698 };
 699
 700 static int mv88e6185_port_set_default_forward(struct mv88e6xxx_chip *chip,
 701                                               int port, bool multicast)
 702 {
 703         int err;
 704         u16 reg;
 705
 706         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
 707         if (err)
 708                 return err;
 709
 710         if (multicast)
 711                 reg |= PORT_CONTROL_2_DEFAULT_FORWARD;
 712         else
 713                 reg &= ~PORT_CONTROL_2_DEFAULT_FORWARD;
 714
 715         return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
 716 }
 717
 718 int mv88e6185_port_set_egress_floods(struct mv88e6xxx_chip *chip, int port,
 719                                      bool unicast, bool multicast)
 720 {
 721         int err;
 722
 723         err = mv88e6185_port_set_forward_unknown(chip, port, unicast);
 724         if (err)
 725                 return err;
 726
 727         return mv88e6185_port_set_default_forward(chip, port, multicast);
 728 }
 729
 730 int mv88e6095_port_set_upstream_port(struct mv88e6xxx_chip *chip, int port,
 731                                      int upstream_port)
 732 {
 733         int err;
 734         u16 reg;
 735
 736         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
 737         if (err)
 738                 return err;
 739
 740         reg &= ~PORT_CONTROL_2_UPSTREAM_MASK;
 741         reg |= upstream_port;
 742
 743         return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
 744 }
 745
 746 int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
 747                                   u16 mode)
 748 {
 749         u16 reg;
 750         int err;
 751
 752         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
 753         if (err)
 754                 return err;
 755
 756         reg &= ~PORT_CONTROL_2_8021Q_MASK;
 757         reg |= mode & PORT_CONTROL_2_8021Q_MASK;
 758
 759         err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
 760         if (err)
 761                 return err;
 762
 763         netdev_dbg(chip->ds->ports[port].netdev, "802.1QMode set to %s\n",
 764                    mv88e6xxx_port_8021q_mode_names[mode]);
 765
 766         return 0;
 767 }
 768
 769 int mv88e6xxx_port_set_map_da(struct mv88e6xxx_chip *chip, int port)
 770 {
 771         u16 reg;
 772         int err;
 773
 774         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
 775         if (err)
 776                 return err;
 777
 778         reg |= PORT_CONTROL_2_MAP_DA;
 779
 780         return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
 781 }
 782
 783 int mv88e6165_port_jumbo_config(struct mv88e6xxx_chip *chip, int port)
 784 {
 785         u16 reg;
 786         int err;
 787
 788         err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_2, &reg);
 789         if (err)
 790                 return err;
 791
 792         reg |= PORT_CONTROL_2_JUMBO_10240;
 793
 794         return mv88e6xxx_port_write(chip, port, PORT_CONTROL_2, reg);
 795 }
 796
 797 /* Offset 0x09: Port Rate Control */
 798
 799 int mv88e6095_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port)
 800 {
 801         return mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL, 0x0000);
 802 }
 803
 804 int mv88e6097_port_egress_rate_limiting(struct mv88e6xxx_chip *chip, int port)
 805 {
 806         return mv88e6xxx_port_write(chip, port, PORT_RATE_CONTROL, 0x0001);
 807 }
 808
 809 /* Offset 0x0C: Port ATU Control */
 810
 811 int mv88e6xxx_port_disable_learn_limit(struct mv88e6xxx_chip *chip, int port)
 812 {
 813         return mv88e6xxx_port_write(chip, port, PORT_ATU_CONTROL, 0);
 814 }
 815
 816 /* Offset 0x0D: (Priority) Override Register */
 817
 818 int mv88e6xxx_port_disable_pri_override(struct mv88e6xxx_chip *chip, int port)
 819 {
 820         return mv88e6xxx_port_write(chip, port, PORT_PRI_OVERRIDE, 0);
 821 }
 822
 823 /* Offset 0x0f: Port Ether type */
 824
 825 int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
 826                                   u16 etype)
 827 {
 828         return mv88e6xxx_port_write(chip, port, PORT_ETH_TYPE, etype);
 829 }
 830
 831 /* Offset 0x18: Port IEEE Priority Remapping Registers [0-3]
 832  * Offset 0x19: Port IEEE Priority Remapping Registers [4-7]
 833  */
 834
 835 int mv88e6095_port_tag_remap(struct mv88e6xxx_chip *chip, int port)
 836 {
 837         int err;
 838
 839         /* Use a direct priority mapping for all IEEE tagged frames */
 840         err = mv88e6xxx_port_write(chip, port, PORT_TAG_REGMAP_0123, 0x3210);
 841         if (err)
 842                 return err;
 843
 844         return mv88e6xxx_port_write(chip, port, PORT_TAG_REGMAP_4567, 0x7654);
 845 }
 846
 847 static int mv88e6xxx_port_ieeepmt_write(struct mv88e6xxx_chip *chip,
 848                                         int port, u16 table,
 849                                         u8 pointer, u16 data)
 850 {
 851         u16 reg;
 852
 853         reg = PORT_IEEE_PRIO_MAP_TABLE_UPDATE |
 854                 table |
 855                 (pointer << PORT_IEEE_PRIO_MAP_TABLE_POINTER_SHIFT) |
 856                 data;
 857
 858         return mv88e6xxx_port_write(chip, port, PORT_IEEE_PRIO_MAP_TABLE, reg);
 859 }
 860
 861 int mv88e6390_port_tag_remap(struct mv88e6xxx_chip *chip, int port)
 862 {
 863         int err, i;
 864
 865         for (i = 0; i <= 7; i++) {
 866                 err = mv88e6xxx_port_ieeepmt_write(
 867                         chip, port, PORT_IEEE_PRIO_MAP_TABLE_INGRESS_PCP,
 868                         i, (i | i << 4));
 869                 if (err)
 870                         return err;
 871
 872                 err = mv88e6xxx_port_ieeepmt_write(
 873                         chip, port, PORT_IEEE_PRIO_MAP_TABLE_EGRESS_GREEN_PCP,
 874                         i, i);
 875                 if (err)
 876                         return err;
 877
 878                 err = mv88e6xxx_port_ieeepmt_write(
 879                         chip, port, PORT_IEEE_PRIO_MAP_TABLE_EGRESS_YELLOW_PCP,
 880                         i, i);
 881                 if (err)
 882                         return err;
 883
 884                 err = mv88e6xxx_port_ieeepmt_write(
 885                         chip, port, PORT_IEEE_PRIO_MAP_TABLE_EGRESS_AVB_PCP,
 886                         i, i);
 887                 if (err)
 888                         return err;
 889         }
 890
 891         return 0;
 892 }