drivers/net/ethernet/mscc/ocelot.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /*
   3  * Microsemi Ocelot Switch driver
   4  *
   5  * Copyright (c) 2017 Microsemi Corporation
   6  */
   7 #include <linux/dsa/ocelot.h>
   8 #include <linux/if_bridge.h>
   9 #include <linux/ptp_classify.h>
  10 #include <soc/mscc/ocelot_vcap.h>
  11 #include "ocelot.h"
  12 #include "ocelot_vcap.h"
  13
  14 #define TABLE_UPDATE_SLEEP_US 10
  15 #define TABLE_UPDATE_TIMEOUT_US 100000
  16 #define OCELOT_RSV_VLAN_RANGE_START 4000
  17
  18 struct ocelot_mact_entry {
  19         u8 mac[ETH_ALEN];
  20         u16 vid;
  21         enum macaccess_entry_type type;
  22 };
  23
  24 /* Caller must hold &ocelot->mact_lock */
  25 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
  26 {
  27         return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
  28 }
  29
  30 /* Caller must hold &ocelot->mact_lock */
  31 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
  32 {
  33         u32 val;
  34
  35         return readx_poll_timeout(ocelot_mact_read_macaccess,
  36                 ocelot, val,
  37                 (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
  38                 MACACCESS_CMD_IDLE,
  39                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
  40 }
  41
  42 /* Caller must hold &ocelot->mact_lock */
  43 static void ocelot_mact_select(struct ocelot *ocelot,
  44                                const unsigned char mac[ETH_ALEN],
  45                                unsigned int vid)
  46 {
  47         u32 macl = 0, mach = 0;
  48
  49         /* Set the MAC address to handle and the vlan associated in a format
  50          * understood by the hardware.
  51          */
  52         mach |= vid    << 16;
  53         mach |= mac[0] << 8;
  54         mach |= mac[1] << 0;
  55         macl |= mac[2] << 24;
  56         macl |= mac[3] << 16;
  57         macl |= mac[4] << 8;
  58         macl |= mac[5] << 0;
  59
  60         ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
  61         ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
  62
  63 }
  64
  65 static int __ocelot_mact_learn(struct ocelot *ocelot, int port,
  66                                const unsigned char mac[ETH_ALEN],
  67                                unsigned int vid, enum macaccess_entry_type type)
  68 {
  69         u32 cmd = ANA_TABLES_MACACCESS_VALID |
  70                 ANA_TABLES_MACACCESS_DEST_IDX(port) |
  71                 ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
  72                 ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN);
  73         unsigned int mc_ports;
  74         int err;
  75
  76         /* Set MAC_CPU_COPY if the CPU port is used by a multicast entry */
  77         if (type == ENTRYTYPE_MACv4)
  78                 mc_ports = (mac[1] << 8) | mac[2];
  79         else if (type == ENTRYTYPE_MACv6)
  80                 mc_ports = (mac[0] << 8) | mac[1];
  81         else
  82                 mc_ports = 0;
  83
  84         if (mc_ports & BIT(ocelot->num_phys_ports))
  85                 cmd |= ANA_TABLES_MACACCESS_MAC_CPU_COPY;
  86
  87         ocelot_mact_select(ocelot, mac, vid);
  88
  89         /* Issue a write command */
  90         ocelot_write(ocelot, cmd, ANA_TABLES_MACACCESS);
  91
  92         err = ocelot_mact_wait_for_completion(ocelot);
  93
  94         return err;
  95 }
  96
  97 int ocelot_mact_learn(struct ocelot *ocelot, int port,
  98                       const unsigned char mac[ETH_ALEN],
  99                       unsigned int vid, enum macaccess_entry_type type)
 100 {
 101         int ret;
 102
 103         mutex_lock(&ocelot->mact_lock);
 104         ret = __ocelot_mact_learn(ocelot, port, mac, vid, type);
 105         mutex_unlock(&ocelot->mact_lock);
 106
 107         return ret;
 108 }
 109 EXPORT_SYMBOL(ocelot_mact_learn);
 110
 111 int ocelot_mact_forget(struct ocelot *ocelot,
 112                        const unsigned char mac[ETH_ALEN], unsigned int vid)
 113 {
 114         int err;
 115
 116         mutex_lock(&ocelot->mact_lock);
 117
 118         ocelot_mact_select(ocelot, mac, vid);
 119
 120         /* Issue a forget command */
 121         ocelot_write(ocelot,
 122                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
 123                      ANA_TABLES_MACACCESS);
 124
 125         err = ocelot_mact_wait_for_completion(ocelot);
 126
 127         mutex_unlock(&ocelot->mact_lock);
 128
 129         return err;
 130 }
 131 EXPORT_SYMBOL(ocelot_mact_forget);
 132
 133 int ocelot_mact_lookup(struct ocelot *ocelot, int *dst_idx,
 134                        const unsigned char mac[ETH_ALEN],
 135                        unsigned int vid, enum macaccess_entry_type *type)
 136 {
 137         int val;
 138
 139         mutex_lock(&ocelot->mact_lock);
 140
 141         ocelot_mact_select(ocelot, mac, vid);
 142
 143         /* Issue a read command with MACACCESS_VALID=1. */
 144         ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
 145                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 146                      ANA_TABLES_MACACCESS);
 147
 148         if (ocelot_mact_wait_for_completion(ocelot)) {
 149                 mutex_unlock(&ocelot->mact_lock);
 150                 return -ETIMEDOUT;
 151         }
 152
 153         /* Read back the entry flags */
 154         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 155
 156         mutex_unlock(&ocelot->mact_lock);
 157
 158         if (!(val & ANA_TABLES_MACACCESS_VALID))
 159                 return -ENOENT;
 160
 161         *dst_idx = ANA_TABLES_MACACCESS_DEST_IDX_X(val);
 162         *type = ANA_TABLES_MACACCESS_ENTRYTYPE_X(val);
 163
 164         return 0;
 165 }
 166 EXPORT_SYMBOL(ocelot_mact_lookup);
 167
 168 int ocelot_mact_learn_streamdata(struct ocelot *ocelot, int dst_idx,
 169                                  const unsigned char mac[ETH_ALEN],
 170                                  unsigned int vid,
 171                                  enum macaccess_entry_type type,
 172                                  int sfid, int ssid)
 173 {
 174         int ret;
 175
 176         mutex_lock(&ocelot->mact_lock);
 177
 178         ocelot_write(ocelot,
 179                      (sfid < 0 ? 0 : ANA_TABLES_STREAMDATA_SFID_VALID) |
 180                      ANA_TABLES_STREAMDATA_SFID(sfid) |
 181                      (ssid < 0 ? 0 : ANA_TABLES_STREAMDATA_SSID_VALID) |
 182                      ANA_TABLES_STREAMDATA_SSID(ssid),
 183                      ANA_TABLES_STREAMDATA);
 184
 185         ret = __ocelot_mact_learn(ocelot, dst_idx, mac, vid, type);
 186
 187         mutex_unlock(&ocelot->mact_lock);
 188
 189         return ret;
 190 }
 191 EXPORT_SYMBOL(ocelot_mact_learn_streamdata);
 192
 193 static void ocelot_mact_init(struct ocelot *ocelot)
 194 {
 195         /* Configure the learning mode entries attributes:
 196          * - Do not copy the frame to the CPU extraction queues.
 197          * - Use the vlan and mac_cpoy for dmac lookup.
 198          */
 199         ocelot_rmw(ocelot, 0,
 200                    ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
 201                    | ANA_AGENCTRL_LEARN_FWD_KILL
 202                    | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
 203                    ANA_AGENCTRL);
 204
 205         /* Clear the MAC table. We are not concurrent with anyone, so
 206          * holding &ocelot->mact_lock is pointless.
 207          */
 208         ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
 209 }
 210
 211 static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
 212 {
 213         ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
 214                          ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
 215                          ANA_PORT_VCAP_S2_CFG, port);
 216
 217         ocelot_write_gix(ocelot, ANA_PORT_VCAP_CFG_S1_ENA,
 218                          ANA_PORT_VCAP_CFG, port);
 219
 220         ocelot_rmw_gix(ocelot, REW_PORT_CFG_ES0_EN,
 221                        REW_PORT_CFG_ES0_EN,
 222                        REW_PORT_CFG, port);
 223 }
 224
 225 static int ocelot_single_vlan_aware_bridge(struct ocelot *ocelot,
 226                                            struct netlink_ext_ack *extack)
 227 {
 228         struct net_device *bridge = NULL;
 229         int port;
 230
 231         for (port = 0; port < ocelot->num_phys_ports; port++) {
 232                 struct ocelot_port *ocelot_port = ocelot->ports[port];
 233
 234                 if (!ocelot_port || !ocelot_port->bridge ||
 235                     !br_vlan_enabled(ocelot_port->bridge))
 236                         continue;
 237
 238                 if (!bridge) {
 239                         bridge = ocelot_port->bridge;
 240                         continue;
 241                 }
 242
 243                 if (bridge == ocelot_port->bridge)
 244                         continue;
 245
 246                 NL_SET_ERR_MSG_MOD(extack,
 247                                    "Only one VLAN-aware bridge is supported");
 248                 return -EBUSY;
 249         }
 250
 251         return 0;
 252 }
 253
 254 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
 255 {
 256         return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
 257 }
 258
 259 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
 260 {
 261         u32 val;
 262
 263         return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
 264                 ocelot,
 265                 val,
 266                 (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
 267                 ANA_TABLES_VLANACCESS_CMD_IDLE,
 268                 TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
 269 }
 270
 271 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
 272 {
 273         /* Select the VID to configure */
 274         ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
 275                      ANA_TABLES_VLANTIDX);
 276         /* Set the vlan port members mask and issue a write command */
 277         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
 278                              ANA_TABLES_VLANACCESS_CMD_WRITE,
 279                      ANA_TABLES_VLANACCESS);
 280
 281         return ocelot_vlant_wait_for_completion(ocelot);
 282 }
 283
 284 static int ocelot_port_num_untagged_vlans(struct ocelot *ocelot, int port)
 285 {
 286         struct ocelot_bridge_vlan *vlan;
 287         int num_untagged = 0;
 288
 289         list_for_each_entry(vlan, &ocelot->vlans, list) {
 290                 if (!(vlan->portmask & BIT(port)))
 291                         continue;
 292
 293                 if (vlan->untagged & BIT(port))
 294                         num_untagged++;
 295         }
 296
 297         return num_untagged;
 298 }
 299
 300 static int ocelot_port_num_tagged_vlans(struct ocelot *ocelot, int port)
 301 {
 302         struct ocelot_bridge_vlan *vlan;
 303         int num_tagged = 0;
 304
 305         list_for_each_entry(vlan, &ocelot->vlans, list) {
 306                 if (!(vlan->portmask & BIT(port)))
 307                         continue;
 308
 309                 if (!(vlan->untagged & BIT(port)))
 310                         num_tagged++;
 311         }
 312
 313         return num_tagged;
 314 }
 315
 316 /* We use native VLAN when we have to mix egress-tagged VLANs with exactly
 317  * _one_ egress-untagged VLAN (_the_ native VLAN)
 318  */
 319 static bool ocelot_port_uses_native_vlan(struct ocelot *ocelot, int port)
 320 {
 321         return ocelot_port_num_tagged_vlans(ocelot, port) &&
 322                ocelot_port_num_untagged_vlans(ocelot, port) == 1;
 323 }
 324
 325 static struct ocelot_bridge_vlan *
 326 ocelot_port_find_native_vlan(struct ocelot *ocelot, int port)
 327 {
 328         struct ocelot_bridge_vlan *vlan;
 329
 330         list_for_each_entry(vlan, &ocelot->vlans, list)
 331                 if (vlan->portmask & BIT(port) && vlan->untagged & BIT(port))
 332                         return vlan;
 333
 334         return NULL;
 335 }
 336
 337 /* Keep in sync REW_TAG_CFG_TAG_CFG and, if applicable,
 338  * REW_PORT_VLAN_CFG_PORT_VID, with the bridge VLAN table and VLAN awareness
 339  * state of the port.
 340  */
 341 static void ocelot_port_manage_port_tag(struct ocelot *ocelot, int port)
 342 {
 343         struct ocelot_port *ocelot_port = ocelot->ports[port];
 344         enum ocelot_port_tag_config tag_cfg;
 345         bool uses_native_vlan = false;
 346
 347         if (ocelot_port->vlan_aware) {
 348                 uses_native_vlan = ocelot_port_uses_native_vlan(ocelot, port);
 349
 350                 if (uses_native_vlan)
 351                         tag_cfg = OCELOT_PORT_TAG_NATIVE;
 352                 else if (ocelot_port_num_untagged_vlans(ocelot, port))
 353                         tag_cfg = OCELOT_PORT_TAG_DISABLED;
 354                 else
 355                         tag_cfg = OCELOT_PORT_TAG_TRUNK;
 356         } else {
 357                 tag_cfg = OCELOT_PORT_TAG_DISABLED;
 358         }
 359
 360         ocelot_rmw_gix(ocelot, REW_TAG_CFG_TAG_CFG(tag_cfg),
 361                        REW_TAG_CFG_TAG_CFG_M,
 362                        REW_TAG_CFG, port);
 363
 364         if (uses_native_vlan) {
 365                 struct ocelot_bridge_vlan *native_vlan;
 366
 367                 /* Not having a native VLAN is impossible, because
 368                  * ocelot_port_num_untagged_vlans has returned 1.
 369                  * So there is no use in checking for NULL here.
 370                  */
 371                 native_vlan = ocelot_port_find_native_vlan(ocelot, port);
 372
 373                 ocelot_rmw_gix(ocelot,
 374                                REW_PORT_VLAN_CFG_PORT_VID(native_vlan->vid),
 375                                REW_PORT_VLAN_CFG_PORT_VID_M,
 376                                REW_PORT_VLAN_CFG, port);
 377         }
 378 }
 379
 380 int ocelot_bridge_num_find(struct ocelot *ocelot,
 381                            const struct net_device *bridge)
 382 {
 383         int port;
 384
 385         for (port = 0; port < ocelot->num_phys_ports; port++) {
 386                 struct ocelot_port *ocelot_port = ocelot->ports[port];
 387
 388                 if (ocelot_port && ocelot_port->bridge == bridge)
 389                         return ocelot_port->bridge_num;
 390         }
 391
 392         return -1;
 393 }
 394 EXPORT_SYMBOL_GPL(ocelot_bridge_num_find);
 395
 396 static u16 ocelot_vlan_unaware_pvid(struct ocelot *ocelot,
 397                                     const struct net_device *bridge)
 398 {
 399         int bridge_num;
 400
 401         /* Standalone ports use VID 0 */
 402         if (!bridge)
 403                 return 0;
 404
 405         bridge_num = ocelot_bridge_num_find(ocelot, bridge);
 406         if (WARN_ON(bridge_num < 0))
 407                 return 0;
 408
 409         /* VLAN-unaware bridges use a reserved VID going from 4095 downwards */
 410         return VLAN_N_VID - bridge_num - 1;
 411 }
 412
 413 /* Default vlan to clasify for untagged frames (may be zero) */
 414 static void ocelot_port_set_pvid(struct ocelot *ocelot, int port,
 415                                  const struct ocelot_bridge_vlan *pvid_vlan)
 416 {
 417         struct ocelot_port *ocelot_port = ocelot->ports[port];
 418         u16 pvid = ocelot_vlan_unaware_pvid(ocelot, ocelot_port->bridge);
 419         u32 val = 0;
 420
 421         ocelot_port->pvid_vlan = pvid_vlan;
 422
 423         if (ocelot_port->vlan_aware && pvid_vlan)
 424                 pvid = pvid_vlan->vid;
 425
 426         ocelot_rmw_gix(ocelot,
 427                        ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
 428                        ANA_PORT_VLAN_CFG_VLAN_VID_M,
 429                        ANA_PORT_VLAN_CFG, port);
 430
 431         /* If there's no pvid, we should drop not only untagged traffic (which
 432          * happens automatically), but also 802.1p traffic which gets
 433          * classified to VLAN 0, but that is always in our RX filter, so it
 434          * would get accepted were it not for this setting.
 435          */
 436         if (!pvid_vlan && ocelot_port->vlan_aware)
 437                 val = ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 438                       ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
 439
 440         ocelot_rmw_gix(ocelot, val,
 441                        ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
 442                        ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
 443                        ANA_PORT_DROP_CFG, port);
 444 }
 445
 446 static struct ocelot_bridge_vlan *ocelot_bridge_vlan_find(struct ocelot *ocelot,
 447                                                           u16 vid)
 448 {
 449         struct ocelot_bridge_vlan *vlan;
 450
 451         list_for_each_entry(vlan, &ocelot->vlans, list)
 452                 if (vlan->vid == vid)
 453                         return vlan;
 454
 455         return NULL;
 456 }
 457
 458 static int ocelot_vlan_member_add(struct ocelot *ocelot, int port, u16 vid,
 459                                   bool untagged)
 460 {
 461         struct ocelot_bridge_vlan *vlan = ocelot_bridge_vlan_find(ocelot, vid);
 462         unsigned long portmask;
 463         int err;
 464
 465         if (vlan) {
 466                 portmask = vlan->portmask | BIT(port);
 467
 468                 err = ocelot_vlant_set_mask(ocelot, vid, portmask);
 469                 if (err)
 470                         return err;
 471
 472                 vlan->portmask = portmask;
 473                 /* Bridge VLANs can be overwritten with a different
 474                  * egress-tagging setting, so make sure to override an untagged
 475                  * with a tagged VID if that's going on.
 476                  */
 477                 if (untagged)
 478                         vlan->untagged |= BIT(port);
 479                 else
 480                         vlan->untagged &= ~BIT(port);
 481
 482                 return 0;
 483         }
 484
 485         vlan = kzalloc(sizeof(*vlan), GFP_KERNEL);
 486         if (!vlan)
 487                 return -ENOMEM;
 488
 489         portmask = BIT(port);
 490
 491         err = ocelot_vlant_set_mask(ocelot, vid, portmask);
 492         if (err) {
 493                 kfree(vlan);
 494                 return err;
 495         }
 496
 497         vlan->vid = vid;
 498         vlan->portmask = portmask;
 499         if (untagged)
 500                 vlan->untagged = BIT(port);
 501         INIT_LIST_HEAD(&vlan->list);
 502         list_add_tail(&vlan->list, &ocelot->vlans);
 503
 504         return 0;
 505 }
 506
 507 static int ocelot_vlan_member_del(struct ocelot *ocelot, int port, u16 vid)
 508 {
 509         struct ocelot_bridge_vlan *vlan = ocelot_bridge_vlan_find(ocelot, vid);
 510         unsigned long portmask;
 511         int err;
 512
 513         if (!vlan)
 514                 return 0;
 515
 516         portmask = vlan->portmask & ~BIT(port);
 517
 518         err = ocelot_vlant_set_mask(ocelot, vid, portmask);
 519         if (err)
 520                 return err;
 521
 522         vlan->portmask = portmask;
 523         if (vlan->portmask)
 524                 return 0;
 525
 526         list_del(&vlan->list);
 527         kfree(vlan);
 528
 529         return 0;
 530 }
 531
 532 static int ocelot_add_vlan_unaware_pvid(struct ocelot *ocelot, int port,
 533                                         const struct net_device *bridge)
 534 {
 535         u16 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
 536
 537         return ocelot_vlan_member_add(ocelot, port, vid, true);
 538 }
 539
 540 static int ocelot_del_vlan_unaware_pvid(struct ocelot *ocelot, int port,
 541                                         const struct net_device *bridge)
 542 {
 543         u16 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
 544
 545         return ocelot_vlan_member_del(ocelot, port, vid);
 546 }
 547
 548 int ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
 549                                bool vlan_aware, struct netlink_ext_ack *extack)
 550 {
 551         struct ocelot_vcap_block *block = &ocelot->block[VCAP_IS1];
 552         struct ocelot_port *ocelot_port = ocelot->ports[port];
 553         struct ocelot_vcap_filter *filter;
 554         int err = 0;
 555         u32 val;
 556
 557         list_for_each_entry(filter, &block->rules, list) {
 558                 if (filter->ingress_port_mask & BIT(port) &&
 559                     filter->action.vid_replace_ena) {
 560                         NL_SET_ERR_MSG_MOD(extack,
 561                                            "Cannot change VLAN state with vlan modify rules active");
 562                         return -EBUSY;
 563                 }
 564         }
 565
 566         err = ocelot_single_vlan_aware_bridge(ocelot, extack);
 567         if (err)
 568                 return err;
 569
 570         if (vlan_aware)
 571                 err = ocelot_del_vlan_unaware_pvid(ocelot, port,
 572                                                    ocelot_port->bridge);
 573         else if (ocelot_port->bridge)
 574                 err = ocelot_add_vlan_unaware_pvid(ocelot, port,
 575                                                    ocelot_port->bridge);
 576         if (err)
 577                 return err;
 578
 579         ocelot_port->vlan_aware = vlan_aware;
 580
 581         if (vlan_aware)
 582                 val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 583                       ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
 584         else
 585                 val = 0;
 586         ocelot_rmw_gix(ocelot, val,
 587                        ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
 588                        ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
 589                        ANA_PORT_VLAN_CFG, port);
 590
 591         ocelot_port_set_pvid(ocelot, port, ocelot_port->pvid_vlan);
 592         ocelot_port_manage_port_tag(ocelot, port);
 593
 594         return 0;
 595 }
 596 EXPORT_SYMBOL(ocelot_port_vlan_filtering);
 597
 598 int ocelot_vlan_prepare(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 599                         bool untagged, struct netlink_ext_ack *extack)
 600 {
 601         if (untagged) {
 602                 /* We are adding an egress-tagged VLAN */
 603                 if (ocelot_port_uses_native_vlan(ocelot, port)) {
 604                         NL_SET_ERR_MSG_MOD(extack,
 605                                            "Port with egress-tagged VLANs cannot have more than one egress-untagged (native) VLAN");
 606                         return -EBUSY;
 607                 }
 608         } else {
 609                 /* We are adding an egress-tagged VLAN */
 610                 if (ocelot_port_num_untagged_vlans(ocelot, port) > 1) {
 611                         NL_SET_ERR_MSG_MOD(extack,
 612                                            "Port with more than one egress-untagged VLAN cannot have egress-tagged VLANs");
 613                         return -EBUSY;
 614                 }
 615         }
 616
 617         if (vid > OCELOT_RSV_VLAN_RANGE_START) {
 618                 NL_SET_ERR_MSG_MOD(extack,
 619                                    "VLAN range 4000-4095 reserved for VLAN-unaware bridging");
 620                 return -EBUSY;
 621         }
 622
 623         return 0;
 624 }
 625 EXPORT_SYMBOL(ocelot_vlan_prepare);
 626
 627 int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
 628                     bool untagged)
 629 {
 630         int err;
 631
 632         /* Ignore VID 0 added to our RX filter by the 8021q module, since
 633          * that collides with OCELOT_STANDALONE_PVID and changes it from
 634          * egress-untagged to egress-tagged.
 635          */
 636         if (!vid)
 637                 return 0;
 638
 639         err = ocelot_vlan_member_add(ocelot, port, vid, untagged);
 640         if (err)
 641                 return err;
 642
 643         /* Default ingress vlan classification */
 644         if (pvid)
 645                 ocelot_port_set_pvid(ocelot, port,
 646                                      ocelot_bridge_vlan_find(ocelot, vid));
 647
 648         /* Untagged egress vlan clasification */
 649         ocelot_port_manage_port_tag(ocelot, port);
 650
 651         return 0;
 652 }
 653 EXPORT_SYMBOL(ocelot_vlan_add);
 654
 655 int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
 656 {
 657         struct ocelot_port *ocelot_port = ocelot->ports[port];
 658         bool del_pvid = false;
 659         int err;
 660
 661         if (!vid)
 662                 return 0;
 663
 664         if (ocelot_port->pvid_vlan && ocelot_port->pvid_vlan->vid == vid)
 665                 del_pvid = true;
 666
 667         err = ocelot_vlan_member_del(ocelot, port, vid);
 668         if (err)
 669                 return err;
 670
 671         /* Ingress */
 672         if (del_pvid)
 673                 ocelot_port_set_pvid(ocelot, port, NULL);
 674
 675         /* Egress */
 676         ocelot_port_manage_port_tag(ocelot, port);
 677
 678         return 0;
 679 }
 680 EXPORT_SYMBOL(ocelot_vlan_del);
 681
 682 static void ocelot_vlan_init(struct ocelot *ocelot)
 683 {
 684         unsigned long all_ports = GENMASK(ocelot->num_phys_ports - 1, 0);
 685         u16 port, vid;
 686
 687         /* Clear VLAN table, by default all ports are members of all VLANs */
 688         ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
 689                      ANA_TABLES_VLANACCESS);
 690         ocelot_vlant_wait_for_completion(ocelot);
 691
 692         /* Configure the port VLAN memberships */
 693         for (vid = 1; vid < VLAN_N_VID; vid++)
 694                 ocelot_vlant_set_mask(ocelot, vid, 0);
 695
 696         /* We need VID 0 to get traffic on standalone ports.
 697          * It is added automatically if the 8021q module is loaded, but we
 698          * can't rely on that since it might not be.
 699          */
 700         ocelot_vlant_set_mask(ocelot, OCELOT_STANDALONE_PVID, all_ports);
 701
 702         /* Set vlan ingress filter mask to all ports but the CPU port by
 703          * default.
 704          */
 705         ocelot_write(ocelot, all_ports, ANA_VLANMASK);
 706
 707         for (port = 0; port < ocelot->num_phys_ports; port++) {
 708                 ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
 709                 ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
 710         }
 711 }
 712
 713 static u32 ocelot_read_eq_avail(struct ocelot *ocelot, int port)
 714 {
 715         return ocelot_read_rix(ocelot, QSYS_SW_STATUS, port);
 716 }
 717
 718 static int ocelot_port_flush(struct ocelot *ocelot, int port)
 719 {
 720         unsigned int pause_ena;
 721         int err, val;
 722
 723         /* Disable dequeuing from the egress queues */
 724         ocelot_rmw_rix(ocelot, QSYS_PORT_MODE_DEQUEUE_DIS,
 725                        QSYS_PORT_MODE_DEQUEUE_DIS,
 726                        QSYS_PORT_MODE, port);
 727
 728         /* Disable flow control */
 729         ocelot_fields_read(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, &pause_ena);
 730         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 0);
 731
 732         /* Disable priority flow control */
 733         ocelot_fields_write(ocelot, port,
 734                             QSYS_SWITCH_PORT_MODE_TX_PFC_ENA, 0);
 735
 736         /* Wait at least the time it takes to receive a frame of maximum length
 737          * at the port.
 738          * Worst-case delays for 10 kilobyte jumbo frames are:
 739          * 8 ms on a 10M port
 740          * 800 μs on a 100M port
 741          * 80 μs on a 1G port
 742          * 32 μs on a 2.5G port
 743          */
 744         usleep_range(8000, 10000);
 745
 746         /* Disable half duplex backpressure. */
 747         ocelot_rmw_rix(ocelot, 0, SYS_FRONT_PORT_MODE_HDX_MODE,
 748                        SYS_FRONT_PORT_MODE, port);
 749
 750         /* Flush the queues associated with the port. */
 751         ocelot_rmw_gix(ocelot, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG_FLUSH_ENA,
 752                        REW_PORT_CFG, port);
 753
 754         /* Enable dequeuing from the egress queues. */
 755         ocelot_rmw_rix(ocelot, 0, QSYS_PORT_MODE_DEQUEUE_DIS, QSYS_PORT_MODE,
 756                        port);
 757
 758         /* Wait until flushing is complete. */
 759         err = read_poll_timeout(ocelot_read_eq_avail, val, !val,
 760                                 100, 2000000, false, ocelot, port);
 761
 762         /* Clear flushing again. */
 763         ocelot_rmw_gix(ocelot, 0, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG, port);
 764
 765         /* Re-enable flow control */
 766         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, pause_ena);
 767
 768         return err;
 769 }
 770
 771 void ocelot_phylink_mac_link_down(struct ocelot *ocelot, int port,
 772                                   unsigned int link_an_mode,
 773                                   phy_interface_t interface,
 774                                   unsigned long quirks)
 775 {
 776         struct ocelot_port *ocelot_port = ocelot->ports[port];
 777         int err;
 778
 779         ocelot_port->speed = SPEED_UNKNOWN;
 780
 781         ocelot_port_rmwl(ocelot_port, 0, DEV_MAC_ENA_CFG_RX_ENA,
 782                          DEV_MAC_ENA_CFG);
 783
 784         if (ocelot->ops->cut_through_fwd) {
 785                 mutex_lock(&ocelot->fwd_domain_lock);
 786                 ocelot->ops->cut_through_fwd(ocelot);
 787                 mutex_unlock(&ocelot->fwd_domain_lock);
 788         }
 789
 790         ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
 791
 792         err = ocelot_port_flush(ocelot, port);
 793         if (err)
 794                 dev_err(ocelot->dev, "failed to flush port %d: %d\n",
 795                         port, err);
 796
 797         /* Put the port in reset. */
 798         if (interface != PHY_INTERFACE_MODE_QSGMII ||
 799             !(quirks & OCELOT_QUIRK_QSGMII_PORTS_MUST_BE_UP))
 800                 ocelot_port_rmwl(ocelot_port,
 801                                  DEV_CLOCK_CFG_MAC_TX_RST |
 802                                  DEV_CLOCK_CFG_MAC_RX_RST,
 803                                  DEV_CLOCK_CFG_MAC_TX_RST |
 804                                  DEV_CLOCK_CFG_MAC_RX_RST,
 805                                  DEV_CLOCK_CFG);
 806 }
 807 EXPORT_SYMBOL_GPL(ocelot_phylink_mac_link_down);
 808
 809 void ocelot_phylink_mac_link_up(struct ocelot *ocelot, int port,
 810                                 struct phy_device *phydev,
 811                                 unsigned int link_an_mode,
 812                                 phy_interface_t interface,
 813                                 int speed, int duplex,
 814                                 bool tx_pause, bool rx_pause,
 815                                 unsigned long quirks)
 816 {
 817         struct ocelot_port *ocelot_port = ocelot->ports[port];
 818         int mac_speed, mode = 0;
 819         u32 mac_fc_cfg;
 820
 821         ocelot_port->speed = speed;
 822
 823         /* The MAC might be integrated in systems where the MAC speed is fixed
 824          * and it's the PCS who is performing the rate adaptation, so we have
 825          * to write "1000Mbps" into the LINK_SPEED field of DEV_CLOCK_CFG
 826          * (which is also its default value).
 827          */
 828         if ((quirks & OCELOT_QUIRK_PCS_PERFORMS_RATE_ADAPTATION) ||
 829             speed == SPEED_1000) {
 830                 mac_speed = OCELOT_SPEED_1000;
 831                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 832         } else if (speed == SPEED_2500) {
 833                 mac_speed = OCELOT_SPEED_2500;
 834                 mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
 835         } else if (speed == SPEED_100) {
 836                 mac_speed = OCELOT_SPEED_100;
 837         } else {
 838                 mac_speed = OCELOT_SPEED_10;
 839         }
 840
 841         if (duplex == DUPLEX_FULL)
 842                 mode |= DEV_MAC_MODE_CFG_FDX_ENA;
 843
 844         ocelot_port_writel(ocelot_port, mode, DEV_MAC_MODE_CFG);
 845
 846         /* Take port out of reset by clearing the MAC_TX_RST, MAC_RX_RST and
 847          * PORT_RST bits in DEV_CLOCK_CFG.
 848          */
 849         ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(mac_speed),
 850                            DEV_CLOCK_CFG);
 851
 852         switch (speed) {
 853         case SPEED_10:
 854                 mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(OCELOT_SPEED_10);
 855                 break;
 856         case SPEED_100:
 857                 mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(OCELOT_SPEED_100);
 858                 break;
 859         case SPEED_1000:
 860         case SPEED_2500:
 861                 mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(OCELOT_SPEED_1000);
 862                 break;
 863         default:
 864                 dev_err(ocelot->dev, "Unsupported speed on port %d: %d\n",
 865                         port, speed);
 866                 return;
 867         }
 868
 869         /* Handle RX pause in all cases, with 2500base-X this is used for rate
 870          * adaptation.
 871          */
 872         mac_fc_cfg |= SYS_MAC_FC_CFG_RX_FC_ENA;
 873
 874         if (tx_pause)
 875                 mac_fc_cfg |= SYS_MAC_FC_CFG_TX_FC_ENA |
 876                               SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
 877                               SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
 878                               SYS_MAC_FC_CFG_ZERO_PAUSE_ENA;
 879
 880         /* Flow control. Link speed is only used here to evaluate the time
 881          * specification in incoming pause frames.
 882          */
 883         ocelot_write_rix(ocelot, mac_fc_cfg, SYS_MAC_FC_CFG, port);
 884
 885         ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
 886
 887         /* Don't attempt to send PAUSE frames on the NPI port, it's broken */
 888         if (port != ocelot->npi)
 889                 ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA,
 890                                     tx_pause);
 891
 892         /* Undo the effects of ocelot_phylink_mac_link_down:
 893          * enable MAC module
 894          */
 895         ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
 896                            DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
 897
 898         /* If the port supports cut-through forwarding, update the masks before
 899          * enabling forwarding on the port.
 900          */
 901         if (ocelot->ops->cut_through_fwd) {
 902                 mutex_lock(&ocelot->fwd_domain_lock);
 903                 ocelot->ops->cut_through_fwd(ocelot);
 904                 mutex_unlock(&ocelot->fwd_domain_lock);
 905         }
 906
 907         /* Core: Enable port for frame transfer */
 908         ocelot_fields_write(ocelot, port,
 909                             QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 910 }
 911 EXPORT_SYMBOL_GPL(ocelot_phylink_mac_link_up);
 912
 913 static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
 914                                         struct sk_buff *clone)
 915 {
 916         struct ocelot_port *ocelot_port = ocelot->ports[port];
 917         unsigned long flags;
 918
 919         spin_lock_irqsave(&ocelot->ts_id_lock, flags);
 920
 921         if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
 922             ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
 923                 spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
 924                 return -EBUSY;
 925         }
 926
 927         skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
 928         /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
 929         OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
 930
 931         ocelot_port->ts_id++;
 932         if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
 933                 ocelot_port->ts_id = 0;
 934
 935         ocelot_port->ptp_skbs_in_flight++;
 936         ocelot->ptp_skbs_in_flight++;
 937
 938         skb_queue_tail(&ocelot_port->tx_skbs, clone);
 939
 940         spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
 941
 942         return 0;
 943 }
 944
 945 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
 946                                        unsigned int ptp_class)
 947 {
 948         struct ptp_header *hdr;
 949         u8 msgtype, twostep;
 950
 951         hdr = ptp_parse_header(skb, ptp_class);
 952         if (!hdr)
 953                 return false;
 954
 955         msgtype = ptp_get_msgtype(hdr, ptp_class);
 956         twostep = hdr->flag_field[0] & 0x2;
 957
 958         if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
 959                 return true;
 960
 961         return false;
 962 }
 963
 964 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
 965                                  struct sk_buff *skb,
 966                                  struct sk_buff **clone)
 967 {
 968         struct ocelot_port *ocelot_port = ocelot->ports[port];
 969         u8 ptp_cmd = ocelot_port->ptp_cmd;
 970         unsigned int ptp_class;
 971         int err;
 972
 973         /* Don't do anything if PTP timestamping not enabled */
 974         if (!ptp_cmd)
 975                 return 0;
 976
 977         ptp_class = ptp_classify_raw(skb);
 978         if (ptp_class == PTP_CLASS_NONE)
 979                 return -EINVAL;
 980
 981         /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
 982         if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
 983                 if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
 984                         OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
 985                         return 0;
 986                 }
 987
 988                 /* Fall back to two-step timestamping */
 989                 ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
 990         }
 991
 992         if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 993                 *clone = skb_clone_sk(skb);
 994                 if (!(*clone))
 995                         return -ENOMEM;
 996
 997                 err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
 998                 if (err)
 999                         return err;
1000
1001                 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
1002                 OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
1003         }
1004
1005         return 0;
1006 }
1007 EXPORT_SYMBOL(ocelot_port_txtstamp_request);
1008
1009 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
1010                                    struct timespec64 *ts)
1011 {
1012         unsigned long flags;
1013         u32 val;
1014
1015         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
1016
1017         /* Read current PTP time to get seconds */
1018         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
1019
1020         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
1021         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
1022         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
1023         ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
1024
1025         /* Read packet HW timestamp from FIFO */
1026         val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
1027         ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
1028
1029         /* Sec has incremented since the ts was registered */
1030         if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
1031                 ts->tv_sec--;
1032
1033         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
1034 }
1035
1036 static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
1037 {
1038         struct ptp_header *hdr;
1039
1040         hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
1041         if (WARN_ON(!hdr))
1042                 return false;
1043
1044         return seqid == ntohs(hdr->sequence_id);
1045 }
1046
1047 void ocelot_get_txtstamp(struct ocelot *ocelot)
1048 {
1049         int budget = OCELOT_PTP_QUEUE_SZ;
1050
1051         while (budget--) {
1052                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
1053                 struct skb_shared_hwtstamps shhwtstamps;
1054                 u32 val, id, seqid, txport;
1055                 struct ocelot_port *port;
1056                 struct timespec64 ts;
1057                 unsigned long flags;
1058
1059                 val = ocelot_read(ocelot, SYS_PTP_STATUS);
1060
1061                 /* Check if a timestamp can be retrieved */
1062                 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
1063                         break;
1064
1065                 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
1066
1067                 /* Retrieve the ts ID and Tx port */
1068                 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
1069                 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
1070                 seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
1071
1072                 port = ocelot->ports[txport];
1073
1074                 spin_lock(&ocelot->ts_id_lock);
1075                 port->ptp_skbs_in_flight--;
1076                 ocelot->ptp_skbs_in_flight--;
1077                 spin_unlock(&ocelot->ts_id_lock);
1078
1079                 /* Retrieve its associated skb */
1080 try_again:
1081                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
1082
1083                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
1084                         if (OCELOT_SKB_CB(skb)->ts_id != id)
1085                                 continue;
1086                         __skb_unlink(skb, &port->tx_skbs);
1087                         skb_match = skb;
1088                         break;
1089                 }
1090
1091                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
1092
1093                 if (WARN_ON(!skb_match))
1094                         continue;
1095
1096                 if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
1097                         dev_err_ratelimited(ocelot->dev,
1098                                             "port %d received stale TX timestamp for seqid %d, discarding\n",
1099                                             txport, seqid);
1100                         dev_kfree_skb_any(skb);
1101                         goto try_again;
1102                 }
1103
1104                 /* Get the h/w timestamp */
1105                 ocelot_get_hwtimestamp(ocelot, &ts);
1106
1107                 /* Set the timestamp into the skb */
1108                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
1109                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
1110                 skb_complete_tx_timestamp(skb_match, &shhwtstamps);
1111
1112                 /* Next ts */
1113                 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
1114         }
1115 }
1116 EXPORT_SYMBOL(ocelot_get_txtstamp);
1117
1118 static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
1119                                 u32 *rval)
1120 {
1121         u32 bytes_valid, val;
1122
1123         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1124         if (val == XTR_NOT_READY) {
1125                 if (ifh)
1126                         return -EIO;
1127
1128                 do {
1129                         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1130                 } while (val == XTR_NOT_READY);
1131         }
1132
1133         switch (val) {
1134         case XTR_ABORT:
1135                 return -EIO;
1136         case XTR_EOF_0:
1137         case XTR_EOF_1:
1138         case XTR_EOF_2:
1139         case XTR_EOF_3:
1140         case XTR_PRUNED:
1141                 bytes_valid = XTR_VALID_BYTES(val);
1142                 val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1143                 if (val == XTR_ESCAPE)
1144                         *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1145                 else
1146                         *rval = val;
1147
1148                 return bytes_valid;
1149         case XTR_ESCAPE:
1150                 *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1151
1152                 return 4;
1153         default:
1154                 *rval = val;
1155
1156                 return 4;
1157         }
1158 }
1159
1160 static int ocelot_xtr_poll_xfh(struct ocelot *ocelot, int grp, u32 *xfh)
1161 {
1162         int i, err = 0;
1163
1164         for (i = 0; i < OCELOT_TAG_LEN / 4; i++) {
1165                 err = ocelot_rx_frame_word(ocelot, grp, true, &xfh[i]);
1166                 if (err != 4)
1167                         return (err < 0) ? err : -EIO;
1168         }
1169
1170         return 0;
1171 }
1172
1173 void ocelot_ptp_rx_timestamp(struct ocelot *ocelot, struct sk_buff *skb,
1174                              u64 timestamp)
1175 {
1176         struct skb_shared_hwtstamps *shhwtstamps;
1177         u64 tod_in_ns, full_ts_in_ns;
1178         struct timespec64 ts;
1179
1180         ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
1181
1182         tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
1183         if ((tod_in_ns & 0xffffffff) < timestamp)
1184                 full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
1185                                 timestamp;
1186         else
1187                 full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
1188                                 timestamp;
1189
1190         shhwtstamps = skb_hwtstamps(skb);
1191         memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
1192         shhwtstamps->hwtstamp = full_ts_in_ns;
1193 }
1194 EXPORT_SYMBOL(ocelot_ptp_rx_timestamp);
1195
1196 int ocelot_xtr_poll_frame(struct ocelot *ocelot, int grp, struct sk_buff **nskb)
1197 {
1198         u64 timestamp, src_port, len;
1199         u32 xfh[OCELOT_TAG_LEN / 4];
1200         struct net_device *dev;
1201         struct sk_buff *skb;
1202         int sz, buf_len;
1203         u32 val, *buf;
1204         int err;
1205
1206         err = ocelot_xtr_poll_xfh(ocelot, grp, xfh);
1207         if (err)
1208                 return err;
1209
1210         ocelot_xfh_get_src_port(xfh, &src_port);
1211         ocelot_xfh_get_len(xfh, &len);
1212         ocelot_xfh_get_rew_val(xfh, &timestamp);
1213
1214         if (WARN_ON(src_port >= ocelot->num_phys_ports))
1215                 return -EINVAL;
1216
1217         dev = ocelot->ops->port_to_netdev(ocelot, src_port);
1218         if (!dev)
1219                 return -EINVAL;
1220
1221         skb = netdev_alloc_skb(dev, len);
1222         if (unlikely(!skb)) {
1223                 netdev_err(dev, "Unable to allocate sk_buff\n");
1224                 return -ENOMEM;
1225         }
1226
1227         buf_len = len - ETH_FCS_LEN;
1228         buf = (u32 *)skb_put(skb, buf_len);
1229
1230         len = 0;
1231         do {
1232                 sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
1233                 if (sz < 0) {
1234                         err = sz;
1235                         goto out_free_skb;
1236                 }
1237                 *buf++ = val;
1238                 len += sz;
1239         } while (len < buf_len);
1240
1241         /* Read the FCS */
1242         sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
1243         if (sz < 0) {
1244                 err = sz;
1245                 goto out_free_skb;
1246         }
1247
1248         /* Update the statistics if part of the FCS was read before */
1249         len -= ETH_FCS_LEN - sz;
1250
1251         if (unlikely(dev->features & NETIF_F_RXFCS)) {
1252                 buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
1253                 *buf = val;
1254         }
1255
1256         if (ocelot->ptp)
1257                 ocelot_ptp_rx_timestamp(ocelot, skb, timestamp);
1258
1259         /* Everything we see on an interface that is in the HW bridge
1260          * has already been forwarded.
1261          */
1262         if (ocelot->ports[src_port]->bridge)
1263                 skb->offload_fwd_mark = 1;
1264
1265         skb->protocol = eth_type_trans(skb, dev);
1266
1267         *nskb = skb;
1268
1269         return 0;
1270
1271 out_free_skb:
1272         kfree_skb(skb);
1273         return err;
1274 }
1275 EXPORT_SYMBOL(ocelot_xtr_poll_frame);
1276
1277 bool ocelot_can_inject(struct ocelot *ocelot, int grp)
1278 {
1279         u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
1280
1281         if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))))
1282                 return false;
1283         if (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp)))
1284                 return false;
1285
1286         return true;
1287 }
1288 EXPORT_SYMBOL(ocelot_can_inject);
1289
1290 void ocelot_ifh_port_set(void *ifh, int port, u32 rew_op, u32 vlan_tag)
1291 {
1292         ocelot_ifh_set_bypass(ifh, 1);
1293         ocelot_ifh_set_dest(ifh, BIT_ULL(port));
1294         ocelot_ifh_set_tag_type(ifh, IFH_TAG_TYPE_C);
1295         if (vlan_tag)
1296                 ocelot_ifh_set_vlan_tci(ifh, vlan_tag);
1297         if (rew_op)
1298                 ocelot_ifh_set_rew_op(ifh, rew_op);
1299 }
1300 EXPORT_SYMBOL(ocelot_ifh_port_set);
1301
1302 void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
1303                               u32 rew_op, struct sk_buff *skb)
1304 {
1305         u32 ifh[OCELOT_TAG_LEN / 4] = {0};
1306         unsigned int i, count, last;
1307
1308         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
1309                          QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
1310
1311         ocelot_ifh_port_set(ifh, port, rew_op, skb_vlan_tag_get(skb));
1312
1313         for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
1314                 ocelot_write_rix(ocelot, ifh[i], QS_INJ_WR, grp);
1315
1316         count = DIV_ROUND_UP(skb->len, 4);
1317         last = skb->len % 4;
1318         for (i = 0; i < count; i++)
1319                 ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
1320
1321         /* Add padding */
1322         while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
1323                 ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
1324                 i++;
1325         }
1326
1327         /* Indicate EOF and valid bytes in last word */
1328         ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
1329                          QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
1330                          QS_INJ_CTRL_EOF,
1331                          QS_INJ_CTRL, grp);
1332
1333         /* Add dummy CRC */
1334         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
1335         skb_tx_timestamp(skb);
1336
1337         skb->dev->stats.tx_packets++;
1338         skb->dev->stats.tx_bytes += skb->len;
1339 }
1340 EXPORT_SYMBOL(ocelot_port_inject_frame);
1341
1342 void ocelot_drain_cpu_queue(struct ocelot *ocelot, int grp)
1343 {
1344         while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp))
1345                 ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1346 }
1347 EXPORT_SYMBOL(ocelot_drain_cpu_queue);
1348
1349 int ocelot_fdb_add(struct ocelot *ocelot, int port, const unsigned char *addr,
1350                    u16 vid, const struct net_device *bridge)
1351 {
1352         int pgid = port;
1353
1354         if (port == ocelot->npi)
1355                 pgid = PGID_CPU;
1356
1357         if (!vid)
1358                 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
1359
1360         return ocelot_mact_learn(ocelot, pgid, addr, vid, ENTRYTYPE_LOCKED);
1361 }
1362 EXPORT_SYMBOL(ocelot_fdb_add);
1363
1364 int ocelot_fdb_del(struct ocelot *ocelot, int port, const unsigned char *addr,
1365                    u16 vid, const struct net_device *bridge)
1366 {
1367         if (!vid)
1368                 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
1369
1370         return ocelot_mact_forget(ocelot, addr, vid);
1371 }
1372 EXPORT_SYMBOL(ocelot_fdb_del);
1373
1374 int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
1375                             bool is_static, void *data)
1376 {
1377         struct ocelot_dump_ctx *dump = data;
1378         u32 portid = NETLINK_CB(dump->cb->skb).portid;
1379         u32 seq = dump->cb->nlh->nlmsg_seq;
1380         struct nlmsghdr *nlh;
1381         struct ndmsg *ndm;
1382
1383         if (dump->idx < dump->cb->args[2])
1384                 goto skip;
1385
1386         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
1387                         sizeof(*ndm), NLM_F_MULTI);
1388         if (!nlh)
1389                 return -EMSGSIZE;
1390
1391         ndm = nlmsg_data(nlh);
1392         ndm->ndm_family  = AF_BRIDGE;
1393         ndm->ndm_pad1    = 0;
1394         ndm->ndm_pad2    = 0;
1395         ndm->ndm_flags   = NTF_SELF;
1396         ndm->ndm_type    = 0;
1397         ndm->ndm_ifindex = dump->dev->ifindex;
1398         ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
1399
1400         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
1401                 goto nla_put_failure;
1402
1403         if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
1404                 goto nla_put_failure;
1405
1406         nlmsg_end(dump->skb, nlh);
1407
1408 skip:
1409         dump->idx++;
1410         return 0;
1411
1412 nla_put_failure:
1413         nlmsg_cancel(dump->skb, nlh);
1414         return -EMSGSIZE;
1415 }
1416 EXPORT_SYMBOL(ocelot_port_fdb_do_dump);
1417
1418 /* Caller must hold &ocelot->mact_lock */
1419 static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
1420                             struct ocelot_mact_entry *entry)
1421 {
1422         u32 val, dst, macl, mach;
1423         char mac[ETH_ALEN];
1424
1425         /* Set row and column to read from */
1426         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
1427         ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
1428
1429         /* Issue a read command */
1430         ocelot_write(ocelot,
1431                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
1432                      ANA_TABLES_MACACCESS);
1433
1434         if (ocelot_mact_wait_for_completion(ocelot))
1435                 return -ETIMEDOUT;
1436
1437         /* Read the entry flags */
1438         val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
1439         if (!(val & ANA_TABLES_MACACCESS_VALID))
1440                 return -EINVAL;
1441
1442         /* If the entry read has another port configured as its destination,
1443          * do not report it.
1444          */
1445         dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
1446         if (dst != port)
1447                 return -EINVAL;
1448
1449         /* Get the entry's MAC address and VLAN id */
1450         macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
1451         mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
1452
1453         mac[0] = (mach >> 8)  & 0xff;
1454         mac[1] = (mach >> 0)  & 0xff;
1455         mac[2] = (macl >> 24) & 0xff;
1456         mac[3] = (macl >> 16) & 0xff;
1457         mac[4] = (macl >> 8)  & 0xff;
1458         mac[5] = (macl >> 0)  & 0xff;
1459
1460         entry->vid = (mach >> 16) & 0xfff;
1461         ether_addr_copy(entry->mac, mac);
1462
1463         return 0;
1464 }
1465
1466 int ocelot_mact_flush(struct ocelot *ocelot, int port)
1467 {
1468         int err;
1469
1470         mutex_lock(&ocelot->mact_lock);
1471
1472         /* Program ageing filter for a single port */
1473         ocelot_write(ocelot, ANA_ANAGEFIL_PID_EN | ANA_ANAGEFIL_PID_VAL(port),
1474                      ANA_ANAGEFIL);
1475
1476         /* Flushing dynamic FDB entries requires two successive age scans */
1477         ocelot_write(ocelot,
1478                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_AGE),
1479                      ANA_TABLES_MACACCESS);
1480
1481         err = ocelot_mact_wait_for_completion(ocelot);
1482         if (err) {
1483                 mutex_unlock(&ocelot->mact_lock);
1484                 return err;
1485         }
1486
1487         /* And second... */
1488         ocelot_write(ocelot,
1489                      ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_AGE),
1490                      ANA_TABLES_MACACCESS);
1491
1492         err = ocelot_mact_wait_for_completion(ocelot);
1493
1494         /* Restore ageing filter */
1495         ocelot_write(ocelot, 0, ANA_ANAGEFIL);
1496
1497         mutex_unlock(&ocelot->mact_lock);
1498
1499         return err;
1500 }
1501 EXPORT_SYMBOL_GPL(ocelot_mact_flush);
1502
1503 int ocelot_fdb_dump(struct ocelot *ocelot, int port,
1504                     dsa_fdb_dump_cb_t *cb, void *data)
1505 {
1506         int err = 0;
1507         int i, j;
1508
1509         /* We could take the lock just around ocelot_mact_read, but doing so
1510          * thousands of times in a row seems rather pointless and inefficient.
1511          */
1512         mutex_lock(&ocelot->mact_lock);
1513
1514         /* Loop through all the mac tables entries. */
1515         for (i = 0; i < ocelot->num_mact_rows; i++) {
1516                 for (j = 0; j < 4; j++) {
1517                         struct ocelot_mact_entry entry;
1518                         bool is_static;
1519
1520                         err = ocelot_mact_read(ocelot, port, i, j, &entry);
1521                         /* If the entry is invalid (wrong port, invalid...),
1522                          * skip it.
1523                          */
1524                         if (err == -EINVAL)
1525                                 continue;
1526                         else if (err)
1527                                 break;
1528
1529                         is_static = (entry.type == ENTRYTYPE_LOCKED);
1530
1531                         /* Hide the reserved VLANs used for
1532                          * VLAN-unaware bridging.
1533                          */
1534                         if (entry.vid > OCELOT_RSV_VLAN_RANGE_START)
1535                                 entry.vid = 0;
1536
1537                         err = cb(entry.mac, entry.vid, is_static, data);
1538                         if (err)
1539                                 break;
1540                 }
1541         }
1542
1543         mutex_unlock(&ocelot->mact_lock);
1544
1545         return err;
1546 }
1547 EXPORT_SYMBOL(ocelot_fdb_dump);
1548
1549 static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap)
1550 {
1551         trap->key_type = OCELOT_VCAP_KEY_ETYPE;
1552         *(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588);
1553         *(__be16 *)trap->key.etype.etype.mask = htons(0xffff);
1554 }
1555
1556 static void
1557 ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
1558 {
1559         trap->key_type = OCELOT_VCAP_KEY_IPV4;
1560         trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1561         trap->key.ipv4.proto.mask[0] = 0xff;
1562         trap->key.ipv4.dport.value = PTP_EV_PORT;
1563         trap->key.ipv4.dport.mask = 0xffff;
1564 }
1565
1566 static void
1567 ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
1568 {
1569         trap->key_type = OCELOT_VCAP_KEY_IPV6;
1570         trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1571         trap->key.ipv4.proto.mask[0] = 0xff;
1572         trap->key.ipv6.dport.value = PTP_EV_PORT;
1573         trap->key.ipv6.dport.mask = 0xffff;
1574 }
1575
1576 static void
1577 ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
1578 {
1579         trap->key_type = OCELOT_VCAP_KEY_IPV4;
1580         trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1581         trap->key.ipv4.proto.mask[0] = 0xff;
1582         trap->key.ipv4.dport.value = PTP_GEN_PORT;
1583         trap->key.ipv4.dport.mask = 0xffff;
1584 }
1585
1586 static void
1587 ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
1588 {
1589         trap->key_type = OCELOT_VCAP_KEY_IPV6;
1590         trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1591         trap->key.ipv4.proto.mask[0] = 0xff;
1592         trap->key.ipv6.dport.value = PTP_GEN_PORT;
1593         trap->key.ipv6.dport.mask = 0xffff;
1594 }
1595
1596 int ocelot_trap_add(struct ocelot *ocelot, int port,
1597                     unsigned long cookie, bool take_ts,
1598                     void (*populate)(struct ocelot_vcap_filter *f))
1599 {
1600         struct ocelot_vcap_block *block_vcap_is2;
1601         struct ocelot_vcap_filter *trap;
1602         bool new = false;
1603         int err;
1604
1605         block_vcap_is2 = &ocelot->block[VCAP_IS2];
1606
1607         trap = ocelot_vcap_block_find_filter_by_id(block_vcap_is2, cookie,
1608                                                    false);
1609         if (!trap) {
1610                 trap = kzalloc(sizeof(*trap), GFP_KERNEL);
1611                 if (!trap)
1612                         return -ENOMEM;
1613
1614                 populate(trap);
1615                 trap->prio = 1;
1616                 trap->id.cookie = cookie;
1617                 trap->id.tc_offload = false;
1618                 trap->block_id = VCAP_IS2;
1619                 trap->type = OCELOT_VCAP_FILTER_OFFLOAD;
1620                 trap->lookup = 0;
1621                 trap->action.cpu_copy_ena = true;
1622                 trap->action.mask_mode = OCELOT_MASK_MODE_PERMIT_DENY;
1623                 trap->action.port_mask = 0;
1624                 trap->take_ts = take_ts;
1625                 list_add_tail(&trap->trap_list, &ocelot->traps);
1626                 new = true;
1627         }
1628
1629         trap->ingress_port_mask |= BIT(port);
1630
1631         if (new)
1632                 err = ocelot_vcap_filter_add(ocelot, trap, NULL);
1633         else
1634                 err = ocelot_vcap_filter_replace(ocelot, trap);
1635         if (err) {
1636                 trap->ingress_port_mask &= ~BIT(port);
1637                 if (!trap->ingress_port_mask) {
1638                         list_del(&trap->trap_list);
1639                         kfree(trap);
1640                 }
1641                 return err;
1642         }
1643
1644         return 0;
1645 }
1646
1647 int ocelot_trap_del(struct ocelot *ocelot, int port, unsigned long cookie)
1648 {
1649         struct ocelot_vcap_block *block_vcap_is2;
1650         struct ocelot_vcap_filter *trap;
1651
1652         block_vcap_is2 = &ocelot->block[VCAP_IS2];
1653
1654         trap = ocelot_vcap_block_find_filter_by_id(block_vcap_is2, cookie,
1655                                                    false);
1656         if (!trap)
1657                 return 0;
1658
1659         trap->ingress_port_mask &= ~BIT(port);
1660         if (!trap->ingress_port_mask) {
1661                 list_del(&trap->trap_list);
1662
1663                 return ocelot_vcap_filter_del(ocelot, trap);
1664         }
1665
1666         return ocelot_vcap_filter_replace(ocelot, trap);
1667 }
1668
1669 static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port)
1670 {
1671         unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
1672
1673         return ocelot_trap_add(ocelot, port, l2_cookie, true,
1674                                ocelot_populate_l2_ptp_trap_key);
1675 }
1676
1677 static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port)
1678 {
1679         unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
1680
1681         return ocelot_trap_del(ocelot, port, l2_cookie);
1682 }
1683
1684 static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port)
1685 {
1686         unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
1687         unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
1688         int err;
1689
1690         err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true,
1691                               ocelot_populate_ipv4_ptp_event_trap_key);
1692         if (err)
1693                 return err;
1694
1695         err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false,
1696                               ocelot_populate_ipv4_ptp_general_trap_key);
1697         if (err)
1698                 ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
1699
1700         return err;
1701 }
1702
1703 static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port)
1704 {
1705         unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
1706         unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
1707         int err;
1708
1709         err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
1710         err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie);
1711         return err;
1712 }
1713
1714 static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port)
1715 {
1716         unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
1717         unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
1718         int err;
1719
1720         err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true,
1721                               ocelot_populate_ipv6_ptp_event_trap_key);
1722         if (err)
1723                 return err;
1724
1725         err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false,
1726                               ocelot_populate_ipv6_ptp_general_trap_key);
1727         if (err)
1728                 ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
1729
1730         return err;
1731 }
1732
1733 static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port)
1734 {
1735         unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
1736         unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
1737         int err;
1738
1739         err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
1740         err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie);
1741         return err;
1742 }
1743
1744 static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
1745                                   bool l2, bool l4)
1746 {
1747         int err;
1748
1749         if (l2)
1750                 err = ocelot_l2_ptp_trap_add(ocelot, port);
1751         else
1752                 err = ocelot_l2_ptp_trap_del(ocelot, port);
1753         if (err)
1754                 return err;
1755
1756         if (l4) {
1757                 err = ocelot_ipv4_ptp_trap_add(ocelot, port);
1758                 if (err)
1759                         goto err_ipv4;
1760
1761                 err = ocelot_ipv6_ptp_trap_add(ocelot, port);
1762                 if (err)
1763                         goto err_ipv6;
1764         } else {
1765                 err = ocelot_ipv4_ptp_trap_del(ocelot, port);
1766
1767                 err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
1768         }
1769         if (err)
1770                 return err;
1771
1772         return 0;
1773
1774 err_ipv6:
1775         ocelot_ipv4_ptp_trap_del(ocelot, port);
1776 err_ipv4:
1777         if (l2)
1778                 ocelot_l2_ptp_trap_del(ocelot, port);
1779         return err;
1780 }
1781
1782 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
1783 {
1784         return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
1785                             sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
1786 }
1787 EXPORT_SYMBOL(ocelot_hwstamp_get);
1788
1789 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
1790 {
1791         struct ocelot_port *ocelot_port = ocelot->ports[port];
1792         bool l2 = false, l4 = false;
1793         struct hwtstamp_config cfg;
1794         int err;
1795
1796         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
1797                 return -EFAULT;
1798
1799         /* Tx type sanity check */
1800         switch (cfg.tx_type) {
1801         case HWTSTAMP_TX_ON:
1802                 ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
1803                 break;
1804         case HWTSTAMP_TX_ONESTEP_SYNC:
1805                 /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
1806                  * need to update the origin time.
1807                  */
1808                 ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
1809                 break;
1810         case HWTSTAMP_TX_OFF:
1811                 ocelot_port->ptp_cmd = 0;
1812                 break;
1813         default:
1814                 return -ERANGE;
1815         }
1816
1817         mutex_lock(&ocelot->ptp_lock);
1818
1819         switch (cfg.rx_filter) {
1820         case HWTSTAMP_FILTER_NONE:
1821                 break;
1822         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1823         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1824         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1825                 l4 = true;
1826                 break;
1827         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1828         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1829         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1830                 l2 = true;
1831                 break;
1832         case HWTSTAMP_FILTER_PTP_V2_EVENT:
1833         case HWTSTAMP_FILTER_PTP_V2_SYNC:
1834         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1835                 l2 = true;
1836                 l4 = true;
1837                 break;
1838         default:
1839                 mutex_unlock(&ocelot->ptp_lock);
1840                 return -ERANGE;
1841         }
1842
1843         err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
1844         if (err) {
1845                 mutex_unlock(&ocelot->ptp_lock);
1846                 return err;
1847         }
1848
1849         if (l2 && l4)
1850                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
1851         else if (l2)
1852                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
1853         else if (l4)
1854                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
1855         else
1856                 cfg.rx_filter = HWTSTAMP_FILTER_NONE;
1857
1858         /* Commit back the result & save it */
1859         memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
1860         mutex_unlock(&ocelot->ptp_lock);
1861
1862         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
1863 }
1864 EXPORT_SYMBOL(ocelot_hwstamp_set);
1865
1866 void ocelot_get_strings(struct ocelot *ocelot, int port, u32 sset, u8 *data)
1867 {
1868         int i;
1869
1870         if (sset != ETH_SS_STATS)
1871                 return;
1872
1873         for (i = 0; i < ocelot->num_stats; i++)
1874                 memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
1875                        ETH_GSTRING_LEN);
1876 }
1877 EXPORT_SYMBOL(ocelot_get_strings);
1878
1879 /* Caller must hold &ocelot->stats_lock */
1880 static int ocelot_port_update_stats(struct ocelot *ocelot, int port)
1881 {
1882         unsigned int idx = port * ocelot->num_stats;
1883         struct ocelot_stats_region *region;
1884         int err, j;
1885
1886         /* Configure the port to read the stats from */
1887         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port), SYS_STAT_CFG);
1888
1889         list_for_each_entry(region, &ocelot->stats_regions, node) {
1890                 err = ocelot_bulk_read_rix(ocelot, SYS_COUNT_RX_OCTETS,
1891                                            region->offset, region->buf,
1892                                            region->count);
1893                 if (err)
1894                         return err;
1895
1896                 for (j = 0; j < region->count; j++) {
1897                         u64 *stat = &ocelot->stats[idx + j];
1898                         u64 val = region->buf[j];
1899
1900                         if (val < (*stat & U32_MAX))
1901                                 *stat += (u64)1 << 32;
1902
1903                         *stat = (*stat & ~(u64)U32_MAX) + val;
1904                 }
1905
1906                 idx += region->count;
1907         }
1908
1909         return err;
1910 }
1911
1912 static void ocelot_check_stats_work(struct work_struct *work)
1913 {
1914         struct delayed_work *del_work = to_delayed_work(work);
1915         struct ocelot *ocelot = container_of(del_work, struct ocelot,
1916                                              stats_work);
1917         int i, err;
1918
1919         mutex_lock(&ocelot->stats_lock);
1920         for (i = 0; i < ocelot->num_phys_ports; i++) {
1921                 err = ocelot_port_update_stats(ocelot, i);
1922                 if (err)
1923                         break;
1924         }
1925         mutex_unlock(&ocelot->stats_lock);
1926
1927         if (err)
1928                 dev_err(ocelot->dev, "Error %d updating ethtool stats\n",  err);
1929
1930         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
1931                            OCELOT_STATS_CHECK_DELAY);
1932 }
1933
1934 void ocelot_get_ethtool_stats(struct ocelot *ocelot, int port, u64 *data)
1935 {
1936         int i, err;
1937
1938         mutex_lock(&ocelot->stats_lock);
1939
1940         /* check and update now */
1941         err = ocelot_port_update_stats(ocelot, port);
1942
1943         /* Copy all counters */
1944         for (i = 0; i < ocelot->num_stats; i++)
1945                 *data++ = ocelot->stats[port * ocelot->num_stats + i];
1946
1947         mutex_unlock(&ocelot->stats_lock);
1948
1949         if (err)
1950                 dev_err(ocelot->dev, "Error %d updating ethtool stats\n", err);
1951 }
1952 EXPORT_SYMBOL(ocelot_get_ethtool_stats);
1953
1954 int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
1955 {
1956         if (sset != ETH_SS_STATS)
1957                 return -EOPNOTSUPP;
1958
1959         return ocelot->num_stats;
1960 }
1961 EXPORT_SYMBOL(ocelot_get_sset_count);
1962
1963 static int ocelot_prepare_stats_regions(struct ocelot *ocelot)
1964 {
1965         struct ocelot_stats_region *region = NULL;
1966         unsigned int last;
1967         int i;
1968
1969         INIT_LIST_HEAD(&ocelot->stats_regions);
1970
1971         for (i = 0; i < ocelot->num_stats; i++) {
1972                 if (region && ocelot->stats_layout[i].offset == last + 1) {
1973                         region->count++;
1974                 } else {
1975                         region = devm_kzalloc(ocelot->dev, sizeof(*region),
1976                                               GFP_KERNEL);
1977                         if (!region)
1978                                 return -ENOMEM;
1979
1980                         region->offset = ocelot->stats_layout[i].offset;
1981                         region->count = 1;
1982                         list_add_tail(&region->node, &ocelot->stats_regions);
1983                 }
1984
1985                 last = ocelot->stats_layout[i].offset;
1986         }
1987
1988         list_for_each_entry(region, &ocelot->stats_regions, node) {
1989                 region->buf = devm_kcalloc(ocelot->dev, region->count,
1990                                            sizeof(*region->buf), GFP_KERNEL);
1991                 if (!region->buf)
1992                         return -ENOMEM;
1993         }
1994
1995         return 0;
1996 }
1997
1998 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
1999                        struct ethtool_ts_info *info)
2000 {
2001         info->phc_index = ocelot->ptp_clock ?
2002                           ptp_clock_index(ocelot->ptp_clock) : -1;
2003         if (info->phc_index == -1) {
2004                 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
2005                                          SOF_TIMESTAMPING_RX_SOFTWARE |
2006                                          SOF_TIMESTAMPING_SOFTWARE;
2007                 return 0;
2008         }
2009         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
2010                                  SOF_TIMESTAMPING_RX_SOFTWARE |
2011                                  SOF_TIMESTAMPING_SOFTWARE |
2012                                  SOF_TIMESTAMPING_TX_HARDWARE |
2013                                  SOF_TIMESTAMPING_RX_HARDWARE |
2014                                  SOF_TIMESTAMPING_RAW_HARDWARE;
2015         info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
2016                          BIT(HWTSTAMP_TX_ONESTEP_SYNC);
2017         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
2018                            BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
2019                            BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
2020                            BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
2021
2022         return 0;
2023 }
2024 EXPORT_SYMBOL(ocelot_get_ts_info);
2025
2026 static u32 ocelot_get_bond_mask(struct ocelot *ocelot, struct net_device *bond)
2027 {
2028         u32 mask = 0;
2029         int port;
2030
2031         lockdep_assert_held(&ocelot->fwd_domain_lock);
2032
2033         for (port = 0; port < ocelot->num_phys_ports; port++) {
2034                 struct ocelot_port *ocelot_port = ocelot->ports[port];
2035
2036                 if (!ocelot_port)
2037                         continue;
2038
2039                 if (ocelot_port->bond == bond)
2040                         mask |= BIT(port);
2041         }
2042
2043         return mask;
2044 }
2045
2046 /* The logical port number of a LAG is equal to the lowest numbered physical
2047  * port ID present in that LAG. It may change if that port ever leaves the LAG.
2048  */
2049 static int ocelot_bond_get_id(struct ocelot *ocelot, struct net_device *bond)
2050 {
2051         int bond_mask = ocelot_get_bond_mask(ocelot, bond);
2052
2053         if (!bond_mask)
2054                 return -ENOENT;
2055
2056         return __ffs(bond_mask);
2057 }
2058
2059 u32 ocelot_get_bridge_fwd_mask(struct ocelot *ocelot, int src_port)
2060 {
2061         struct ocelot_port *ocelot_port = ocelot->ports[src_port];
2062         const struct net_device *bridge;
2063         u32 mask = 0;
2064         int port;
2065
2066         if (!ocelot_port || ocelot_port->stp_state != BR_STATE_FORWARDING)
2067                 return 0;
2068
2069         bridge = ocelot_port->bridge;
2070         if (!bridge)
2071                 return 0;
2072
2073         for (port = 0; port < ocelot->num_phys_ports; port++) {
2074                 ocelot_port = ocelot->ports[port];
2075
2076                 if (!ocelot_port)
2077                         continue;
2078
2079                 if (ocelot_port->stp_state == BR_STATE_FORWARDING &&
2080                     ocelot_port->bridge == bridge)
2081                         mask |= BIT(port);
2082         }
2083
2084         return mask;
2085 }
2086 EXPORT_SYMBOL_GPL(ocelot_get_bridge_fwd_mask);
2087
2088 u32 ocelot_get_dsa_8021q_cpu_mask(struct ocelot *ocelot)
2089 {
2090         u32 mask = 0;
2091         int port;
2092
2093         for (port = 0; port < ocelot->num_phys_ports; port++) {
2094                 struct ocelot_port *ocelot_port = ocelot->ports[port];
2095
2096                 if (!ocelot_port)
2097                         continue;
2098
2099                 if (ocelot_port->is_dsa_8021q_cpu)
2100                         mask |= BIT(port);
2101         }
2102
2103         return mask;
2104 }
2105 EXPORT_SYMBOL_GPL(ocelot_get_dsa_8021q_cpu_mask);
2106
2107 void ocelot_apply_bridge_fwd_mask(struct ocelot *ocelot, bool joining)
2108 {
2109         unsigned long cpu_fwd_mask;
2110         int port;
2111
2112         lockdep_assert_held(&ocelot->fwd_domain_lock);
2113
2114         /* If cut-through forwarding is supported, update the masks before a
2115          * port joins the forwarding domain, to avoid potential underruns if it
2116          * has the highest speed from the new domain.
2117          */
2118         if (joining && ocelot->ops->cut_through_fwd)
2119                 ocelot->ops->cut_through_fwd(ocelot);
2120
2121         /* If a DSA tag_8021q CPU exists, it needs to be included in the
2122          * regular forwarding path of the front ports regardless of whether
2123          * those are bridged or standalone.
2124          * If DSA tag_8021q is not used, this returns 0, which is fine because
2125          * the hardware-based CPU port module can be a destination for packets
2126          * even if it isn't part of PGID_SRC.
2127          */
2128         cpu_fwd_mask = ocelot_get_dsa_8021q_cpu_mask(ocelot);
2129
2130         /* Apply FWD mask. The loop is needed to add/remove the current port as
2131          * a source for the other ports.
2132          */
2133         for (port = 0; port < ocelot->num_phys_ports; port++) {
2134                 struct ocelot_port *ocelot_port = ocelot->ports[port];
2135                 unsigned long mask;
2136
2137                 if (!ocelot_port) {
2138                         /* Unused ports can't send anywhere */
2139                         mask = 0;
2140                 } else if (ocelot_port->is_dsa_8021q_cpu) {
2141                         /* The DSA tag_8021q CPU ports need to be able to
2142                          * forward packets to all other ports except for
2143                          * themselves
2144                          */
2145                         mask = GENMASK(ocelot->num_phys_ports - 1, 0);
2146                         mask &= ~cpu_fwd_mask;
2147                 } else if (ocelot_port->bridge) {
2148                         struct net_device *bond = ocelot_port->bond;
2149
2150                         mask = ocelot_get_bridge_fwd_mask(ocelot, port);
2151                         mask |= cpu_fwd_mask;
2152                         mask &= ~BIT(port);
2153                         if (bond)
2154                                 mask &= ~ocelot_get_bond_mask(ocelot, bond);
2155                 } else {
2156                         /* Standalone ports forward only to DSA tag_8021q CPU
2157                          * ports (if those exist), or to the hardware CPU port
2158                          * module otherwise.
2159                          */
2160                         mask = cpu_fwd_mask;
2161                 }
2162
2163                 ocelot_write_rix(ocelot, mask, ANA_PGID_PGID, PGID_SRC + port);
2164         }
2165
2166         /* If cut-through forwarding is supported and a port is leaving, there
2167          * is a chance that cut-through was disabled on the other ports due to
2168          * the port which is leaving (it has a higher link speed). We need to
2169          * update the cut-through masks of the remaining ports no earlier than
2170          * after the port has left, to prevent underruns from happening between
2171          * the cut-through update and the forwarding domain update.
2172          */
2173         if (!joining && ocelot->ops->cut_through_fwd)
2174                 ocelot->ops->cut_through_fwd(ocelot);
2175 }
2176 EXPORT_SYMBOL(ocelot_apply_bridge_fwd_mask);
2177
2178 void ocelot_port_set_dsa_8021q_cpu(struct ocelot *ocelot, int port)
2179 {
2180         u16 vid;
2181
2182         ocelot->ports[port]->is_dsa_8021q_cpu = true;
2183
2184         for (vid = OCELOT_RSV_VLAN_RANGE_START; vid < VLAN_N_VID; vid++)
2185                 ocelot_vlan_member_add(ocelot, port, vid, true);
2186 }
2187 EXPORT_SYMBOL_GPL(ocelot_port_set_dsa_8021q_cpu);
2188
2189 void ocelot_port_unset_dsa_8021q_cpu(struct ocelot *ocelot, int port)
2190 {
2191         u16 vid;
2192
2193         ocelot->ports[port]->is_dsa_8021q_cpu = false;
2194
2195         for (vid = OCELOT_RSV_VLAN_RANGE_START; vid < VLAN_N_VID; vid++)
2196                 ocelot_vlan_member_del(ocelot, port, vid);
2197 }
2198 EXPORT_SYMBOL_GPL(ocelot_port_unset_dsa_8021q_cpu);
2199
2200 void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
2201 {
2202         struct ocelot_port *ocelot_port = ocelot->ports[port];
2203         u32 learn_ena = 0;
2204
2205         mutex_lock(&ocelot->fwd_domain_lock);
2206
2207         ocelot_port->stp_state = state;
2208
2209         if ((state == BR_STATE_LEARNING || state == BR_STATE_FORWARDING) &&
2210             ocelot_port->learn_ena)
2211                 learn_ena = ANA_PORT_PORT_CFG_LEARN_ENA;
2212
2213         ocelot_rmw_gix(ocelot, learn_ena, ANA_PORT_PORT_CFG_LEARN_ENA,
2214                        ANA_PORT_PORT_CFG, port);
2215
2216         ocelot_apply_bridge_fwd_mask(ocelot, state == BR_STATE_FORWARDING);
2217
2218         mutex_unlock(&ocelot->fwd_domain_lock);
2219 }
2220 EXPORT_SYMBOL(ocelot_bridge_stp_state_set);
2221
2222 void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
2223 {
2224         unsigned int age_period = ANA_AUTOAGE_AGE_PERIOD(msecs / 2000);
2225
2226         /* Setting AGE_PERIOD to zero effectively disables automatic aging,
2227          * which is clearly not what our intention is. So avoid that.
2228          */
2229         if (!age_period)
2230                 age_period = 1;
2231
2232         ocelot_rmw(ocelot, age_period, ANA_AUTOAGE_AGE_PERIOD_M, ANA_AUTOAGE);
2233 }
2234 EXPORT_SYMBOL(ocelot_set_ageing_time);
2235
2236 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
2237                                                      const unsigned char *addr,
2238                                                      u16 vid)
2239 {
2240         struct ocelot_multicast *mc;
2241
2242         list_for_each_entry(mc, &ocelot->multicast, list) {
2243                 if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
2244                         return mc;
2245         }
2246
2247         return NULL;
2248 }
2249
2250 static enum macaccess_entry_type ocelot_classify_mdb(const unsigned char *addr)
2251 {
2252         if (addr[0] == 0x01 && addr[1] == 0x00 && addr[2] == 0x5e)
2253                 return ENTRYTYPE_MACv4;
2254         if (addr[0] == 0x33 && addr[1] == 0x33)
2255                 return ENTRYTYPE_MACv6;
2256         return ENTRYTYPE_LOCKED;
2257 }
2258
2259 static struct ocelot_pgid *ocelot_pgid_alloc(struct ocelot *ocelot, int index,
2260                                              unsigned long ports)
2261 {
2262         struct ocelot_pgid *pgid;
2263
2264         pgid = kzalloc(sizeof(*pgid), GFP_KERNEL);
2265         if (!pgid)
2266                 return ERR_PTR(-ENOMEM);
2267
2268         pgid->ports = ports;
2269         pgid->index = index;
2270         refcount_set(&pgid->refcount, 1);
2271         list_add_tail(&pgid->list, &ocelot->pgids);
2272
2273         return pgid;
2274 }
2275
2276 static void ocelot_pgid_free(struct ocelot *ocelot, struct ocelot_pgid *pgid)
2277 {
2278         if (!refcount_dec_and_test(&pgid->refcount))
2279                 return;
2280
2281         list_del(&pgid->list);
2282         kfree(pgid);
2283 }
2284
2285 static struct ocelot_pgid *ocelot_mdb_get_pgid(struct ocelot *ocelot,
2286                                                const struct ocelot_multicast *mc)
2287 {
2288         struct ocelot_pgid *pgid;
2289         int index;
2290
2291         /* According to VSC7514 datasheet 3.9.1.5 IPv4 Multicast Entries and
2292          * 3.9.1.6 IPv6 Multicast Entries, "Instead of a lookup in the
2293          * destination mask table (PGID), the destination set is programmed as
2294          * part of the entry MAC address.", and the DEST_IDX is set to 0.
2295          */
2296         if (mc->entry_type == ENTRYTYPE_MACv4 ||
2297             mc->entry_type == ENTRYTYPE_MACv6)
2298                 return ocelot_pgid_alloc(ocelot, 0, mc->ports);
2299
2300         list_for_each_entry(pgid, &ocelot->pgids, list) {
2301                 /* When searching for a nonreserved multicast PGID, ignore the
2302                  * dummy PGID of zero that we have for MACv4/MACv6 entries
2303                  */
2304                 if (pgid->index && pgid->ports == mc->ports) {
2305                         refcount_inc(&pgid->refcount);
2306                         return pgid;
2307                 }
2308         }
2309
2310         /* Search for a free index in the nonreserved multicast PGID area */
2311         for_each_nonreserved_multicast_dest_pgid(ocelot, index) {
2312                 bool used = false;
2313
2314                 list_for_each_entry(pgid, &ocelot->pgids, list) {
2315                         if (pgid->index == index) {
2316                                 used = true;
2317                                 break;
2318                         }
2319                 }
2320
2321                 if (!used)
2322                         return ocelot_pgid_alloc(ocelot, index, mc->ports);
2323         }
2324
2325         return ERR_PTR(-ENOSPC);
2326 }
2327
2328 static void ocelot_encode_ports_to_mdb(unsigned char *addr,
2329                                        struct ocelot_multicast *mc)
2330 {
2331         ether_addr_copy(addr, mc->addr);
2332
2333         if (mc->entry_type == ENTRYTYPE_MACv4) {
2334                 addr[0] = 0;
2335                 addr[1] = mc->ports >> 8;
2336                 addr[2] = mc->ports & 0xff;
2337         } else if (mc->entry_type == ENTRYTYPE_MACv6) {
2338                 addr[0] = mc->ports >> 8;
2339                 addr[1] = mc->ports & 0xff;
2340         }
2341 }
2342
2343 int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
2344                         const struct switchdev_obj_port_mdb *mdb,
2345                         const struct net_device *bridge)
2346 {
2347         unsigned char addr[ETH_ALEN];
2348         struct ocelot_multicast *mc;
2349         struct ocelot_pgid *pgid;
2350         u16 vid = mdb->vid;
2351
2352         if (port == ocelot->npi)
2353                 port = ocelot->num_phys_ports;
2354
2355         if (!vid)
2356                 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2357
2358         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
2359         if (!mc) {
2360                 /* New entry */
2361                 mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
2362                 if (!mc)
2363                         return -ENOMEM;
2364
2365                 mc->entry_type = ocelot_classify_mdb(mdb->addr);
2366                 ether_addr_copy(mc->addr, mdb->addr);
2367                 mc->vid = vid;
2368
2369                 list_add_tail(&mc->list, &ocelot->multicast);
2370         } else {
2371                 /* Existing entry. Clean up the current port mask from
2372                  * hardware now, because we'll be modifying it.
2373                  */
2374                 ocelot_pgid_free(ocelot, mc->pgid);
2375                 ocelot_encode_ports_to_mdb(addr, mc);
2376                 ocelot_mact_forget(ocelot, addr, vid);
2377         }
2378
2379         mc->ports |= BIT(port);
2380
2381         pgid = ocelot_mdb_get_pgid(ocelot, mc);
2382         if (IS_ERR(pgid)) {
2383                 dev_err(ocelot->dev,
2384                         "Cannot allocate PGID for mdb %pM vid %d\n",
2385                         mc->addr, mc->vid);
2386                 devm_kfree(ocelot->dev, mc);
2387                 return PTR_ERR(pgid);
2388         }
2389         mc->pgid = pgid;
2390
2391         ocelot_encode_ports_to_mdb(addr, mc);
2392
2393         if (mc->entry_type != ENTRYTYPE_MACv4 &&
2394             mc->entry_type != ENTRYTYPE_MACv6)
2395                 ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
2396                                  pgid->index);
2397
2398         return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
2399                                  mc->entry_type);
2400 }
2401 EXPORT_SYMBOL(ocelot_port_mdb_add);
2402
2403 int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
2404                         const struct switchdev_obj_port_mdb *mdb,
2405                         const struct net_device *bridge)
2406 {
2407         unsigned char addr[ETH_ALEN];
2408         struct ocelot_multicast *mc;
2409         struct ocelot_pgid *pgid;
2410         u16 vid = mdb->vid;
2411
2412         if (port == ocelot->npi)
2413                 port = ocelot->num_phys_ports;
2414
2415         if (!vid)
2416                 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2417
2418         mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
2419         if (!mc)
2420                 return -ENOENT;
2421
2422         ocelot_encode_ports_to_mdb(addr, mc);
2423         ocelot_mact_forget(ocelot, addr, vid);
2424
2425         ocelot_pgid_free(ocelot, mc->pgid);
2426         mc->ports &= ~BIT(port);
2427         if (!mc->ports) {
2428                 list_del(&mc->list);
2429                 devm_kfree(ocelot->dev, mc);
2430                 return 0;
2431         }
2432
2433         /* We have a PGID with fewer ports now */
2434         pgid = ocelot_mdb_get_pgid(ocelot, mc);
2435         if (IS_ERR(pgid))
2436                 return PTR_ERR(pgid);
2437         mc->pgid = pgid;
2438
2439         ocelot_encode_ports_to_mdb(addr, mc);
2440
2441         if (mc->entry_type != ENTRYTYPE_MACv4 &&
2442             mc->entry_type != ENTRYTYPE_MACv6)
2443                 ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
2444                                  pgid->index);
2445
2446         return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
2447                                  mc->entry_type);
2448 }
2449 EXPORT_SYMBOL(ocelot_port_mdb_del);
2450
2451 int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
2452                             struct net_device *bridge, int bridge_num,
2453                             struct netlink_ext_ack *extack)
2454 {
2455         struct ocelot_port *ocelot_port = ocelot->ports[port];
2456         int err;
2457
2458         err = ocelot_single_vlan_aware_bridge(ocelot, extack);
2459         if (err)
2460                 return err;
2461
2462         mutex_lock(&ocelot->fwd_domain_lock);
2463
2464         ocelot_port->bridge = bridge;
2465         ocelot_port->bridge_num = bridge_num;
2466
2467         ocelot_apply_bridge_fwd_mask(ocelot, true);
2468
2469         mutex_unlock(&ocelot->fwd_domain_lock);
2470
2471         if (br_vlan_enabled(bridge))
2472                 return 0;
2473
2474         return ocelot_add_vlan_unaware_pvid(ocelot, port, bridge);
2475 }
2476 EXPORT_SYMBOL(ocelot_port_bridge_join);
2477
2478 void ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
2479                               struct net_device *bridge)
2480 {
2481         struct ocelot_port *ocelot_port = ocelot->ports[port];
2482
2483         mutex_lock(&ocelot->fwd_domain_lock);
2484
2485         if (!br_vlan_enabled(bridge))
2486                 ocelot_del_vlan_unaware_pvid(ocelot, port, bridge);
2487
2488         ocelot_port->bridge = NULL;
2489         ocelot_port->bridge_num = -1;
2490
2491         ocelot_port_set_pvid(ocelot, port, NULL);
2492         ocelot_port_manage_port_tag(ocelot, port);
2493         ocelot_apply_bridge_fwd_mask(ocelot, false);
2494
2495         mutex_unlock(&ocelot->fwd_domain_lock);
2496 }
2497 EXPORT_SYMBOL(ocelot_port_bridge_leave);
2498
2499 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
2500 {
2501         unsigned long visited = GENMASK(ocelot->num_phys_ports - 1, 0);
2502         int i, port, lag;
2503
2504         /* Reset destination and aggregation PGIDS */
2505         for_each_unicast_dest_pgid(ocelot, port)
2506                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
2507
2508         for_each_aggr_pgid(ocelot, i)
2509                 ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
2510                                  ANA_PGID_PGID, i);
2511
2512         /* The visited ports bitmask holds the list of ports offloading any
2513          * bonding interface. Initially we mark all these ports as unvisited,
2514          * then every time we visit a port in this bitmask, we know that it is
2515          * the lowest numbered port, i.e. the one whose logical ID == physical
2516          * port ID == LAG ID. So we mark as visited all further ports in the
2517          * bitmask that are offloading the same bonding interface. This way,
2518          * we set up the aggregation PGIDs only once per bonding interface.
2519          */
2520         for (port = 0; port < ocelot->num_phys_ports; port++) {
2521                 struct ocelot_port *ocelot_port = ocelot->ports[port];
2522
2523                 if (!ocelot_port || !ocelot_port->bond)
2524                         continue;
2525
2526                 visited &= ~BIT(port);
2527         }
2528
2529         /* Now, set PGIDs for each active LAG */
2530         for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
2531                 struct net_device *bond = ocelot->ports[lag]->bond;
2532                 int num_active_ports = 0;
2533                 unsigned long bond_mask;
2534                 u8 aggr_idx[16];
2535
2536                 if (!bond || (visited & BIT(lag)))
2537                         continue;
2538
2539                 bond_mask = ocelot_get_bond_mask(ocelot, bond);
2540
2541                 for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
2542                         struct ocelot_port *ocelot_port = ocelot->ports[port];
2543
2544                         // Destination mask
2545                         ocelot_write_rix(ocelot, bond_mask,
2546                                          ANA_PGID_PGID, port);
2547
2548                         if (ocelot_port->lag_tx_active)
2549                                 aggr_idx[num_active_ports++] = port;
2550                 }
2551
2552                 for_each_aggr_pgid(ocelot, i) {
2553                         u32 ac;
2554
2555                         ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
2556                         ac &= ~bond_mask;
2557                         /* Don't do division by zero if there was no active
2558                          * port. Just make all aggregation codes zero.
2559                          */
2560                         if (num_active_ports)
2561                                 ac |= BIT(aggr_idx[i % num_active_ports]);
2562                         ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
2563                 }
2564
2565                 /* Mark all ports in the same LAG as visited to avoid applying
2566                  * the same config again.
2567                  */
2568                 for (port = lag; port < ocelot->num_phys_ports; port++) {
2569                         struct ocelot_port *ocelot_port = ocelot->ports[port];
2570
2571                         if (!ocelot_port)
2572                                 continue;
2573
2574                         if (ocelot_port->bond == bond)
2575                                 visited |= BIT(port);
2576                 }
2577         }
2578 }
2579
2580 /* When offloading a bonding interface, the switch ports configured under the
2581  * same bond must have the same logical port ID, equal to the physical port ID
2582  * of the lowest numbered physical port in that bond. Otherwise, in standalone/
2583  * bridged mode, each port has a logical port ID equal to its physical port ID.
2584  */
2585 static void ocelot_setup_logical_port_ids(struct ocelot *ocelot)
2586 {
2587         int port;
2588
2589         for (port = 0; port < ocelot->num_phys_ports; port++) {
2590                 struct ocelot_port *ocelot_port = ocelot->ports[port];
2591                 struct net_device *bond;
2592
2593                 if (!ocelot_port)
2594                         continue;
2595
2596                 bond = ocelot_port->bond;
2597                 if (bond) {
2598                         int lag = ocelot_bond_get_id(ocelot, bond);
2599
2600                         ocelot_rmw_gix(ocelot,
2601                                        ANA_PORT_PORT_CFG_PORTID_VAL(lag),
2602                                        ANA_PORT_PORT_CFG_PORTID_VAL_M,
2603                                        ANA_PORT_PORT_CFG, port);
2604                 } else {
2605                         ocelot_rmw_gix(ocelot,
2606                                        ANA_PORT_PORT_CFG_PORTID_VAL(port),
2607                                        ANA_PORT_PORT_CFG_PORTID_VAL_M,
2608                                        ANA_PORT_PORT_CFG, port);
2609                 }
2610         }
2611 }
2612
2613 /* Documentation for PORTID_VAL says:
2614  *     Logical port number for front port. If port is not a member of a LLAG,
2615  *     then PORTID must be set to the physical port number.
2616  *     If port is a member of a LLAG, then PORTID must be set to the common
2617  *     PORTID_VAL used for all member ports of the LLAG.
2618  *     The value must not exceed the number of physical ports on the device.
2619  *
2620  * This means we have little choice but to migrate FDB entries pointing towards
2621  * a logical port when that changes.
2622  */
2623 static void ocelot_migrate_lag_fdbs(struct ocelot *ocelot,
2624                                     struct net_device *bond,
2625                                     int lag)
2626 {
2627         struct ocelot_lag_fdb *fdb;
2628         int err;
2629
2630         lockdep_assert_held(&ocelot->fwd_domain_lock);
2631
2632         list_for_each_entry(fdb, &ocelot->lag_fdbs, list) {
2633                 if (fdb->bond != bond)
2634                         continue;
2635
2636                 err = ocelot_mact_forget(ocelot, fdb->addr, fdb->vid);
2637                 if (err) {
2638                         dev_err(ocelot->dev,
2639                                 "failed to delete LAG %s FDB %pM vid %d: %pe\n",
2640                                 bond->name, fdb->addr, fdb->vid, ERR_PTR(err));
2641                 }
2642
2643                 err = ocelot_mact_learn(ocelot, lag, fdb->addr, fdb->vid,
2644                                         ENTRYTYPE_LOCKED);
2645                 if (err) {
2646                         dev_err(ocelot->dev,
2647                                 "failed to migrate LAG %s FDB %pM vid %d: %pe\n",
2648                                 bond->name, fdb->addr, fdb->vid, ERR_PTR(err));
2649                 }
2650         }
2651 }
2652
2653 int ocelot_port_lag_join(struct ocelot *ocelot, int port,
2654                          struct net_device *bond,
2655                          struct netdev_lag_upper_info *info)
2656 {
2657         if (info->tx_type != NETDEV_LAG_TX_TYPE_HASH)
2658                 return -EOPNOTSUPP;
2659
2660         mutex_lock(&ocelot->fwd_domain_lock);
2661
2662         ocelot->ports[port]->bond = bond;
2663
2664         ocelot_setup_logical_port_ids(ocelot);
2665         ocelot_apply_bridge_fwd_mask(ocelot, true);
2666         ocelot_set_aggr_pgids(ocelot);
2667
2668         mutex_unlock(&ocelot->fwd_domain_lock);
2669
2670         return 0;
2671 }
2672 EXPORT_SYMBOL(ocelot_port_lag_join);
2673
2674 void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
2675                            struct net_device *bond)
2676 {
2677         int old_lag_id, new_lag_id;
2678
2679         mutex_lock(&ocelot->fwd_domain_lock);
2680
2681         old_lag_id = ocelot_bond_get_id(ocelot, bond);
2682
2683         ocelot->ports[port]->bond = NULL;
2684
2685         ocelot_setup_logical_port_ids(ocelot);
2686         ocelot_apply_bridge_fwd_mask(ocelot, false);
2687         ocelot_set_aggr_pgids(ocelot);
2688
2689         new_lag_id = ocelot_bond_get_id(ocelot, bond);
2690
2691         if (new_lag_id >= 0 && old_lag_id != new_lag_id)
2692                 ocelot_migrate_lag_fdbs(ocelot, bond, new_lag_id);
2693
2694         mutex_unlock(&ocelot->fwd_domain_lock);
2695 }
2696 EXPORT_SYMBOL(ocelot_port_lag_leave);
2697
2698 void ocelot_port_lag_change(struct ocelot *ocelot, int port, bool lag_tx_active)
2699 {
2700         struct ocelot_port *ocelot_port = ocelot->ports[port];
2701
2702         mutex_lock(&ocelot->fwd_domain_lock);
2703
2704         ocelot_port->lag_tx_active = lag_tx_active;
2705
2706         /* Rebalance the LAGs */
2707         ocelot_set_aggr_pgids(ocelot);
2708
2709         mutex_unlock(&ocelot->fwd_domain_lock);
2710 }
2711 EXPORT_SYMBOL(ocelot_port_lag_change);
2712
2713 int ocelot_lag_fdb_add(struct ocelot *ocelot, struct net_device *bond,
2714                        const unsigned char *addr, u16 vid,
2715                        const struct net_device *bridge)
2716 {
2717         struct ocelot_lag_fdb *fdb;
2718         int lag, err;
2719
2720         fdb = kzalloc(sizeof(*fdb), GFP_KERNEL);
2721         if (!fdb)
2722                 return -ENOMEM;
2723
2724         mutex_lock(&ocelot->fwd_domain_lock);
2725
2726         if (!vid)
2727                 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2728
2729         ether_addr_copy(fdb->addr, addr);
2730         fdb->vid = vid;
2731         fdb->bond = bond;
2732
2733         lag = ocelot_bond_get_id(ocelot, bond);
2734
2735         err = ocelot_mact_learn(ocelot, lag, addr, vid, ENTRYTYPE_LOCKED);
2736         if (err) {
2737                 mutex_unlock(&ocelot->fwd_domain_lock);
2738                 kfree(fdb);
2739                 return err;
2740         }
2741
2742         list_add_tail(&fdb->list, &ocelot->lag_fdbs);
2743         mutex_unlock(&ocelot->fwd_domain_lock);
2744
2745         return 0;
2746 }
2747 EXPORT_SYMBOL_GPL(ocelot_lag_fdb_add);
2748
2749 int ocelot_lag_fdb_del(struct ocelot *ocelot, struct net_device *bond,
2750                        const unsigned char *addr, u16 vid,
2751                        const struct net_device *bridge)
2752 {
2753         struct ocelot_lag_fdb *fdb, *tmp;
2754
2755         mutex_lock(&ocelot->fwd_domain_lock);
2756
2757         if (!vid)
2758                 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2759
2760         list_for_each_entry_safe(fdb, tmp, &ocelot->lag_fdbs, list) {
2761                 if (!ether_addr_equal(fdb->addr, addr) || fdb->vid != vid ||
2762                     fdb->bond != bond)
2763                         continue;
2764
2765                 ocelot_mact_forget(ocelot, addr, vid);
2766                 list_del(&fdb->list);
2767                 mutex_unlock(&ocelot->fwd_domain_lock);
2768                 kfree(fdb);
2769
2770                 return 0;
2771         }
2772
2773         mutex_unlock(&ocelot->fwd_domain_lock);
2774
2775         return -ENOENT;
2776 }
2777 EXPORT_SYMBOL_GPL(ocelot_lag_fdb_del);
2778
2779 /* Configure the maximum SDU (L2 payload) on RX to the value specified in @sdu.
2780  * The length of VLAN tags is accounted for automatically via DEV_MAC_TAGS_CFG.
2781  * In the special case that it's the NPI port that we're configuring, the
2782  * length of the tag and optional prefix needs to be accounted for privately,
2783  * in order to be able to sustain communication at the requested @sdu.
2784  */
2785 void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
2786 {
2787         struct ocelot_port *ocelot_port = ocelot->ports[port];
2788         int maxlen = sdu + ETH_HLEN + ETH_FCS_LEN;
2789         int pause_start, pause_stop;
2790         int atop, atop_tot;
2791
2792         if (port == ocelot->npi) {
2793                 maxlen += OCELOT_TAG_LEN;
2794
2795                 if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_SHORT)
2796                         maxlen += OCELOT_SHORT_PREFIX_LEN;
2797                 else if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_LONG)
2798                         maxlen += OCELOT_LONG_PREFIX_LEN;
2799         }
2800
2801         ocelot_port_writel(ocelot_port, maxlen, DEV_MAC_MAXLEN_CFG);
2802
2803         /* Set Pause watermark hysteresis */
2804         pause_start = 6 * maxlen / OCELOT_BUFFER_CELL_SZ;
2805         pause_stop = 4 * maxlen / OCELOT_BUFFER_CELL_SZ;
2806         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_START,
2807                             pause_start);
2808         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_STOP,
2809                             pause_stop);
2810
2811         /* Tail dropping watermarks */
2812         atop_tot = (ocelot->packet_buffer_size - 9 * maxlen) /
2813                    OCELOT_BUFFER_CELL_SZ;
2814         atop = (9 * maxlen) / OCELOT_BUFFER_CELL_SZ;
2815         ocelot_write_rix(ocelot, ocelot->ops->wm_enc(atop), SYS_ATOP, port);
2816         ocelot_write(ocelot, ocelot->ops->wm_enc(atop_tot), SYS_ATOP_TOT_CFG);
2817 }
2818 EXPORT_SYMBOL(ocelot_port_set_maxlen);
2819
2820 int ocelot_get_max_mtu(struct ocelot *ocelot, int port)
2821 {
2822         int max_mtu = 65535 - ETH_HLEN - ETH_FCS_LEN;
2823
2824         if (port == ocelot->npi) {
2825                 max_mtu -= OCELOT_TAG_LEN;
2826
2827                 if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_SHORT)
2828                         max_mtu -= OCELOT_SHORT_PREFIX_LEN;
2829                 else if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_LONG)
2830                         max_mtu -= OCELOT_LONG_PREFIX_LEN;
2831         }
2832
2833         return max_mtu;
2834 }
2835 EXPORT_SYMBOL(ocelot_get_max_mtu);
2836
2837 static void ocelot_port_set_learning(struct ocelot *ocelot, int port,
2838                                      bool enabled)
2839 {
2840         struct ocelot_port *ocelot_port = ocelot->ports[port];
2841         u32 val = 0;
2842
2843         if (enabled)
2844                 val = ANA_PORT_PORT_CFG_LEARN_ENA;
2845
2846         ocelot_rmw_gix(ocelot, val, ANA_PORT_PORT_CFG_LEARN_ENA,
2847                        ANA_PORT_PORT_CFG, port);
2848
2849         ocelot_port->learn_ena = enabled;
2850 }
2851
2852 static void ocelot_port_set_ucast_flood(struct ocelot *ocelot, int port,
2853                                         bool enabled)
2854 {
2855         u32 val = 0;
2856
2857         if (enabled)
2858                 val = BIT(port);
2859
2860         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_UC);
2861 }
2862
2863 static void ocelot_port_set_mcast_flood(struct ocelot *ocelot, int port,
2864                                         bool enabled)
2865 {
2866         u32 val = 0;
2867
2868         if (enabled)
2869                 val = BIT(port);
2870
2871         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MC);
2872         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MCIPV4);
2873         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MCIPV6);
2874 }
2875
2876 static void ocelot_port_set_bcast_flood(struct ocelot *ocelot, int port,
2877                                         bool enabled)
2878 {
2879         u32 val = 0;
2880
2881         if (enabled)
2882                 val = BIT(port);
2883
2884         ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_BC);
2885 }
2886
2887 int ocelot_port_pre_bridge_flags(struct ocelot *ocelot, int port,
2888                                  struct switchdev_brport_flags flags)
2889 {
2890         if (flags.mask & ~(BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD |
2891                            BR_BCAST_FLOOD))
2892                 return -EINVAL;
2893
2894         return 0;
2895 }
2896 EXPORT_SYMBOL(ocelot_port_pre_bridge_flags);
2897
2898 void ocelot_port_bridge_flags(struct ocelot *ocelot, int port,
2899                               struct switchdev_brport_flags flags)
2900 {
2901         if (port == ocelot->npi)
2902                 port = ocelot->num_phys_ports;
2903
2904         if (flags.mask & BR_LEARNING)
2905                 ocelot_port_set_learning(ocelot, port,
2906                                          !!(flags.val & BR_LEARNING));
2907
2908         if (flags.mask & BR_FLOOD)
2909                 ocelot_port_set_ucast_flood(ocelot, port,
2910                                             !!(flags.val & BR_FLOOD));
2911
2912         if (flags.mask & BR_MCAST_FLOOD)
2913                 ocelot_port_set_mcast_flood(ocelot, port,
2914                                             !!(flags.val & BR_MCAST_FLOOD));
2915
2916         if (flags.mask & BR_BCAST_FLOOD)
2917                 ocelot_port_set_bcast_flood(ocelot, port,
2918                                             !!(flags.val & BR_BCAST_FLOOD));
2919 }
2920 EXPORT_SYMBOL(ocelot_port_bridge_flags);
2921
2922 int ocelot_port_get_default_prio(struct ocelot *ocelot, int port)
2923 {
2924         int val = ocelot_read_gix(ocelot, ANA_PORT_QOS_CFG, port);
2925
2926         return ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL_X(val);
2927 }
2928 EXPORT_SYMBOL_GPL(ocelot_port_get_default_prio);
2929
2930 int ocelot_port_set_default_prio(struct ocelot *ocelot, int port, u8 prio)
2931 {
2932         if (prio >= OCELOT_NUM_TC)
2933                 return -ERANGE;
2934
2935         ocelot_rmw_gix(ocelot,
2936                        ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL(prio),
2937                        ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL_M,
2938                        ANA_PORT_QOS_CFG,
2939                        port);
2940
2941         return 0;
2942 }
2943 EXPORT_SYMBOL_GPL(ocelot_port_set_default_prio);
2944
2945 int ocelot_port_get_dscp_prio(struct ocelot *ocelot, int port, u8 dscp)
2946 {
2947         int qos_cfg = ocelot_read_gix(ocelot, ANA_PORT_QOS_CFG, port);
2948         int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp);
2949
2950         /* Return error if DSCP prioritization isn't enabled */
2951         if (!(qos_cfg & ANA_PORT_QOS_CFG_QOS_DSCP_ENA))
2952                 return -EOPNOTSUPP;
2953
2954         if (qos_cfg & ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA) {
2955                 dscp = ANA_DSCP_CFG_DSCP_TRANSLATE_VAL_X(dscp_cfg);
2956                 /* Re-read ANA_DSCP_CFG for the translated DSCP */
2957                 dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp);
2958         }
2959
2960         /* If the DSCP value is not trusted, the QoS classification falls back
2961          * to VLAN PCP or port-based default.
2962          */
2963         if (!(dscp_cfg & ANA_DSCP_CFG_DSCP_TRUST_ENA))
2964                 return -EOPNOTSUPP;
2965
2966         return ANA_DSCP_CFG_QOS_DSCP_VAL_X(dscp_cfg);
2967 }
2968 EXPORT_SYMBOL_GPL(ocelot_port_get_dscp_prio);
2969
2970 int ocelot_port_add_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio)
2971 {
2972         int mask, val;
2973
2974         if (prio >= OCELOT_NUM_TC)
2975                 return -ERANGE;
2976
2977         /* There is at least one app table priority (this one), so we need to
2978          * make sure DSCP prioritization is enabled on the port.
2979          * Also make sure DSCP translation is disabled
2980          * (dcbnl doesn't support it).
2981          */
2982         mask = ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
2983                ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA;
2984
2985         ocelot_rmw_gix(ocelot, ANA_PORT_QOS_CFG_QOS_DSCP_ENA, mask,
2986                        ANA_PORT_QOS_CFG, port);
2987
2988         /* Trust this DSCP value and map it to the given QoS class */
2989         val = ANA_DSCP_CFG_DSCP_TRUST_ENA | ANA_DSCP_CFG_QOS_DSCP_VAL(prio);
2990
2991         ocelot_write_rix(ocelot, val, ANA_DSCP_CFG, dscp);
2992
2993         return 0;
2994 }
2995 EXPORT_SYMBOL_GPL(ocelot_port_add_dscp_prio);
2996
2997 int ocelot_port_del_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio)
2998 {
2999         int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp);
3000         int mask, i;
3001
3002         /* During a "dcb app replace" command, the new app table entry will be
3003          * added first, then the old one will be deleted. But the hardware only
3004          * supports one QoS class per DSCP value (duh), so if we blindly delete
3005          * the app table entry for this DSCP value, we end up deleting the
3006          * entry with the new priority. Avoid that by checking whether user
3007          * space wants to delete the priority which is currently configured, or
3008          * something else which is no longer current.
3009          */
3010         if (ANA_DSCP_CFG_QOS_DSCP_VAL_X(dscp_cfg) != prio)
3011                 return 0;
3012
3013         /* Untrust this DSCP value */
3014         ocelot_write_rix(ocelot, 0, ANA_DSCP_CFG, dscp);
3015
3016         for (i = 0; i < 64; i++) {
3017                 int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, i);
3018
3019                 /* There are still app table entries on the port, so we need to
3020                  * keep DSCP enabled, nothing to do.
3021                  */
3022                 if (dscp_cfg & ANA_DSCP_CFG_DSCP_TRUST_ENA)
3023                         return 0;
3024         }
3025
3026         /* Disable DSCP QoS classification if there isn't any trusted
3027          * DSCP value left.
3028          */
3029         mask = ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
3030                ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA;
3031
3032         ocelot_rmw_gix(ocelot, 0, mask, ANA_PORT_QOS_CFG, port);
3033
3034         return 0;
3035 }
3036 EXPORT_SYMBOL_GPL(ocelot_port_del_dscp_prio);
3037
3038 struct ocelot_mirror *ocelot_mirror_get(struct ocelot *ocelot, int to,
3039                                         struct netlink_ext_ack *extack)
3040 {
3041         struct ocelot_mirror *m = ocelot->mirror;
3042
3043         if (m) {
3044                 if (m->to != to) {
3045                         NL_SET_ERR_MSG_MOD(extack,
3046                                            "Mirroring already configured towards different egress port");
3047                         return ERR_PTR(-EBUSY);
3048                 }
3049
3050                 refcount_inc(&m->refcount);
3051                 return m;
3052         }
3053
3054         m = kzalloc(sizeof(*m), GFP_KERNEL);
3055         if (!m)
3056                 return ERR_PTR(-ENOMEM);
3057
3058         m->to = to;
3059         refcount_set(&m->refcount, 1);
3060         ocelot->mirror = m;
3061
3062         /* Program the mirror port to hardware */
3063         ocelot_write(ocelot, BIT(to), ANA_MIRRORPORTS);
3064
3065         return m;
3066 }
3067
3068 void ocelot_mirror_put(struct ocelot *ocelot)
3069 {
3070         struct ocelot_mirror *m = ocelot->mirror;
3071
3072         if (!refcount_dec_and_test(&m->refcount))
3073                 return;
3074
3075         ocelot_write(ocelot, 0, ANA_MIRRORPORTS);
3076         ocelot->mirror = NULL;
3077         kfree(m);
3078 }
3079
3080 int ocelot_port_mirror_add(struct ocelot *ocelot, int from, int to,
3081                            bool ingress, struct netlink_ext_ack *extack)
3082 {
3083         struct ocelot_mirror *m = ocelot_mirror_get(ocelot, to, extack);
3084
3085         if (IS_ERR(m))
3086                 return PTR_ERR(m);
3087
3088         if (ingress) {
3089                 ocelot_rmw_gix(ocelot, ANA_PORT_PORT_CFG_SRC_MIRROR_ENA,
3090                                ANA_PORT_PORT_CFG_SRC_MIRROR_ENA,
3091                                ANA_PORT_PORT_CFG, from);
3092         } else {
3093                 ocelot_rmw(ocelot, BIT(from), BIT(from),
3094                            ANA_EMIRRORPORTS);
3095         }
3096
3097         return 0;
3098 }
3099 EXPORT_SYMBOL_GPL(ocelot_port_mirror_add);
3100
3101 void ocelot_port_mirror_del(struct ocelot *ocelot, int from, bool ingress)
3102 {
3103         if (ingress) {
3104                 ocelot_rmw_gix(ocelot, 0, ANA_PORT_PORT_CFG_SRC_MIRROR_ENA,
3105                                ANA_PORT_PORT_CFG, from);
3106         } else {
3107                 ocelot_rmw(ocelot, 0, BIT(from), ANA_EMIRRORPORTS);
3108         }
3109
3110         ocelot_mirror_put(ocelot);
3111 }
3112 EXPORT_SYMBOL_GPL(ocelot_port_mirror_del);
3113
3114 void ocelot_init_port(struct ocelot *ocelot, int port)
3115 {
3116         struct ocelot_port *ocelot_port = ocelot->ports[port];
3117
3118         skb_queue_head_init(&ocelot_port->tx_skbs);
3119
3120         /* Basic L2 initialization */
3121
3122         /* Set MAC IFG Gaps
3123          * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
3124          * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
3125          */
3126         ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
3127                            DEV_MAC_IFG_CFG);
3128
3129         /* Load seed (0) and set MAC HDX late collision  */
3130         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
3131                            DEV_MAC_HDX_CFG_SEED_LOAD,
3132                            DEV_MAC_HDX_CFG);
3133         mdelay(1);
3134         ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
3135                            DEV_MAC_HDX_CFG);
3136
3137         /* Set Max Length and maximum tags allowed */
3138         ocelot_port_set_maxlen(ocelot, port, ETH_DATA_LEN);
3139         ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
3140                            DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
3141                            DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA |
3142                            DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
3143                            DEV_MAC_TAGS_CFG);
3144
3145         /* Set SMAC of Pause frame (00:00:00:00:00:00) */
3146         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
3147         ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
3148
3149         /* Enable transmission of pause frames */
3150         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
3151
3152         /* Drop frames with multicast source address */
3153         ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
3154                        ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
3155                        ANA_PORT_DROP_CFG, port);
3156
3157         /* Set default VLAN and tag type to 8021Q. */
3158         ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
3159                        REW_PORT_VLAN_CFG_PORT_TPID_M,
3160                        REW_PORT_VLAN_CFG, port);
3161
3162         /* Disable source address learning for standalone mode */
3163         ocelot_port_set_learning(ocelot, port, false);
3164
3165         /* Set the port's initial logical port ID value, enable receiving
3166          * frames on it, and configure the MAC address learning type to
3167          * automatic.
3168          */
3169         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
3170                          ANA_PORT_PORT_CFG_RECV_ENA |
3171                          ANA_PORT_PORT_CFG_PORTID_VAL(port),
3172                          ANA_PORT_PORT_CFG, port);
3173
3174         /* Enable vcap lookups */
3175         ocelot_vcap_enable(ocelot, port);
3176 }
3177 EXPORT_SYMBOL(ocelot_init_port);
3178
3179 /* Configure and enable the CPU port module, which is a set of queues
3180  * accessible through register MMIO, frame DMA or Ethernet (in case
3181  * NPI mode is used).
3182  */
3183 static void ocelot_cpu_port_init(struct ocelot *ocelot)
3184 {
3185         int cpu = ocelot->num_phys_ports;
3186
3187         /* The unicast destination PGID for the CPU port module is unused */
3188         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
3189         /* Instead set up a multicast destination PGID for traffic copied to
3190          * the CPU. Whitelisted MAC addresses like the port netdevice MAC
3191          * addresses will be copied to the CPU via this PGID.
3192          */
3193         ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
3194         ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
3195                          ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
3196                          ANA_PORT_PORT_CFG, cpu);
3197
3198         /* Enable CPU port module */
3199         ocelot_fields_write(ocelot, cpu, QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
3200         /* CPU port Injection/Extraction configuration */
3201         ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_XTR_HDR,
3202                             OCELOT_TAG_PREFIX_NONE);
3203         ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_INJ_HDR,
3204                             OCELOT_TAG_PREFIX_NONE);
3205
3206         /* Configure the CPU port to be VLAN aware */
3207         ocelot_write_gix(ocelot,
3208                          ANA_PORT_VLAN_CFG_VLAN_VID(OCELOT_STANDALONE_PVID) |
3209                          ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
3210                          ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
3211                          ANA_PORT_VLAN_CFG, cpu);
3212 }
3213
3214 static void ocelot_detect_features(struct ocelot *ocelot)
3215 {
3216         int mmgt, eq_ctrl;
3217
3218         /* For Ocelot, Felix, Seville, Serval etc, SYS:MMGT:MMGT:FREECNT holds
3219          * the number of 240-byte free memory words (aka 4-cell chunks) and not
3220          * 192 bytes as the documentation incorrectly says.
3221          */
3222         mmgt = ocelot_read(ocelot, SYS_MMGT);
3223         ocelot->packet_buffer_size = 240 * SYS_MMGT_FREECNT(mmgt);
3224
3225         eq_ctrl = ocelot_read(ocelot, QSYS_EQ_CTRL);
3226         ocelot->num_frame_refs = QSYS_MMGT_EQ_CTRL_FP_FREE_CNT(eq_ctrl);
3227 }
3228
3229 int ocelot_init(struct ocelot *ocelot)
3230 {
3231         char queue_name[32];
3232         int i, ret;
3233         u32 port;
3234
3235         if (ocelot->ops->reset) {
3236                 ret = ocelot->ops->reset(ocelot);
3237                 if (ret) {
3238                         dev_err(ocelot->dev, "Switch reset failed\n");
3239                         return ret;
3240                 }
3241         }
3242
3243         ocelot->stats = devm_kcalloc(ocelot->dev,
3244                                      ocelot->num_phys_ports * ocelot->num_stats,
3245                                      sizeof(u64), GFP_KERNEL);
3246         if (!ocelot->stats)
3247                 return -ENOMEM;
3248
3249         mutex_init(&ocelot->stats_lock);
3250         mutex_init(&ocelot->ptp_lock);
3251         mutex_init(&ocelot->mact_lock);
3252         mutex_init(&ocelot->fwd_domain_lock);
3253         spin_lock_init(&ocelot->ptp_clock_lock);
3254         spin_lock_init(&ocelot->ts_id_lock);
3255         snprintf(queue_name, sizeof(queue_name), "%s-stats",
3256                  dev_name(ocelot->dev));
3257         ocelot->stats_queue = create_singlethread_workqueue(queue_name);
3258         if (!ocelot->stats_queue)
3259                 return -ENOMEM;
3260
3261         ocelot->owq = alloc_ordered_workqueue("ocelot-owq", 0);
3262         if (!ocelot->owq) {
3263                 destroy_workqueue(ocelot->stats_queue);
3264                 return -ENOMEM;
3265         }
3266
3267         INIT_LIST_HEAD(&ocelot->multicast);
3268         INIT_LIST_HEAD(&ocelot->pgids);
3269         INIT_LIST_HEAD(&ocelot->vlans);
3270         INIT_LIST_HEAD(&ocelot->lag_fdbs);
3271         ocelot_detect_features(ocelot);
3272         ocelot_mact_init(ocelot);
3273         ocelot_vlan_init(ocelot);
3274         ocelot_vcap_init(ocelot);
3275         ocelot_cpu_port_init(ocelot);
3276
3277         if (ocelot->ops->psfp_init)
3278                 ocelot->ops->psfp_init(ocelot);
3279
3280         for (port = 0; port < ocelot->num_phys_ports; port++) {
3281                 /* Clear all counters (5 groups) */
3282                 ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
3283                                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
3284                              SYS_STAT_CFG);
3285         }
3286
3287         /* Only use S-Tag */
3288         ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
3289
3290         /* Aggregation mode */
3291         ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
3292                              ANA_AGGR_CFG_AC_DMAC_ENA |
3293                              ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
3294                              ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA |
3295                              ANA_AGGR_CFG_AC_IP6_FLOW_LBL_ENA |
3296                              ANA_AGGR_CFG_AC_IP6_TCPUDP_ENA,
3297                              ANA_AGGR_CFG);
3298
3299         /* Set MAC age time to default value. The entry is aged after
3300          * 2*AGE_PERIOD
3301          */
3302         ocelot_write(ocelot,
3303                      ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
3304                      ANA_AUTOAGE);
3305
3306         /* Disable learning for frames discarded by VLAN ingress filtering */
3307         regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
3308
3309         /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
3310         ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
3311                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
3312
3313         /* Setup flooding PGIDs */
3314         for (i = 0; i < ocelot->num_flooding_pgids; i++)
3315                 ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
3316                                  ANA_FLOODING_FLD_BROADCAST(PGID_BC) |
3317                                  ANA_FLOODING_FLD_UNICAST(PGID_UC),
3318                                  ANA_FLOODING, i);
3319         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
3320                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
3321                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
3322                      ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
3323                      ANA_FLOODING_IPMC);
3324
3325         for (port = 0; port < ocelot->num_phys_ports; port++) {
3326                 /* Transmit the frame to the local port. */
3327                 ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
3328                 /* Do not forward BPDU frames to the front ports. */
3329                 ocelot_write_gix(ocelot,
3330                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
3331                                  ANA_PORT_CPU_FWD_BPDU_CFG,
3332                                  port);
3333                 /* Ensure bridging is disabled */
3334                 ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
3335         }
3336
3337         for_each_nonreserved_multicast_dest_pgid(ocelot, i) {
3338                 u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
3339
3340                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
3341         }
3342
3343         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_BLACKHOLE);
3344
3345         /* Allow broadcast and unknown L2 multicast to the CPU. */
3346         ocelot_rmw_rix(ocelot, ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3347                        ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3348                        ANA_PGID_PGID, PGID_MC);
3349         ocelot_rmw_rix(ocelot, ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3350                        ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3351                        ANA_PGID_PGID, PGID_BC);
3352         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
3353         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
3354
3355         /* Allow manual injection via DEVCPU_QS registers, and byte swap these
3356          * registers endianness.
3357          */
3358         ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
3359                          QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
3360         ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
3361                          QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
3362         ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
3363                      ANA_CPUQ_CFG_CPUQ_LRN(2) |
3364                      ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
3365                      ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
3366                      ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
3367                      ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
3368                      ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
3369                      ANA_CPUQ_CFG_CPUQ_IGMP(6) |
3370                      ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
3371         for (i = 0; i < 16; i++)
3372                 ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
3373                                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
3374                                  ANA_CPUQ_8021_CFG, i);
3375
3376         ret = ocelot_prepare_stats_regions(ocelot);
3377         if (ret) {
3378                 destroy_workqueue(ocelot->stats_queue);
3379                 destroy_workqueue(ocelot->owq);
3380                 return ret;
3381         }
3382
3383         INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
3384         queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
3385                            OCELOT_STATS_CHECK_DELAY);
3386
3387         return 0;
3388 }
3389 EXPORT_SYMBOL(ocelot_init);
3390
3391 void ocelot_deinit(struct ocelot *ocelot)
3392 {
3393         cancel_delayed_work(&ocelot->stats_work);
3394         destroy_workqueue(ocelot->stats_queue);
3395         destroy_workqueue(ocelot->owq);
3396         mutex_destroy(&ocelot->stats_lock);
3397 }
3398 EXPORT_SYMBOL(ocelot_deinit);
3399
3400 void ocelot_deinit_port(struct ocelot *ocelot, int port)
3401 {
3402         struct ocelot_port *ocelot_port = ocelot->ports[port];
3403
3404         skb_queue_purge(&ocelot_port->tx_skbs);
3405 }
3406 EXPORT_SYMBOL(ocelot_deinit_port);
3407
3408 MODULE_LICENSE("Dual MIT/GPL");