ethernet/mscc/ocelot.c

0001 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
0002 /*
0003  * Microsemi Ocelot Switch driver
0004  *
0005  * Copyright (c) 2017 Microsemi Corporation
0006  */
0007 #include <linux/dsa/ocelot.h>
0008 #include <linux/if_bridge.h>
0009 #include <linux/ptp_classify.h>
0010 #include <soc/mscc/ocelot_vcap.h>
0011 #include "ocelot.h"
0012 #include "ocelot_vcap.h"
0013
0014 #define TABLE_UPDATE_SLEEP_US 10
0015 #define TABLE_UPDATE_TIMEOUT_US 100000
0016 #define OCELOT_RSV_VLAN_RANGE_START 4000
0017
0018 struct ocelot_mact_entry {
0019     u8 mac[ETH_ALEN];
0020     u16 vid;
0021     enum macaccess_entry_type type;
0022 };
0023
0024 /* Caller must hold &ocelot->mact_lock */
0025 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
0026 {
0027     return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
0028 }
0029
0030 /* Caller must hold &ocelot->mact_lock */
0031 static inline int ocelot_mact_wait_for_completion(struct ocelot *ocelot)
0032 {
0033     u32 val;
0034
0035     return readx_poll_timeout(ocelot_mact_read_macaccess,
0036         ocelot, val,
0037         (val & ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M) ==
0038         MACACCESS_CMD_IDLE,
0039         TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
0040 }
0041
0042 /* Caller must hold &ocelot->mact_lock */
0043 static void ocelot_mact_select(struct ocelot *ocelot,
0044                    const unsigned char mac[ETH_ALEN],
0045                    unsigned int vid)
0046 {
0047     u32 macl = 0, mach = 0;
0048
0049     /* Set the MAC address to handle and the vlan associated in a format
0050      * understood by the hardware.
0051      */
0052     mach |= vid    << 16;
0053     mach |= mac[0] << 8;
0054     mach |= mac[1] << 0;
0055     macl |= mac[2] << 24;
0056     macl |= mac[3] << 16;
0057     macl |= mac[4] << 8;
0058     macl |= mac[5] << 0;
0059
0060     ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
0061     ocelot_write(ocelot, mach, ANA_TABLES_MACHDATA);
0062
0063 }
0064
0065 static int __ocelot_mact_learn(struct ocelot *ocelot, int port,
0066                    const unsigned char mac[ETH_ALEN],
0067                    unsigned int vid, enum macaccess_entry_type type)
0068 {
0069     u32 cmd = ANA_TABLES_MACACCESS_VALID |
0070         ANA_TABLES_MACACCESS_DEST_IDX(port) |
0071         ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
0072         ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN);
0073     unsigned int mc_ports;
0074     int err;
0075
0076     /* Set MAC_CPU_COPY if the CPU port is used by a multicast entry */
0077     if (type == ENTRYTYPE_MACv4)
0078         mc_ports = (mac[1] << 8) | mac[2];
0079     else if (type == ENTRYTYPE_MACv6)
0080         mc_ports = (mac[0] << 8) | mac[1];
0081     else
0082         mc_ports = 0;
0083
0084     if (mc_ports & BIT(ocelot->num_phys_ports))
0085         cmd |= ANA_TABLES_MACACCESS_MAC_CPU_COPY;
0086
0087     ocelot_mact_select(ocelot, mac, vid);
0088
0089     /* Issue a write command */
0090     ocelot_write(ocelot, cmd, ANA_TABLES_MACACCESS);
0091
0092     err = ocelot_mact_wait_for_completion(ocelot);
0093
0094     return err;
0095 }
0096
0097 int ocelot_mact_learn(struct ocelot *ocelot, int port,
0098               const unsigned char mac[ETH_ALEN],
0099               unsigned int vid, enum macaccess_entry_type type)
0100 {
0101     int ret;
0102
0103     mutex_lock(&ocelot->mact_lock);
0104     ret = __ocelot_mact_learn(ocelot, port, mac, vid, type);
0105     mutex_unlock(&ocelot->mact_lock);
0106
0107     return ret;
0108 }
0109 EXPORT_SYMBOL(ocelot_mact_learn);
0110
0111 int ocelot_mact_forget(struct ocelot *ocelot,
0112                const unsigned char mac[ETH_ALEN], unsigned int vid)
0113 {
0114     int err;
0115
0116     mutex_lock(&ocelot->mact_lock);
0117
0118     ocelot_mact_select(ocelot, mac, vid);
0119
0120     /* Issue a forget command */
0121     ocelot_write(ocelot,
0122              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
0123              ANA_TABLES_MACACCESS);
0124
0125     err = ocelot_mact_wait_for_completion(ocelot);
0126
0127     mutex_unlock(&ocelot->mact_lock);
0128
0129     return err;
0130 }
0131 EXPORT_SYMBOL(ocelot_mact_forget);
0132
0133 int ocelot_mact_lookup(struct ocelot *ocelot, int *dst_idx,
0134                const unsigned char mac[ETH_ALEN],
0135                unsigned int vid, enum macaccess_entry_type *type)
0136 {
0137     int val;
0138
0139     mutex_lock(&ocelot->mact_lock);
0140
0141     ocelot_mact_select(ocelot, mac, vid);
0142
0143     /* Issue a read command with MACACCESS_VALID=1. */
0144     ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
0145              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
0146              ANA_TABLES_MACACCESS);
0147
0148     if (ocelot_mact_wait_for_completion(ocelot)) {
0149         mutex_unlock(&ocelot->mact_lock);
0150         return -ETIMEDOUT;
0151     }
0152
0153     /* Read back the entry flags */
0154     val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
0155
0156     mutex_unlock(&ocelot->mact_lock);
0157
0158     if (!(val & ANA_TABLES_MACACCESS_VALID))
0159         return -ENOENT;
0160
0161     *dst_idx = ANA_TABLES_MACACCESS_DEST_IDX_X(val);
0162     *type = ANA_TABLES_MACACCESS_ENTRYTYPE_X(val);
0163
0164     return 0;
0165 }
0166 EXPORT_SYMBOL(ocelot_mact_lookup);
0167
0168 int ocelot_mact_learn_streamdata(struct ocelot *ocelot, int dst_idx,
0169                  const unsigned char mac[ETH_ALEN],
0170                  unsigned int vid,
0171                  enum macaccess_entry_type type,
0172                  int sfid, int ssid)
0173 {
0174     int ret;
0175
0176     mutex_lock(&ocelot->mact_lock);
0177
0178     ocelot_write(ocelot,
0179              (sfid < 0 ? 0 : ANA_TABLES_STREAMDATA_SFID_VALID) |
0180              ANA_TABLES_STREAMDATA_SFID(sfid) |
0181              (ssid < 0 ? 0 : ANA_TABLES_STREAMDATA_SSID_VALID) |
0182              ANA_TABLES_STREAMDATA_SSID(ssid),
0183              ANA_TABLES_STREAMDATA);
0184
0185     ret = __ocelot_mact_learn(ocelot, dst_idx, mac, vid, type);
0186
0187     mutex_unlock(&ocelot->mact_lock);
0188
0189     return ret;
0190 }
0191 EXPORT_SYMBOL(ocelot_mact_learn_streamdata);
0192
0193 static void ocelot_mact_init(struct ocelot *ocelot)
0194 {
0195     /* Configure the learning mode entries attributes:
0196      * - Do not copy the frame to the CPU extraction queues.
0197      * - Use the vlan and mac_cpoy for dmac lookup.
0198      */
0199     ocelot_rmw(ocelot, 0,
0200            ANA_AGENCTRL_LEARN_CPU_COPY | ANA_AGENCTRL_IGNORE_DMAC_FLAGS
0201            | ANA_AGENCTRL_LEARN_FWD_KILL
0202            | ANA_AGENCTRL_LEARN_IGNORE_VLAN,
0203            ANA_AGENCTRL);
0204
0205     /* Clear the MAC table. We are not concurrent with anyone, so
0206      * holding &ocelot->mact_lock is pointless.
0207      */
0208     ocelot_write(ocelot, MACACCESS_CMD_INIT, ANA_TABLES_MACACCESS);
0209 }
0210
0211 static void ocelot_vcap_enable(struct ocelot *ocelot, int port)
0212 {
0213     ocelot_write_gix(ocelot, ANA_PORT_VCAP_S2_CFG_S2_ENA |
0214              ANA_PORT_VCAP_S2_CFG_S2_IP6_CFG(0xa),
0215              ANA_PORT_VCAP_S2_CFG, port);
0216
0217     ocelot_write_gix(ocelot, ANA_PORT_VCAP_CFG_S1_ENA,
0218              ANA_PORT_VCAP_CFG, port);
0219
0220     ocelot_rmw_gix(ocelot, REW_PORT_CFG_ES0_EN,
0221                REW_PORT_CFG_ES0_EN,
0222                REW_PORT_CFG, port);
0223 }
0224
0225 static int ocelot_single_vlan_aware_bridge(struct ocelot *ocelot,
0226                        struct netlink_ext_ack *extack)
0227 {
0228     struct net_device *bridge = NULL;
0229     int port;
0230
0231     for (port = 0; port < ocelot->num_phys_ports; port++) {
0232         struct ocelot_port *ocelot_port = ocelot->ports[port];
0233
0234         if (!ocelot_port || !ocelot_port->bridge ||
0235             !br_vlan_enabled(ocelot_port->bridge))
0236             continue;
0237
0238         if (!bridge) {
0239             bridge = ocelot_port->bridge;
0240             continue;
0241         }
0242
0243         if (bridge == ocelot_port->bridge)
0244             continue;
0245
0246         NL_SET_ERR_MSG_MOD(extack,
0247                    "Only one VLAN-aware bridge is supported");
0248         return -EBUSY;
0249     }
0250
0251     return 0;
0252 }
0253
0254 static inline u32 ocelot_vlant_read_vlanaccess(struct ocelot *ocelot)
0255 {
0256     return ocelot_read(ocelot, ANA_TABLES_VLANACCESS);
0257 }
0258
0259 static inline int ocelot_vlant_wait_for_completion(struct ocelot *ocelot)
0260 {
0261     u32 val;
0262
0263     return readx_poll_timeout(ocelot_vlant_read_vlanaccess,
0264         ocelot,
0265         val,
0266         (val & ANA_TABLES_VLANACCESS_VLAN_TBL_CMD_M) ==
0267         ANA_TABLES_VLANACCESS_CMD_IDLE,
0268         TABLE_UPDATE_SLEEP_US, TABLE_UPDATE_TIMEOUT_US);
0269 }
0270
0271 static int ocelot_vlant_set_mask(struct ocelot *ocelot, u16 vid, u32 mask)
0272 {
0273     /* Select the VID to configure */
0274     ocelot_write(ocelot, ANA_TABLES_VLANTIDX_V_INDEX(vid),
0275              ANA_TABLES_VLANTIDX);
0276     /* Set the vlan port members mask and issue a write command */
0277     ocelot_write(ocelot, ANA_TABLES_VLANACCESS_VLAN_PORT_MASK(mask) |
0278                  ANA_TABLES_VLANACCESS_CMD_WRITE,
0279              ANA_TABLES_VLANACCESS);
0280
0281     return ocelot_vlant_wait_for_completion(ocelot);
0282 }
0283
0284 static int ocelot_port_num_untagged_vlans(struct ocelot *ocelot, int port)
0285 {
0286     struct ocelot_bridge_vlan *vlan;
0287     int num_untagged = 0;
0288
0289     list_for_each_entry(vlan, &ocelot->vlans, list) {
0290         if (!(vlan->portmask & BIT(port)))
0291             continue;
0292
0293         /* Ignore the VLAN added by ocelot_add_vlan_unaware_pvid(),
0294          * because this is never active in hardware at the same time as
0295          * the bridge VLANs, which only matter in VLAN-aware mode.
0296          */
0297         if (vlan->vid >= OCELOT_RSV_VLAN_RANGE_START)
0298             continue;
0299
0300         if (vlan->untagged & BIT(port))
0301             num_untagged++;
0302     }
0303
0304     return num_untagged;
0305 }
0306
0307 static int ocelot_port_num_tagged_vlans(struct ocelot *ocelot, int port)
0308 {
0309     struct ocelot_bridge_vlan *vlan;
0310     int num_tagged = 0;
0311
0312     list_for_each_entry(vlan, &ocelot->vlans, list) {
0313         if (!(vlan->portmask & BIT(port)))
0314             continue;
0315
0316         if (!(vlan->untagged & BIT(port)))
0317             num_tagged++;
0318     }
0319
0320     return num_tagged;
0321 }
0322
0323 /* We use native VLAN when we have to mix egress-tagged VLANs with exactly
0324  * _one_ egress-untagged VLAN (_the_ native VLAN)
0325  */
0326 static bool ocelot_port_uses_native_vlan(struct ocelot *ocelot, int port)
0327 {
0328     return ocelot_port_num_tagged_vlans(ocelot, port) &&
0329            ocelot_port_num_untagged_vlans(ocelot, port) == 1;
0330 }
0331
0332 static struct ocelot_bridge_vlan *
0333 ocelot_port_find_native_vlan(struct ocelot *ocelot, int port)
0334 {
0335     struct ocelot_bridge_vlan *vlan;
0336
0337     list_for_each_entry(vlan, &ocelot->vlans, list)
0338         if (vlan->portmask & BIT(port) && vlan->untagged & BIT(port))
0339             return vlan;
0340
0341     return NULL;
0342 }
0343
0344 /* Keep in sync REW_TAG_CFG_TAG_CFG and, if applicable,
0345  * REW_PORT_VLAN_CFG_PORT_VID, with the bridge VLAN table and VLAN awareness
0346  * state of the port.
0347  */
0348 static void ocelot_port_manage_port_tag(struct ocelot *ocelot, int port)
0349 {
0350     struct ocelot_port *ocelot_port = ocelot->ports[port];
0351     enum ocelot_port_tag_config tag_cfg;
0352     bool uses_native_vlan = false;
0353
0354     if (ocelot_port->vlan_aware) {
0355         uses_native_vlan = ocelot_port_uses_native_vlan(ocelot, port);
0356
0357         if (uses_native_vlan)
0358             tag_cfg = OCELOT_PORT_TAG_NATIVE;
0359         else if (ocelot_port_num_untagged_vlans(ocelot, port))
0360             tag_cfg = OCELOT_PORT_TAG_DISABLED;
0361         else
0362             tag_cfg = OCELOT_PORT_TAG_TRUNK;
0363     } else {
0364         tag_cfg = OCELOT_PORT_TAG_DISABLED;
0365     }
0366
0367     ocelot_rmw_gix(ocelot, REW_TAG_CFG_TAG_CFG(tag_cfg),
0368                REW_TAG_CFG_TAG_CFG_M,
0369                REW_TAG_CFG, port);
0370
0371     if (uses_native_vlan) {
0372         struct ocelot_bridge_vlan *native_vlan;
0373
0374         /* Not having a native VLAN is impossible, because
0375          * ocelot_port_num_untagged_vlans has returned 1.
0376          * So there is no use in checking for NULL here.
0377          */
0378         native_vlan = ocelot_port_find_native_vlan(ocelot, port);
0379
0380         ocelot_rmw_gix(ocelot,
0381                    REW_PORT_VLAN_CFG_PORT_VID(native_vlan->vid),
0382                    REW_PORT_VLAN_CFG_PORT_VID_M,
0383                    REW_PORT_VLAN_CFG, port);
0384     }
0385 }
0386
0387 int ocelot_bridge_num_find(struct ocelot *ocelot,
0388                const struct net_device *bridge)
0389 {
0390     int port;
0391
0392     for (port = 0; port < ocelot->num_phys_ports; port++) {
0393         struct ocelot_port *ocelot_port = ocelot->ports[port];
0394
0395         if (ocelot_port && ocelot_port->bridge == bridge)
0396             return ocelot_port->bridge_num;
0397     }
0398
0399     return -1;
0400 }
0401 EXPORT_SYMBOL_GPL(ocelot_bridge_num_find);
0402
0403 static u16 ocelot_vlan_unaware_pvid(struct ocelot *ocelot,
0404                     const struct net_device *bridge)
0405 {
0406     int bridge_num;
0407
0408     /* Standalone ports use VID 0 */
0409     if (!bridge)
0410         return 0;
0411
0412     bridge_num = ocelot_bridge_num_find(ocelot, bridge);
0413     if (WARN_ON(bridge_num < 0))
0414         return 0;
0415
0416     /* VLAN-unaware bridges use a reserved VID going from 4095 downwards */
0417     return VLAN_N_VID - bridge_num - 1;
0418 }
0419
0420 /* Default vlan to clasify for untagged frames (may be zero) */
0421 static void ocelot_port_set_pvid(struct ocelot *ocelot, int port,
0422                  const struct ocelot_bridge_vlan *pvid_vlan)
0423 {
0424     struct ocelot_port *ocelot_port = ocelot->ports[port];
0425     u16 pvid = ocelot_vlan_unaware_pvid(ocelot, ocelot_port->bridge);
0426     u32 val = 0;
0427
0428     ocelot_port->pvid_vlan = pvid_vlan;
0429
0430     if (ocelot_port->vlan_aware && pvid_vlan)
0431         pvid = pvid_vlan->vid;
0432
0433     ocelot_rmw_gix(ocelot,
0434                ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
0435                ANA_PORT_VLAN_CFG_VLAN_VID_M,
0436                ANA_PORT_VLAN_CFG, port);
0437
0438     /* If there's no pvid, we should drop not only untagged traffic (which
0439      * happens automatically), but also 802.1p traffic which gets
0440      * classified to VLAN 0, but that is always in our RX filter, so it
0441      * would get accepted were it not for this setting.
0442      */
0443     if (!pvid_vlan && ocelot_port->vlan_aware)
0444         val = ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
0445               ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA;
0446
0447     ocelot_rmw_gix(ocelot, val,
0448                ANA_PORT_DROP_CFG_DROP_PRIO_S_TAGGED_ENA |
0449                ANA_PORT_DROP_CFG_DROP_PRIO_C_TAGGED_ENA,
0450                ANA_PORT_DROP_CFG, port);
0451 }
0452
0453 static struct ocelot_bridge_vlan *ocelot_bridge_vlan_find(struct ocelot *ocelot,
0454                               u16 vid)
0455 {
0456     struct ocelot_bridge_vlan *vlan;
0457
0458     list_for_each_entry(vlan, &ocelot->vlans, list)
0459         if (vlan->vid == vid)
0460             return vlan;
0461
0462     return NULL;
0463 }
0464
0465 static int ocelot_vlan_member_add(struct ocelot *ocelot, int port, u16 vid,
0466                   bool untagged)
0467 {
0468     struct ocelot_bridge_vlan *vlan = ocelot_bridge_vlan_find(ocelot, vid);
0469     unsigned long portmask;
0470     int err;
0471
0472     if (vlan) {
0473         portmask = vlan->portmask | BIT(port);
0474
0475         err = ocelot_vlant_set_mask(ocelot, vid, portmask);
0476         if (err)
0477             return err;
0478
0479         vlan->portmask = portmask;
0480         /* Bridge VLANs can be overwritten with a different
0481          * egress-tagging setting, so make sure to override an untagged
0482          * with a tagged VID if that's going on.
0483          */
0484         if (untagged)
0485             vlan->untagged |= BIT(port);
0486         else
0487             vlan->untagged &= ~BIT(port);
0488
0489         return 0;
0490     }
0491
0492     vlan = kzalloc(sizeof(*vlan), GFP_KERNEL);
0493     if (!vlan)
0494         return -ENOMEM;
0495
0496     portmask = BIT(port);
0497
0498     err = ocelot_vlant_set_mask(ocelot, vid, portmask);
0499     if (err) {
0500         kfree(vlan);
0501         return err;
0502     }
0503
0504     vlan->vid = vid;
0505     vlan->portmask = portmask;
0506     if (untagged)
0507         vlan->untagged = BIT(port);
0508     INIT_LIST_HEAD(&vlan->list);
0509     list_add_tail(&vlan->list, &ocelot->vlans);
0510
0511     return 0;
0512 }
0513
0514 static int ocelot_vlan_member_del(struct ocelot *ocelot, int port, u16 vid)
0515 {
0516     struct ocelot_bridge_vlan *vlan = ocelot_bridge_vlan_find(ocelot, vid);
0517     unsigned long portmask;
0518     int err;
0519
0520     if (!vlan)
0521         return 0;
0522
0523     portmask = vlan->portmask & ~BIT(port);
0524
0525     err = ocelot_vlant_set_mask(ocelot, vid, portmask);
0526     if (err)
0527         return err;
0528
0529     vlan->portmask = portmask;
0530     if (vlan->portmask)
0531         return 0;
0532
0533     list_del(&vlan->list);
0534     kfree(vlan);
0535
0536     return 0;
0537 }
0538
0539 static int ocelot_add_vlan_unaware_pvid(struct ocelot *ocelot, int port,
0540                     const struct net_device *bridge)
0541 {
0542     u16 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
0543
0544     return ocelot_vlan_member_add(ocelot, port, vid, true);
0545 }
0546
0547 static int ocelot_del_vlan_unaware_pvid(struct ocelot *ocelot, int port,
0548                     const struct net_device *bridge)
0549 {
0550     u16 vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
0551
0552     return ocelot_vlan_member_del(ocelot, port, vid);
0553 }
0554
0555 int ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
0556                    bool vlan_aware, struct netlink_ext_ack *extack)
0557 {
0558     struct ocelot_vcap_block *block = &ocelot->block[VCAP_IS1];
0559     struct ocelot_port *ocelot_port = ocelot->ports[port];
0560     struct ocelot_vcap_filter *filter;
0561     int err = 0;
0562     u32 val;
0563
0564     list_for_each_entry(filter, &block->rules, list) {
0565         if (filter->ingress_port_mask & BIT(port) &&
0566             filter->action.vid_replace_ena) {
0567             NL_SET_ERR_MSG_MOD(extack,
0568                        "Cannot change VLAN state with vlan modify rules active");
0569             return -EBUSY;
0570         }
0571     }
0572
0573     err = ocelot_single_vlan_aware_bridge(ocelot, extack);
0574     if (err)
0575         return err;
0576
0577     if (vlan_aware)
0578         err = ocelot_del_vlan_unaware_pvid(ocelot, port,
0579                            ocelot_port->bridge);
0580     else if (ocelot_port->bridge)
0581         err = ocelot_add_vlan_unaware_pvid(ocelot, port,
0582                            ocelot_port->bridge);
0583     if (err)
0584         return err;
0585
0586     ocelot_port->vlan_aware = vlan_aware;
0587
0588     if (vlan_aware)
0589         val = ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
0590               ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
0591     else
0592         val = 0;
0593     ocelot_rmw_gix(ocelot, val,
0594                ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
0595                ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
0596                ANA_PORT_VLAN_CFG, port);
0597
0598     ocelot_port_set_pvid(ocelot, port, ocelot_port->pvid_vlan);
0599     ocelot_port_manage_port_tag(ocelot, port);
0600
0601     return 0;
0602 }
0603 EXPORT_SYMBOL(ocelot_port_vlan_filtering);
0604
0605 int ocelot_vlan_prepare(struct ocelot *ocelot, int port, u16 vid, bool pvid,
0606             bool untagged, struct netlink_ext_ack *extack)
0607 {
0608     if (untagged) {
0609         /* We are adding an egress-tagged VLAN */
0610         if (ocelot_port_uses_native_vlan(ocelot, port)) {
0611             NL_SET_ERR_MSG_MOD(extack,
0612                        "Port with egress-tagged VLANs cannot have more than one egress-untagged (native) VLAN");
0613             return -EBUSY;
0614         }
0615     } else {
0616         /* We are adding an egress-tagged VLAN */
0617         if (ocelot_port_num_untagged_vlans(ocelot, port) > 1) {
0618             NL_SET_ERR_MSG_MOD(extack,
0619                        "Port with more than one egress-untagged VLAN cannot have egress-tagged VLANs");
0620             return -EBUSY;
0621         }
0622     }
0623
0624     if (vid > OCELOT_RSV_VLAN_RANGE_START) {
0625         NL_SET_ERR_MSG_MOD(extack,
0626                    "VLAN range 4000-4095 reserved for VLAN-unaware bridging");
0627         return -EBUSY;
0628     }
0629
0630     return 0;
0631 }
0632 EXPORT_SYMBOL(ocelot_vlan_prepare);
0633
0634 int ocelot_vlan_add(struct ocelot *ocelot, int port, u16 vid, bool pvid,
0635             bool untagged)
0636 {
0637     int err;
0638
0639     /* Ignore VID 0 added to our RX filter by the 8021q module, since
0640      * that collides with OCELOT_STANDALONE_PVID and changes it from
0641      * egress-untagged to egress-tagged.
0642      */
0643     if (!vid)
0644         return 0;
0645
0646     err = ocelot_vlan_member_add(ocelot, port, vid, untagged);
0647     if (err)
0648         return err;
0649
0650     /* Default ingress vlan classification */
0651     if (pvid)
0652         ocelot_port_set_pvid(ocelot, port,
0653                      ocelot_bridge_vlan_find(ocelot, vid));
0654
0655     /* Untagged egress vlan clasification */
0656     ocelot_port_manage_port_tag(ocelot, port);
0657
0658     return 0;
0659 }
0660 EXPORT_SYMBOL(ocelot_vlan_add);
0661
0662 int ocelot_vlan_del(struct ocelot *ocelot, int port, u16 vid)
0663 {
0664     struct ocelot_port *ocelot_port = ocelot->ports[port];
0665     bool del_pvid = false;
0666     int err;
0667
0668     if (!vid)
0669         return 0;
0670
0671     if (ocelot_port->pvid_vlan && ocelot_port->pvid_vlan->vid == vid)
0672         del_pvid = true;
0673
0674     err = ocelot_vlan_member_del(ocelot, port, vid);
0675     if (err)
0676         return err;
0677
0678     /* Ingress */
0679     if (del_pvid)
0680         ocelot_port_set_pvid(ocelot, port, NULL);
0681
0682     /* Egress */
0683     ocelot_port_manage_port_tag(ocelot, port);
0684
0685     return 0;
0686 }
0687 EXPORT_SYMBOL(ocelot_vlan_del);
0688
0689 static void ocelot_vlan_init(struct ocelot *ocelot)
0690 {
0691     unsigned long all_ports = GENMASK(ocelot->num_phys_ports - 1, 0);
0692     u16 port, vid;
0693
0694     /* Clear VLAN table, by default all ports are members of all VLANs */
0695     ocelot_write(ocelot, ANA_TABLES_VLANACCESS_CMD_INIT,
0696              ANA_TABLES_VLANACCESS);
0697     ocelot_vlant_wait_for_completion(ocelot);
0698
0699     /* Configure the port VLAN memberships */
0700     for (vid = 1; vid < VLAN_N_VID; vid++)
0701         ocelot_vlant_set_mask(ocelot, vid, 0);
0702
0703     /* We need VID 0 to get traffic on standalone ports.
0704      * It is added automatically if the 8021q module is loaded, but we
0705      * can't rely on that since it might not be.
0706      */
0707     ocelot_vlant_set_mask(ocelot, OCELOT_STANDALONE_PVID, all_ports);
0708
0709     /* Set vlan ingress filter mask to all ports but the CPU port by
0710      * default.
0711      */
0712     ocelot_write(ocelot, all_ports, ANA_VLANMASK);
0713
0714     for (port = 0; port < ocelot->num_phys_ports; port++) {
0715         ocelot_write_gix(ocelot, 0, REW_PORT_VLAN_CFG, port);
0716         ocelot_write_gix(ocelot, 0, REW_TAG_CFG, port);
0717     }
0718 }
0719
0720 static u32 ocelot_read_eq_avail(struct ocelot *ocelot, int port)
0721 {
0722     return ocelot_read_rix(ocelot, QSYS_SW_STATUS, port);
0723 }
0724
0725 static int ocelot_port_flush(struct ocelot *ocelot, int port)
0726 {
0727     unsigned int pause_ena;
0728     int err, val;
0729
0730     /* Disable dequeuing from the egress queues */
0731     ocelot_rmw_rix(ocelot, QSYS_PORT_MODE_DEQUEUE_DIS,
0732                QSYS_PORT_MODE_DEQUEUE_DIS,
0733                QSYS_PORT_MODE, port);
0734
0735     /* Disable flow control */
0736     ocelot_fields_read(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, &pause_ena);
0737     ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 0);
0738
0739     /* Disable priority flow control */
0740     ocelot_fields_write(ocelot, port,
0741                 QSYS_SWITCH_PORT_MODE_TX_PFC_ENA, 0);
0742
0743     /* Wait at least the time it takes to receive a frame of maximum length
0744      * at the port.
0745      * Worst-case delays for 10 kilobyte jumbo frames are:
0746      * 8 ms on a 10M port
0747      * 800 μs on a 100M port
0748      * 80 μs on a 1G port
0749      * 32 μs on a 2.5G port
0750      */
0751     usleep_range(8000, 10000);
0752
0753     /* Disable half duplex backpressure. */
0754     ocelot_rmw_rix(ocelot, 0, SYS_FRONT_PORT_MODE_HDX_MODE,
0755                SYS_FRONT_PORT_MODE, port);
0756
0757     /* Flush the queues associated with the port. */
0758     ocelot_rmw_gix(ocelot, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG_FLUSH_ENA,
0759                REW_PORT_CFG, port);
0760
0761     /* Enable dequeuing from the egress queues. */
0762     ocelot_rmw_rix(ocelot, 0, QSYS_PORT_MODE_DEQUEUE_DIS, QSYS_PORT_MODE,
0763                port);
0764
0765     /* Wait until flushing is complete. */
0766     err = read_poll_timeout(ocelot_read_eq_avail, val, !val,
0767                 100, 2000000, false, ocelot, port);
0768
0769     /* Clear flushing again. */
0770     ocelot_rmw_gix(ocelot, 0, REW_PORT_CFG_FLUSH_ENA, REW_PORT_CFG, port);
0771
0772     /* Re-enable flow control */
0773     ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, pause_ena);
0774
0775     return err;
0776 }
0777
0778 void ocelot_phylink_mac_link_down(struct ocelot *ocelot, int port,
0779                   unsigned int link_an_mode,
0780                   phy_interface_t interface,
0781                   unsigned long quirks)
0782 {
0783     struct ocelot_port *ocelot_port = ocelot->ports[port];
0784     int err;
0785
0786     ocelot_port->speed = SPEED_UNKNOWN;
0787
0788     ocelot_port_rmwl(ocelot_port, 0, DEV_MAC_ENA_CFG_RX_ENA,
0789              DEV_MAC_ENA_CFG);
0790
0791     if (ocelot->ops->cut_through_fwd) {
0792         mutex_lock(&ocelot->fwd_domain_lock);
0793         ocelot->ops->cut_through_fwd(ocelot);
0794         mutex_unlock(&ocelot->fwd_domain_lock);
0795     }
0796
0797     ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
0798
0799     err = ocelot_port_flush(ocelot, port);
0800     if (err)
0801         dev_err(ocelot->dev, "failed to flush port %d: %d\n",
0802             port, err);
0803
0804     /* Put the port in reset. */
0805     if (interface != PHY_INTERFACE_MODE_QSGMII ||
0806         !(quirks & OCELOT_QUIRK_QSGMII_PORTS_MUST_BE_UP))
0807         ocelot_port_rmwl(ocelot_port,
0808                  DEV_CLOCK_CFG_MAC_TX_RST |
0809                  DEV_CLOCK_CFG_MAC_RX_RST,
0810                  DEV_CLOCK_CFG_MAC_TX_RST |
0811                  DEV_CLOCK_CFG_MAC_RX_RST,
0812                  DEV_CLOCK_CFG);
0813 }
0814 EXPORT_SYMBOL_GPL(ocelot_phylink_mac_link_down);
0815
0816 void ocelot_phylink_mac_link_up(struct ocelot *ocelot, int port,
0817                 struct phy_device *phydev,
0818                 unsigned int link_an_mode,
0819                 phy_interface_t interface,
0820                 int speed, int duplex,
0821                 bool tx_pause, bool rx_pause,
0822                 unsigned long quirks)
0823 {
0824     struct ocelot_port *ocelot_port = ocelot->ports[port];
0825     int mac_speed, mode = 0;
0826     u32 mac_fc_cfg;
0827
0828     ocelot_port->speed = speed;
0829
0830     /* The MAC might be integrated in systems where the MAC speed is fixed
0831      * and it's the PCS who is performing the rate adaptation, so we have
0832      * to write "1000Mbps" into the LINK_SPEED field of DEV_CLOCK_CFG
0833      * (which is also its default value).
0834      */
0835     if ((quirks & OCELOT_QUIRK_PCS_PERFORMS_RATE_ADAPTATION) ||
0836         speed == SPEED_1000) {
0837         mac_speed = OCELOT_SPEED_1000;
0838         mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
0839     } else if (speed == SPEED_2500) {
0840         mac_speed = OCELOT_SPEED_2500;
0841         mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA;
0842     } else if (speed == SPEED_100) {
0843         mac_speed = OCELOT_SPEED_100;
0844     } else {
0845         mac_speed = OCELOT_SPEED_10;
0846     }
0847
0848     if (duplex == DUPLEX_FULL)
0849         mode |= DEV_MAC_MODE_CFG_FDX_ENA;
0850
0851     ocelot_port_writel(ocelot_port, mode, DEV_MAC_MODE_CFG);
0852
0853     /* Take port out of reset by clearing the MAC_TX_RST, MAC_RX_RST and
0854      * PORT_RST bits in DEV_CLOCK_CFG.
0855      */
0856     ocelot_port_writel(ocelot_port, DEV_CLOCK_CFG_LINK_SPEED(mac_speed),
0857                DEV_CLOCK_CFG);
0858
0859     switch (speed) {
0860     case SPEED_10:
0861         mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(OCELOT_SPEED_10);
0862         break;
0863     case SPEED_100:
0864         mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(OCELOT_SPEED_100);
0865         break;
0866     case SPEED_1000:
0867     case SPEED_2500:
0868         mac_fc_cfg = SYS_MAC_FC_CFG_FC_LINK_SPEED(OCELOT_SPEED_1000);
0869         break;
0870     default:
0871         dev_err(ocelot->dev, "Unsupported speed on port %d: %d\n",
0872             port, speed);
0873         return;
0874     }
0875
0876     /* Handle RX pause in all cases, with 2500base-X this is used for rate
0877      * adaptation.
0878      */
0879     mac_fc_cfg |= SYS_MAC_FC_CFG_RX_FC_ENA;
0880
0881     if (tx_pause)
0882         mac_fc_cfg |= SYS_MAC_FC_CFG_TX_FC_ENA |
0883                   SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
0884                   SYS_MAC_FC_CFG_FC_LATENCY_CFG(0x7) |
0885                   SYS_MAC_FC_CFG_ZERO_PAUSE_ENA;
0886
0887     /* Flow control. Link speed is only used here to evaluate the time
0888      * specification in incoming pause frames.
0889      */
0890     ocelot_write_rix(ocelot, mac_fc_cfg, SYS_MAC_FC_CFG, port);
0891
0892     ocelot_write_rix(ocelot, 0, ANA_POL_FLOWC, port);
0893
0894     /* Don't attempt to send PAUSE frames on the NPI port, it's broken */
0895     if (port != ocelot->npi)
0896         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA,
0897                     tx_pause);
0898
0899     /* Undo the effects of ocelot_phylink_mac_link_down:
0900      * enable MAC module
0901      */
0902     ocelot_port_writel(ocelot_port, DEV_MAC_ENA_CFG_RX_ENA |
0903                DEV_MAC_ENA_CFG_TX_ENA, DEV_MAC_ENA_CFG);
0904
0905     /* If the port supports cut-through forwarding, update the masks before
0906      * enabling forwarding on the port.
0907      */
0908     if (ocelot->ops->cut_through_fwd) {
0909         mutex_lock(&ocelot->fwd_domain_lock);
0910         ocelot->ops->cut_through_fwd(ocelot);
0911         mutex_unlock(&ocelot->fwd_domain_lock);
0912     }
0913
0914     /* Core: Enable port for frame transfer */
0915     ocelot_fields_write(ocelot, port,
0916                 QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
0917 }
0918 EXPORT_SYMBOL_GPL(ocelot_phylink_mac_link_up);
0919
0920 static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
0921                     struct sk_buff *clone)
0922 {
0923     struct ocelot_port *ocelot_port = ocelot->ports[port];
0924     unsigned long flags;
0925
0926     spin_lock_irqsave(&ocelot->ts_id_lock, flags);
0927
0928     if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
0929         ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
0930         spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
0931         return -EBUSY;
0932     }
0933
0934     skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
0935     /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
0936     OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
0937
0938     ocelot_port->ts_id++;
0939     if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
0940         ocelot_port->ts_id = 0;
0941
0942     ocelot_port->ptp_skbs_in_flight++;
0943     ocelot->ptp_skbs_in_flight++;
0944
0945     skb_queue_tail(&ocelot_port->tx_skbs, clone);
0946
0947     spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
0948
0949     return 0;
0950 }
0951
0952 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
0953                        unsigned int ptp_class)
0954 {
0955     struct ptp_header *hdr;
0956     u8 msgtype, twostep;
0957
0958     hdr = ptp_parse_header(skb, ptp_class);
0959     if (!hdr)
0960         return false;
0961
0962     msgtype = ptp_get_msgtype(hdr, ptp_class);
0963     twostep = hdr->flag_field[0] & 0x2;
0964
0965     if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
0966         return true;
0967
0968     return false;
0969 }
0970
0971 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
0972                  struct sk_buff *skb,
0973                  struct sk_buff **clone)
0974 {
0975     struct ocelot_port *ocelot_port = ocelot->ports[port];
0976     u8 ptp_cmd = ocelot_port->ptp_cmd;
0977     unsigned int ptp_class;
0978     int err;
0979
0980     /* Don't do anything if PTP timestamping not enabled */
0981     if (!ptp_cmd)
0982         return 0;
0983
0984     ptp_class = ptp_classify_raw(skb);
0985     if (ptp_class == PTP_CLASS_NONE)
0986         return -EINVAL;
0987
0988     /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
0989     if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
0990         if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
0991             OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
0992             return 0;
0993         }
0994
0995         /* Fall back to two-step timestamping */
0996         ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
0997     }
0998
0999     if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
1000         *clone = skb_clone_sk(skb);
1001         if (!(*clone))
1002             return -ENOMEM;
1003
1004         err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
1005         if (err)
1006             return err;
1007
1008         OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
1009         OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
1010     }
1011
1012     return 0;
1013 }
1014 EXPORT_SYMBOL(ocelot_port_txtstamp_request);
1015
1016 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
1017                    struct timespec64 *ts)
1018 {
1019     unsigned long flags;
1020     u32 val;
1021
1022     spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
1023
1024     /* Read current PTP time to get seconds */
1025     val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
1026
1027     val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
1028     val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
1029     ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
1030     ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
1031
1032     /* Read packet HW timestamp from FIFO */
1033     val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
1034     ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
1035
1036     /* Sec has incremented since the ts was registered */
1037     if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
1038         ts->tv_sec--;
1039
1040     spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
1041 }
1042
1043 static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
1044 {
1045     struct ptp_header *hdr;
1046
1047     hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
1048     if (WARN_ON(!hdr))
1049         return false;
1050
1051     return seqid == ntohs(hdr->sequence_id);
1052 }
1053
1054 void ocelot_get_txtstamp(struct ocelot *ocelot)
1055 {
1056     int budget = OCELOT_PTP_QUEUE_SZ;
1057
1058     while (budget--) {
1059         struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
1060         struct skb_shared_hwtstamps shhwtstamps;
1061         u32 val, id, seqid, txport;
1062         struct ocelot_port *port;
1063         struct timespec64 ts;
1064         unsigned long flags;
1065
1066         val = ocelot_read(ocelot, SYS_PTP_STATUS);
1067
1068         /* Check if a timestamp can be retrieved */
1069         if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
1070             break;
1071
1072         WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
1073
1074         /* Retrieve the ts ID and Tx port */
1075         id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
1076         txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
1077         seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
1078
1079         port = ocelot->ports[txport];
1080
1081         spin_lock(&ocelot->ts_id_lock);
1082         port->ptp_skbs_in_flight--;
1083         ocelot->ptp_skbs_in_flight--;
1084         spin_unlock(&ocelot->ts_id_lock);
1085
1086         /* Retrieve its associated skb */
1087 try_again:
1088         spin_lock_irqsave(&port->tx_skbs.lock, flags);
1089
1090         skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
1091             if (OCELOT_SKB_CB(skb)->ts_id != id)
1092                 continue;
1093             __skb_unlink(skb, &port->tx_skbs);
1094             skb_match = skb;
1095             break;
1096         }
1097
1098         spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
1099
1100         if (WARN_ON(!skb_match))
1101             continue;
1102
1103         if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
1104             dev_err_ratelimited(ocelot->dev,
1105                         "port %d received stale TX timestamp for seqid %d, discarding\n",
1106                         txport, seqid);
1107             dev_kfree_skb_any(skb);
1108             goto try_again;
1109         }
1110
1111         /* Get the h/w timestamp */
1112         ocelot_get_hwtimestamp(ocelot, &ts);
1113
1114         /* Set the timestamp into the skb */
1115         memset(&shhwtstamps, 0, sizeof(shhwtstamps));
1116         shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
1117         skb_complete_tx_timestamp(skb_match, &shhwtstamps);
1118
1119         /* Next ts */
1120         ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
1121     }
1122 }
1123 EXPORT_SYMBOL(ocelot_get_txtstamp);
1124
1125 static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
1126                 u32 *rval)
1127 {
1128     u32 bytes_valid, val;
1129
1130     val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1131     if (val == XTR_NOT_READY) {
1132         if (ifh)
1133             return -EIO;
1134
1135         do {
1136             val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1137         } while (val == XTR_NOT_READY);
1138     }
1139
1140     switch (val) {
1141     case XTR_ABORT:
1142         return -EIO;
1143     case XTR_EOF_0:
1144     case XTR_EOF_1:
1145     case XTR_EOF_2:
1146     case XTR_EOF_3:
1147     case XTR_PRUNED:
1148         bytes_valid = XTR_VALID_BYTES(val);
1149         val = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1150         if (val == XTR_ESCAPE)
1151             *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1152         else
1153             *rval = val;
1154
1155         return bytes_valid;
1156     case XTR_ESCAPE:
1157         *rval = ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1158
1159         return 4;
1160     default:
1161         *rval = val;
1162
1163         return 4;
1164     }
1165 }
1166
1167 static int ocelot_xtr_poll_xfh(struct ocelot *ocelot, int grp, u32 *xfh)
1168 {
1169     int i, err = 0;
1170
1171     for (i = 0; i < OCELOT_TAG_LEN / 4; i++) {
1172         err = ocelot_rx_frame_word(ocelot, grp, true, &xfh[i]);
1173         if (err != 4)
1174             return (err < 0) ? err : -EIO;
1175     }
1176
1177     return 0;
1178 }
1179
1180 void ocelot_ptp_rx_timestamp(struct ocelot *ocelot, struct sk_buff *skb,
1181                  u64 timestamp)
1182 {
1183     struct skb_shared_hwtstamps *shhwtstamps;
1184     u64 tod_in_ns, full_ts_in_ns;
1185     struct timespec64 ts;
1186
1187     ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
1188
1189     tod_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
1190     if ((tod_in_ns & 0xffffffff) < timestamp)
1191         full_ts_in_ns = (((tod_in_ns >> 32) - 1) << 32) |
1192                 timestamp;
1193     else
1194         full_ts_in_ns = (tod_in_ns & GENMASK_ULL(63, 32)) |
1195                 timestamp;
1196
1197     shhwtstamps = skb_hwtstamps(skb);
1198     memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
1199     shhwtstamps->hwtstamp = full_ts_in_ns;
1200 }
1201 EXPORT_SYMBOL(ocelot_ptp_rx_timestamp);
1202
1203 int ocelot_xtr_poll_frame(struct ocelot *ocelot, int grp, struct sk_buff **nskb)
1204 {
1205     u64 timestamp, src_port, len;
1206     u32 xfh[OCELOT_TAG_LEN / 4];
1207     struct net_device *dev;
1208     struct sk_buff *skb;
1209     int sz, buf_len;
1210     u32 val, *buf;
1211     int err;
1212
1213     err = ocelot_xtr_poll_xfh(ocelot, grp, xfh);
1214     if (err)
1215         return err;
1216
1217     ocelot_xfh_get_src_port(xfh, &src_port);
1218     ocelot_xfh_get_len(xfh, &len);
1219     ocelot_xfh_get_rew_val(xfh, &timestamp);
1220
1221     if (WARN_ON(src_port >= ocelot->num_phys_ports))
1222         return -EINVAL;
1223
1224     dev = ocelot->ops->port_to_netdev(ocelot, src_port);
1225     if (!dev)
1226         return -EINVAL;
1227
1228     skb = netdev_alloc_skb(dev, len);
1229     if (unlikely(!skb)) {
1230         netdev_err(dev, "Unable to allocate sk_buff\n");
1231         return -ENOMEM;
1232     }
1233
1234     buf_len = len - ETH_FCS_LEN;
1235     buf = (u32 *)skb_put(skb, buf_len);
1236
1237     len = 0;
1238     do {
1239         sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
1240         if (sz < 0) {
1241             err = sz;
1242             goto out_free_skb;
1243         }
1244         *buf++ = val;
1245         len += sz;
1246     } while (len < buf_len);
1247
1248     /* Read the FCS */
1249     sz = ocelot_rx_frame_word(ocelot, grp, false, &val);
1250     if (sz < 0) {
1251         err = sz;
1252         goto out_free_skb;
1253     }
1254
1255     /* Update the statistics if part of the FCS was read before */
1256     len -= ETH_FCS_LEN - sz;
1257
1258     if (unlikely(dev->features & NETIF_F_RXFCS)) {
1259         buf = (u32 *)skb_put(skb, ETH_FCS_LEN);
1260         *buf = val;
1261     }
1262
1263     if (ocelot->ptp)
1264         ocelot_ptp_rx_timestamp(ocelot, skb, timestamp);
1265
1266     /* Everything we see on an interface that is in the HW bridge
1267      * has already been forwarded.
1268      */
1269     if (ocelot->ports[src_port]->bridge)
1270         skb->offload_fwd_mark = 1;
1271
1272     skb->protocol = eth_type_trans(skb, dev);
1273
1274     *nskb = skb;
1275
1276     return 0;
1277
1278 out_free_skb:
1279     kfree_skb(skb);
1280     return err;
1281 }
1282 EXPORT_SYMBOL(ocelot_xtr_poll_frame);
1283
1284 bool ocelot_can_inject(struct ocelot *ocelot, int grp)
1285 {
1286     u32 val = ocelot_read(ocelot, QS_INJ_STATUS);
1287
1288     if (!(val & QS_INJ_STATUS_FIFO_RDY(BIT(grp))))
1289         return false;
1290     if (val & QS_INJ_STATUS_WMARK_REACHED(BIT(grp)))
1291         return false;
1292
1293     return true;
1294 }
1295 EXPORT_SYMBOL(ocelot_can_inject);
1296
1297 void ocelot_ifh_port_set(void *ifh, int port, u32 rew_op, u32 vlan_tag)
1298 {
1299     ocelot_ifh_set_bypass(ifh, 1);
1300     ocelot_ifh_set_dest(ifh, BIT_ULL(port));
1301     ocelot_ifh_set_tag_type(ifh, IFH_TAG_TYPE_C);
1302     if (vlan_tag)
1303         ocelot_ifh_set_vlan_tci(ifh, vlan_tag);
1304     if (rew_op)
1305         ocelot_ifh_set_rew_op(ifh, rew_op);
1306 }
1307 EXPORT_SYMBOL(ocelot_ifh_port_set);
1308
1309 void ocelot_port_inject_frame(struct ocelot *ocelot, int port, int grp,
1310                   u32 rew_op, struct sk_buff *skb)
1311 {
1312     u32 ifh[OCELOT_TAG_LEN / 4] = {0};
1313     unsigned int i, count, last;
1314
1315     ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
1316              QS_INJ_CTRL_SOF, QS_INJ_CTRL, grp);
1317
1318     ocelot_ifh_port_set(ifh, port, rew_op, skb_vlan_tag_get(skb));
1319
1320     for (i = 0; i < OCELOT_TAG_LEN / 4; i++)
1321         ocelot_write_rix(ocelot, ifh[i], QS_INJ_WR, grp);
1322
1323     count = DIV_ROUND_UP(skb->len, 4);
1324     last = skb->len % 4;
1325     for (i = 0; i < count; i++)
1326         ocelot_write_rix(ocelot, ((u32 *)skb->data)[i], QS_INJ_WR, grp);
1327
1328     /* Add padding */
1329     while (i < (OCELOT_BUFFER_CELL_SZ / 4)) {
1330         ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
1331         i++;
1332     }
1333
1334     /* Indicate EOF and valid bytes in last word */
1335     ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(1) |
1336              QS_INJ_CTRL_VLD_BYTES(skb->len < OCELOT_BUFFER_CELL_SZ ? 0 : last) |
1337              QS_INJ_CTRL_EOF,
1338              QS_INJ_CTRL, grp);
1339
1340     /* Add dummy CRC */
1341     ocelot_write_rix(ocelot, 0, QS_INJ_WR, grp);
1342     skb_tx_timestamp(skb);
1343
1344     skb->dev->stats.tx_packets++;
1345     skb->dev->stats.tx_bytes += skb->len;
1346 }
1347 EXPORT_SYMBOL(ocelot_port_inject_frame);
1348
1349 void ocelot_drain_cpu_queue(struct ocelot *ocelot, int grp)
1350 {
1351     while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp))
1352         ocelot_read_rix(ocelot, QS_XTR_RD, grp);
1353 }
1354 EXPORT_SYMBOL(ocelot_drain_cpu_queue);
1355
1356 int ocelot_fdb_add(struct ocelot *ocelot, int port, const unsigned char *addr,
1357            u16 vid, const struct net_device *bridge)
1358 {
1359     if (!vid)
1360         vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
1361
1362     return ocelot_mact_learn(ocelot, port, addr, vid, ENTRYTYPE_LOCKED);
1363 }
1364 EXPORT_SYMBOL(ocelot_fdb_add);
1365
1366 int ocelot_fdb_del(struct ocelot *ocelot, int port, const unsigned char *addr,
1367            u16 vid, const struct net_device *bridge)
1368 {
1369     if (!vid)
1370         vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
1371
1372     return ocelot_mact_forget(ocelot, addr, vid);
1373 }
1374 EXPORT_SYMBOL(ocelot_fdb_del);
1375
1376 int ocelot_port_fdb_do_dump(const unsigned char *addr, u16 vid,
1377                 bool is_static, void *data)
1378 {
1379     struct ocelot_dump_ctx *dump = data;
1380     u32 portid = NETLINK_CB(dump->cb->skb).portid;
1381     u32 seq = dump->cb->nlh->nlmsg_seq;
1382     struct nlmsghdr *nlh;
1383     struct ndmsg *ndm;
1384
1385     if (dump->idx < dump->cb->args[2])
1386         goto skip;
1387
1388     nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
1389             sizeof(*ndm), NLM_F_MULTI);
1390     if (!nlh)
1391         return -EMSGSIZE;
1392
1393     ndm = nlmsg_data(nlh);
1394     ndm->ndm_family  = AF_BRIDGE;
1395     ndm->ndm_pad1    = 0;
1396     ndm->ndm_pad2    = 0;
1397     ndm->ndm_flags   = NTF_SELF;
1398     ndm->ndm_type    = 0;
1399     ndm->ndm_ifindex = dump->dev->ifindex;
1400     ndm->ndm_state   = is_static ? NUD_NOARP : NUD_REACHABLE;
1401
1402     if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, addr))
1403         goto nla_put_failure;
1404
1405     if (vid && nla_put_u16(dump->skb, NDA_VLAN, vid))
1406         goto nla_put_failure;
1407
1408     nlmsg_end(dump->skb, nlh);
1409
1410 skip:
1411     dump->idx++;
1412     return 0;
1413
1414 nla_put_failure:
1415     nlmsg_cancel(dump->skb, nlh);
1416     return -EMSGSIZE;
1417 }
1418 EXPORT_SYMBOL(ocelot_port_fdb_do_dump);
1419
1420 /* Caller must hold &ocelot->mact_lock */
1421 static int ocelot_mact_read(struct ocelot *ocelot, int port, int row, int col,
1422                 struct ocelot_mact_entry *entry)
1423 {
1424     u32 val, dst, macl, mach;
1425     char mac[ETH_ALEN];
1426
1427     /* Set row and column to read from */
1428     ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
1429     ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
1430
1431     /* Issue a read command */
1432     ocelot_write(ocelot,
1433              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
1434              ANA_TABLES_MACACCESS);
1435
1436     if (ocelot_mact_wait_for_completion(ocelot))
1437         return -ETIMEDOUT;
1438
1439     /* Read the entry flags */
1440     val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
1441     if (!(val & ANA_TABLES_MACACCESS_VALID))
1442         return -EINVAL;
1443
1444     /* If the entry read has another port configured as its destination,
1445      * do not report it.
1446      */
1447     dst = (val & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
1448     if (dst != port)
1449         return -EINVAL;
1450
1451     /* Get the entry's MAC address and VLAN id */
1452     macl = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
1453     mach = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
1454
1455     mac[0] = (mach >> 8)  & 0xff;
1456     mac[1] = (mach >> 0)  & 0xff;
1457     mac[2] = (macl >> 24) & 0xff;
1458     mac[3] = (macl >> 16) & 0xff;
1459     mac[4] = (macl >> 8)  & 0xff;
1460     mac[5] = (macl >> 0)  & 0xff;
1461
1462     entry->vid = (mach >> 16) & 0xfff;
1463     ether_addr_copy(entry->mac, mac);
1464
1465     return 0;
1466 }
1467
1468 int ocelot_mact_flush(struct ocelot *ocelot, int port)
1469 {
1470     int err;
1471
1472     mutex_lock(&ocelot->mact_lock);
1473
1474     /* Program ageing filter for a single port */
1475     ocelot_write(ocelot, ANA_ANAGEFIL_PID_EN | ANA_ANAGEFIL_PID_VAL(port),
1476              ANA_ANAGEFIL);
1477
1478     /* Flushing dynamic FDB entries requires two successive age scans */
1479     ocelot_write(ocelot,
1480              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_AGE),
1481              ANA_TABLES_MACACCESS);
1482
1483     err = ocelot_mact_wait_for_completion(ocelot);
1484     if (err) {
1485         mutex_unlock(&ocelot->mact_lock);
1486         return err;
1487     }
1488
1489     /* And second... */
1490     ocelot_write(ocelot,
1491              ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_AGE),
1492              ANA_TABLES_MACACCESS);
1493
1494     err = ocelot_mact_wait_for_completion(ocelot);
1495
1496     /* Restore ageing filter */
1497     ocelot_write(ocelot, 0, ANA_ANAGEFIL);
1498
1499     mutex_unlock(&ocelot->mact_lock);
1500
1501     return err;
1502 }
1503 EXPORT_SYMBOL_GPL(ocelot_mact_flush);
1504
1505 int ocelot_fdb_dump(struct ocelot *ocelot, int port,
1506             dsa_fdb_dump_cb_t *cb, void *data)
1507 {
1508     int err = 0;
1509     int i, j;
1510
1511     /* We could take the lock just around ocelot_mact_read, but doing so
1512      * thousands of times in a row seems rather pointless and inefficient.
1513      */
1514     mutex_lock(&ocelot->mact_lock);
1515
1516     /* Loop through all the mac tables entries. */
1517     for (i = 0; i < ocelot->num_mact_rows; i++) {
1518         for (j = 0; j < 4; j++) {
1519             struct ocelot_mact_entry entry;
1520             bool is_static;
1521
1522             err = ocelot_mact_read(ocelot, port, i, j, &entry);
1523             /* If the entry is invalid (wrong port, invalid...),
1524              * skip it.
1525              */
1526             if (err == -EINVAL)
1527                 continue;
1528             else if (err)
1529                 break;
1530
1531             is_static = (entry.type == ENTRYTYPE_LOCKED);
1532
1533             /* Hide the reserved VLANs used for
1534              * VLAN-unaware bridging.
1535              */
1536             if (entry.vid > OCELOT_RSV_VLAN_RANGE_START)
1537                 entry.vid = 0;
1538
1539             err = cb(entry.mac, entry.vid, is_static, data);
1540             if (err)
1541                 break;
1542         }
1543     }
1544
1545     mutex_unlock(&ocelot->mact_lock);
1546
1547     return err;
1548 }
1549 EXPORT_SYMBOL(ocelot_fdb_dump);
1550
1551 static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap)
1552 {
1553     trap->key_type = OCELOT_VCAP_KEY_ETYPE;
1554     *(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588);
1555     *(__be16 *)trap->key.etype.etype.mask = htons(0xffff);
1556 }
1557
1558 static void
1559 ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
1560 {
1561     trap->key_type = OCELOT_VCAP_KEY_IPV4;
1562     trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1563     trap->key.ipv4.proto.mask[0] = 0xff;
1564     trap->key.ipv4.dport.value = PTP_EV_PORT;
1565     trap->key.ipv4.dport.mask = 0xffff;
1566 }
1567
1568 static void
1569 ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
1570 {
1571     trap->key_type = OCELOT_VCAP_KEY_IPV6;
1572     trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1573     trap->key.ipv4.proto.mask[0] = 0xff;
1574     trap->key.ipv6.dport.value = PTP_EV_PORT;
1575     trap->key.ipv6.dport.mask = 0xffff;
1576 }
1577
1578 static void
1579 ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
1580 {
1581     trap->key_type = OCELOT_VCAP_KEY_IPV4;
1582     trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1583     trap->key.ipv4.proto.mask[0] = 0xff;
1584     trap->key.ipv4.dport.value = PTP_GEN_PORT;
1585     trap->key.ipv4.dport.mask = 0xffff;
1586 }
1587
1588 static void
1589 ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
1590 {
1591     trap->key_type = OCELOT_VCAP_KEY_IPV6;
1592     trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
1593     trap->key.ipv4.proto.mask[0] = 0xff;
1594     trap->key.ipv6.dport.value = PTP_GEN_PORT;
1595     trap->key.ipv6.dport.mask = 0xffff;
1596 }
1597
1598 int ocelot_trap_add(struct ocelot *ocelot, int port,
1599             unsigned long cookie, bool take_ts,
1600             void (*populate)(struct ocelot_vcap_filter *f))
1601 {
1602     struct ocelot_vcap_block *block_vcap_is2;
1603     struct ocelot_vcap_filter *trap;
1604     bool new = false;
1605     int err;
1606
1607     block_vcap_is2 = &ocelot->block[VCAP_IS2];
1608
1609     trap = ocelot_vcap_block_find_filter_by_id(block_vcap_is2, cookie,
1610                            false);
1611     if (!trap) {
1612         trap = kzalloc(sizeof(*trap), GFP_KERNEL);
1613         if (!trap)
1614             return -ENOMEM;
1615
1616         populate(trap);
1617         trap->prio = 1;
1618         trap->id.cookie = cookie;
1619         trap->id.tc_offload = false;
1620         trap->block_id = VCAP_IS2;
1621         trap->type = OCELOT_VCAP_FILTER_OFFLOAD;
1622         trap->lookup = 0;
1623         trap->action.cpu_copy_ena = true;
1624         trap->action.mask_mode = OCELOT_MASK_MODE_PERMIT_DENY;
1625         trap->action.port_mask = 0;
1626         trap->take_ts = take_ts;
1627         trap->is_trap = true;
1628         new = true;
1629     }
1630
1631     trap->ingress_port_mask |= BIT(port);
1632
1633     if (new)
1634         err = ocelot_vcap_filter_add(ocelot, trap, NULL);
1635     else
1636         err = ocelot_vcap_filter_replace(ocelot, trap);
1637     if (err) {
1638         trap->ingress_port_mask &= ~BIT(port);
1639         if (!trap->ingress_port_mask)
1640             kfree(trap);
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         return ocelot_vcap_filter_del(ocelot, trap);
1662
1663     return ocelot_vcap_filter_replace(ocelot, trap);
1664 }
1665
1666 static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port)
1667 {
1668     unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
1669
1670     return ocelot_trap_add(ocelot, port, l2_cookie, true,
1671                    ocelot_populate_l2_ptp_trap_key);
1672 }
1673
1674 static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port)
1675 {
1676     unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
1677
1678     return ocelot_trap_del(ocelot, port, l2_cookie);
1679 }
1680
1681 static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port)
1682 {
1683     unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
1684     unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
1685     int err;
1686
1687     err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true,
1688                   ocelot_populate_ipv4_ptp_event_trap_key);
1689     if (err)
1690         return err;
1691
1692     err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false,
1693                   ocelot_populate_ipv4_ptp_general_trap_key);
1694     if (err)
1695         ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
1696
1697     return err;
1698 }
1699
1700 static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port)
1701 {
1702     unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
1703     unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
1704     int err;
1705
1706     err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
1707     err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie);
1708     return err;
1709 }
1710
1711 static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port)
1712 {
1713     unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
1714     unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
1715     int err;
1716
1717     err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true,
1718                   ocelot_populate_ipv6_ptp_event_trap_key);
1719     if (err)
1720         return err;
1721
1722     err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false,
1723                   ocelot_populate_ipv6_ptp_general_trap_key);
1724     if (err)
1725         ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
1726
1727     return err;
1728 }
1729
1730 static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port)
1731 {
1732     unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
1733     unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
1734     int err;
1735
1736     err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
1737     err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie);
1738     return err;
1739 }
1740
1741 static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
1742                   bool l2, bool l4)
1743 {
1744     int err;
1745
1746     if (l2)
1747         err = ocelot_l2_ptp_trap_add(ocelot, port);
1748     else
1749         err = ocelot_l2_ptp_trap_del(ocelot, port);
1750     if (err)
1751         return err;
1752
1753     if (l4) {
1754         err = ocelot_ipv4_ptp_trap_add(ocelot, port);
1755         if (err)
1756             goto err_ipv4;
1757
1758         err = ocelot_ipv6_ptp_trap_add(ocelot, port);
1759         if (err)
1760             goto err_ipv6;
1761     } else {
1762         err = ocelot_ipv4_ptp_trap_del(ocelot, port);
1763
1764         err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
1765     }
1766     if (err)
1767         return err;
1768
1769     return 0;
1770
1771 err_ipv6:
1772     ocelot_ipv4_ptp_trap_del(ocelot, port);
1773 err_ipv4:
1774     if (l2)
1775         ocelot_l2_ptp_trap_del(ocelot, port);
1776     return err;
1777 }
1778
1779 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
1780 {
1781     return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
1782                 sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
1783 }
1784 EXPORT_SYMBOL(ocelot_hwstamp_get);
1785
1786 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
1787 {
1788     struct ocelot_port *ocelot_port = ocelot->ports[port];
1789     bool l2 = false, l4 = false;
1790     struct hwtstamp_config cfg;
1791     int err;
1792
1793     if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
1794         return -EFAULT;
1795
1796     /* Tx type sanity check */
1797     switch (cfg.tx_type) {
1798     case HWTSTAMP_TX_ON:
1799         ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
1800         break;
1801     case HWTSTAMP_TX_ONESTEP_SYNC:
1802         /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
1803          * need to update the origin time.
1804          */
1805         ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
1806         break;
1807     case HWTSTAMP_TX_OFF:
1808         ocelot_port->ptp_cmd = 0;
1809         break;
1810     default:
1811         return -ERANGE;
1812     }
1813
1814     mutex_lock(&ocelot->ptp_lock);
1815
1816     switch (cfg.rx_filter) {
1817     case HWTSTAMP_FILTER_NONE:
1818         break;
1819     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1820     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1821     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1822         l4 = true;
1823         break;
1824     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1825     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1826     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1827         l2 = true;
1828         break;
1829     case HWTSTAMP_FILTER_PTP_V2_EVENT:
1830     case HWTSTAMP_FILTER_PTP_V2_SYNC:
1831     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1832         l2 = true;
1833         l4 = true;
1834         break;
1835     default:
1836         mutex_unlock(&ocelot->ptp_lock);
1837         return -ERANGE;
1838     }
1839
1840     err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
1841     if (err) {
1842         mutex_unlock(&ocelot->ptp_lock);
1843         return err;
1844     }
1845
1846     if (l2 && l4)
1847         cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
1848     else if (l2)
1849         cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
1850     else if (l4)
1851         cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
1852     else
1853         cfg.rx_filter = HWTSTAMP_FILTER_NONE;
1854
1855     /* Commit back the result & save it */
1856     memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
1857     mutex_unlock(&ocelot->ptp_lock);
1858
1859     return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
1860 }
1861 EXPORT_SYMBOL(ocelot_hwstamp_set);
1862
1863 void ocelot_get_strings(struct ocelot *ocelot, int port, u32 sset, u8 *data)
1864 {
1865     int i;
1866
1867     if (sset != ETH_SS_STATS)
1868         return;
1869
1870     for (i = 0; i < OCELOT_NUM_STATS; i++) {
1871         if (ocelot->stats_layout[i].name[0] == '\0')
1872             continue;
1873
1874         memcpy(data + i * ETH_GSTRING_LEN, ocelot->stats_layout[i].name,
1875                ETH_GSTRING_LEN);
1876     }
1877 }
1878 EXPORT_SYMBOL(ocelot_get_strings);
1879
1880 /* Caller must hold &ocelot->stats_lock */
1881 static int ocelot_port_update_stats(struct ocelot *ocelot, int port)
1882 {
1883     unsigned int idx = port * OCELOT_NUM_STATS;
1884     struct ocelot_stats_region *region;
1885     int err, j;
1886
1887     /* Configure the port to read the stats from */
1888     ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port), SYS_STAT_CFG);
1889
1890     list_for_each_entry(region, &ocelot->stats_regions, node) {
1891         err = ocelot_bulk_read(ocelot, region->base, 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     spin_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     spin_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     spin_lock(&ocelot->stats_lock);
1939
1940     /* check and update now */
1941     err = ocelot_port_update_stats(ocelot, port);
1942
1943     /* Copy all supported counters */
1944     for (i = 0; i < OCELOT_NUM_STATS; i++) {
1945         int index = port * OCELOT_NUM_STATS + i;
1946
1947         if (ocelot->stats_layout[i].name[0] == '\0')
1948             continue;
1949
1950         *data++ = ocelot->stats[index];
1951     }
1952
1953     spin_unlock(&ocelot->stats_lock);
1954
1955     if (err)
1956         dev_err(ocelot->dev, "Error %d updating ethtool stats\n", err);
1957 }
1958 EXPORT_SYMBOL(ocelot_get_ethtool_stats);
1959
1960 int ocelot_get_sset_count(struct ocelot *ocelot, int port, int sset)
1961 {
1962     int i, num_stats = 0;
1963
1964     if (sset != ETH_SS_STATS)
1965         return -EOPNOTSUPP;
1966
1967     for (i = 0; i < OCELOT_NUM_STATS; i++)
1968         if (ocelot->stats_layout[i].name[0] != '\0')
1969             num_stats++;
1970
1971     return num_stats;
1972 }
1973 EXPORT_SYMBOL(ocelot_get_sset_count);
1974
1975 static int ocelot_prepare_stats_regions(struct ocelot *ocelot)
1976 {
1977     struct ocelot_stats_region *region = NULL;
1978     unsigned int last;
1979     int i;
1980
1981     INIT_LIST_HEAD(&ocelot->stats_regions);
1982
1983     for (i = 0; i < OCELOT_NUM_STATS; i++) {
1984         if (ocelot->stats_layout[i].name[0] == '\0')
1985             continue;
1986
1987         if (region && ocelot->stats_layout[i].reg == last + 4) {
1988             region->count++;
1989         } else {
1990             region = devm_kzalloc(ocelot->dev, sizeof(*region),
1991                           GFP_KERNEL);
1992             if (!region)
1993                 return -ENOMEM;
1994
1995             region->base = ocelot->stats_layout[i].reg;
1996             region->count = 1;
1997             list_add_tail(&region->node, &ocelot->stats_regions);
1998         }
1999
2000         last = ocelot->stats_layout[i].reg;
2001     }
2002
2003     list_for_each_entry(region, &ocelot->stats_regions, node) {
2004         region->buf = devm_kcalloc(ocelot->dev, region->count,
2005                        sizeof(*region->buf), GFP_KERNEL);
2006         if (!region->buf)
2007             return -ENOMEM;
2008     }
2009
2010     return 0;
2011 }
2012
2013 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
2014                struct ethtool_ts_info *info)
2015 {
2016     info->phc_index = ocelot->ptp_clock ?
2017               ptp_clock_index(ocelot->ptp_clock) : -1;
2018     if (info->phc_index == -1) {
2019         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
2020                      SOF_TIMESTAMPING_RX_SOFTWARE |
2021                      SOF_TIMESTAMPING_SOFTWARE;
2022         return 0;
2023     }
2024     info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
2025                  SOF_TIMESTAMPING_RX_SOFTWARE |
2026                  SOF_TIMESTAMPING_SOFTWARE |
2027                  SOF_TIMESTAMPING_TX_HARDWARE |
2028                  SOF_TIMESTAMPING_RX_HARDWARE |
2029                  SOF_TIMESTAMPING_RAW_HARDWARE;
2030     info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
2031              BIT(HWTSTAMP_TX_ONESTEP_SYNC);
2032     info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
2033                BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
2034                BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
2035                BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
2036
2037     return 0;
2038 }
2039 EXPORT_SYMBOL(ocelot_get_ts_info);
2040
2041 static u32 ocelot_get_bond_mask(struct ocelot *ocelot, struct net_device *bond)
2042 {
2043     u32 mask = 0;
2044     int port;
2045
2046     lockdep_assert_held(&ocelot->fwd_domain_lock);
2047
2048     for (port = 0; port < ocelot->num_phys_ports; port++) {
2049         struct ocelot_port *ocelot_port = ocelot->ports[port];
2050
2051         if (!ocelot_port)
2052             continue;
2053
2054         if (ocelot_port->bond == bond)
2055             mask |= BIT(port);
2056     }
2057
2058     return mask;
2059 }
2060
2061 /* The logical port number of a LAG is equal to the lowest numbered physical
2062  * port ID present in that LAG. It may change if that port ever leaves the LAG.
2063  */
2064 static int ocelot_bond_get_id(struct ocelot *ocelot, struct net_device *bond)
2065 {
2066     int bond_mask = ocelot_get_bond_mask(ocelot, bond);
2067
2068     if (!bond_mask)
2069         return -ENOENT;
2070
2071     return __ffs(bond_mask);
2072 }
2073
2074 static u32 ocelot_dsa_8021q_cpu_assigned_ports(struct ocelot *ocelot,
2075                            struct ocelot_port *cpu)
2076 {
2077     u32 mask = 0;
2078     int port;
2079
2080     for (port = 0; port < ocelot->num_phys_ports; port++) {
2081         struct ocelot_port *ocelot_port = ocelot->ports[port];
2082
2083         if (!ocelot_port)
2084             continue;
2085
2086         if (ocelot_port->dsa_8021q_cpu == cpu)
2087             mask |= BIT(port);
2088     }
2089
2090     return mask;
2091 }
2092
2093 u32 ocelot_port_assigned_dsa_8021q_cpu_mask(struct ocelot *ocelot, int port)
2094 {
2095     struct ocelot_port *ocelot_port = ocelot->ports[port];
2096     struct ocelot_port *cpu_port = ocelot_port->dsa_8021q_cpu;
2097
2098     if (!cpu_port)
2099         return 0;
2100
2101     return BIT(cpu_port->index);
2102 }
2103 EXPORT_SYMBOL_GPL(ocelot_port_assigned_dsa_8021q_cpu_mask);
2104
2105 u32 ocelot_get_bridge_fwd_mask(struct ocelot *ocelot, int src_port)
2106 {
2107     struct ocelot_port *ocelot_port = ocelot->ports[src_port];
2108     const struct net_device *bridge;
2109     u32 mask = 0;
2110     int port;
2111
2112     if (!ocelot_port || ocelot_port->stp_state != BR_STATE_FORWARDING)
2113         return 0;
2114
2115     bridge = ocelot_port->bridge;
2116     if (!bridge)
2117         return 0;
2118
2119     for (port = 0; port < ocelot->num_phys_ports; port++) {
2120         ocelot_port = ocelot->ports[port];
2121
2122         if (!ocelot_port)
2123             continue;
2124
2125         if (ocelot_port->stp_state == BR_STATE_FORWARDING &&
2126             ocelot_port->bridge == bridge)
2127             mask |= BIT(port);
2128     }
2129
2130     return mask;
2131 }
2132 EXPORT_SYMBOL_GPL(ocelot_get_bridge_fwd_mask);
2133
2134 static void ocelot_apply_bridge_fwd_mask(struct ocelot *ocelot, bool joining)
2135 {
2136     int port;
2137
2138     lockdep_assert_held(&ocelot->fwd_domain_lock);
2139
2140     /* If cut-through forwarding is supported, update the masks before a
2141      * port joins the forwarding domain, to avoid potential underruns if it
2142      * has the highest speed from the new domain.
2143      */
2144     if (joining && ocelot->ops->cut_through_fwd)
2145         ocelot->ops->cut_through_fwd(ocelot);
2146
2147     /* Apply FWD mask. The loop is needed to add/remove the current port as
2148      * a source for the other ports.
2149      */
2150     for (port = 0; port < ocelot->num_phys_ports; port++) {
2151         struct ocelot_port *ocelot_port = ocelot->ports[port];
2152         unsigned long mask;
2153
2154         if (!ocelot_port) {
2155             /* Unused ports can't send anywhere */
2156             mask = 0;
2157         } else if (ocelot_port->is_dsa_8021q_cpu) {
2158             /* The DSA tag_8021q CPU ports need to be able to
2159              * forward packets to all ports assigned to them.
2160              */
2161             mask = ocelot_dsa_8021q_cpu_assigned_ports(ocelot,
2162                                    ocelot_port);
2163         } else if (ocelot_port->bridge) {
2164             struct net_device *bond = ocelot_port->bond;
2165
2166             mask = ocelot_get_bridge_fwd_mask(ocelot, port);
2167             mask &= ~BIT(port);
2168
2169             mask |= ocelot_port_assigned_dsa_8021q_cpu_mask(ocelot,
2170                                     port);
2171
2172             if (bond)
2173                 mask &= ~ocelot_get_bond_mask(ocelot, bond);
2174         } else {
2175             /* Standalone ports forward only to DSA tag_8021q CPU
2176              * ports (if those exist), or to the hardware CPU port
2177              * module otherwise.
2178              */
2179             mask = ocelot_port_assigned_dsa_8021q_cpu_mask(ocelot,
2180                                        port);
2181         }
2182
2183         ocelot_write_rix(ocelot, mask, ANA_PGID_PGID, PGID_SRC + port);
2184     }
2185
2186     /* If cut-through forwarding is supported and a port is leaving, there
2187      * is a chance that cut-through was disabled on the other ports due to
2188      * the port which is leaving (it has a higher link speed). We need to
2189      * update the cut-through masks of the remaining ports no earlier than
2190      * after the port has left, to prevent underruns from happening between
2191      * the cut-through update and the forwarding domain update.
2192      */
2193     if (!joining && ocelot->ops->cut_through_fwd)
2194         ocelot->ops->cut_through_fwd(ocelot);
2195 }
2196
2197 /* Update PGID_CPU which is the destination port mask used for whitelisting
2198  * unicast addresses filtered towards the host. In the normal and NPI modes,
2199  * this points to the analyzer entry for the CPU port module, while in DSA
2200  * tag_8021q mode, it is a bit mask of all active CPU ports.
2201  * PGID_SRC will take care of forwarding a packet from one user port to
2202  * no more than a single CPU port.
2203  */
2204 static void ocelot_update_pgid_cpu(struct ocelot *ocelot)
2205 {
2206     int pgid_cpu = 0;
2207     int port;
2208
2209     for (port = 0; port < ocelot->num_phys_ports; port++) {
2210         struct ocelot_port *ocelot_port = ocelot->ports[port];
2211
2212         if (!ocelot_port || !ocelot_port->is_dsa_8021q_cpu)
2213             continue;
2214
2215         pgid_cpu |= BIT(port);
2216     }
2217
2218     if (!pgid_cpu)
2219         pgid_cpu = BIT(ocelot->num_phys_ports);
2220
2221     ocelot_write_rix(ocelot, pgid_cpu, ANA_PGID_PGID, PGID_CPU);
2222 }
2223
2224 void ocelot_port_assign_dsa_8021q_cpu(struct ocelot *ocelot, int port,
2225                       int cpu)
2226 {
2227     struct ocelot_port *cpu_port = ocelot->ports[cpu];
2228     u16 vid;
2229
2230     mutex_lock(&ocelot->fwd_domain_lock);
2231
2232     ocelot->ports[port]->dsa_8021q_cpu = cpu_port;
2233
2234     if (!cpu_port->is_dsa_8021q_cpu) {
2235         cpu_port->is_dsa_8021q_cpu = true;
2236
2237         for (vid = OCELOT_RSV_VLAN_RANGE_START; vid < VLAN_N_VID; vid++)
2238             ocelot_vlan_member_add(ocelot, cpu, vid, true);
2239
2240         ocelot_update_pgid_cpu(ocelot);
2241     }
2242
2243     ocelot_apply_bridge_fwd_mask(ocelot, true);
2244
2245     mutex_unlock(&ocelot->fwd_domain_lock);
2246 }
2247 EXPORT_SYMBOL_GPL(ocelot_port_assign_dsa_8021q_cpu);
2248
2249 void ocelot_port_unassign_dsa_8021q_cpu(struct ocelot *ocelot, int port)
2250 {
2251     struct ocelot_port *cpu_port = ocelot->ports[port]->dsa_8021q_cpu;
2252     bool keep = false;
2253     u16 vid;
2254     int p;
2255
2256     mutex_lock(&ocelot->fwd_domain_lock);
2257
2258     ocelot->ports[port]->dsa_8021q_cpu = NULL;
2259
2260     for (p = 0; p < ocelot->num_phys_ports; p++) {
2261         if (!ocelot->ports[p])
2262             continue;
2263
2264         if (ocelot->ports[p]->dsa_8021q_cpu == cpu_port) {
2265             keep = true;
2266             break;
2267         }
2268     }
2269
2270     if (!keep) {
2271         cpu_port->is_dsa_8021q_cpu = false;
2272
2273         for (vid = OCELOT_RSV_VLAN_RANGE_START; vid < VLAN_N_VID; vid++)
2274             ocelot_vlan_member_del(ocelot, cpu_port->index, vid);
2275
2276         ocelot_update_pgid_cpu(ocelot);
2277     }
2278
2279     ocelot_apply_bridge_fwd_mask(ocelot, true);
2280
2281     mutex_unlock(&ocelot->fwd_domain_lock);
2282 }
2283 EXPORT_SYMBOL_GPL(ocelot_port_unassign_dsa_8021q_cpu);
2284
2285 void ocelot_bridge_stp_state_set(struct ocelot *ocelot, int port, u8 state)
2286 {
2287     struct ocelot_port *ocelot_port = ocelot->ports[port];
2288     u32 learn_ena = 0;
2289
2290     mutex_lock(&ocelot->fwd_domain_lock);
2291
2292     ocelot_port->stp_state = state;
2293
2294     if ((state == BR_STATE_LEARNING || state == BR_STATE_FORWARDING) &&
2295         ocelot_port->learn_ena)
2296         learn_ena = ANA_PORT_PORT_CFG_LEARN_ENA;
2297
2298     ocelot_rmw_gix(ocelot, learn_ena, ANA_PORT_PORT_CFG_LEARN_ENA,
2299                ANA_PORT_PORT_CFG, port);
2300
2301     ocelot_apply_bridge_fwd_mask(ocelot, state == BR_STATE_FORWARDING);
2302
2303     mutex_unlock(&ocelot->fwd_domain_lock);
2304 }
2305 EXPORT_SYMBOL(ocelot_bridge_stp_state_set);
2306
2307 void ocelot_set_ageing_time(struct ocelot *ocelot, unsigned int msecs)
2308 {
2309     unsigned int age_period = ANA_AUTOAGE_AGE_PERIOD(msecs / 2000);
2310
2311     /* Setting AGE_PERIOD to zero effectively disables automatic aging,
2312      * which is clearly not what our intention is. So avoid that.
2313      */
2314     if (!age_period)
2315         age_period = 1;
2316
2317     ocelot_rmw(ocelot, age_period, ANA_AUTOAGE_AGE_PERIOD_M, ANA_AUTOAGE);
2318 }
2319 EXPORT_SYMBOL(ocelot_set_ageing_time);
2320
2321 static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
2322                              const unsigned char *addr,
2323                              u16 vid)
2324 {
2325     struct ocelot_multicast *mc;
2326
2327     list_for_each_entry(mc, &ocelot->multicast, list) {
2328         if (ether_addr_equal(mc->addr, addr) && mc->vid == vid)
2329             return mc;
2330     }
2331
2332     return NULL;
2333 }
2334
2335 static enum macaccess_entry_type ocelot_classify_mdb(const unsigned char *addr)
2336 {
2337     if (addr[0] == 0x01 && addr[1] == 0x00 && addr[2] == 0x5e)
2338         return ENTRYTYPE_MACv4;
2339     if (addr[0] == 0x33 && addr[1] == 0x33)
2340         return ENTRYTYPE_MACv6;
2341     return ENTRYTYPE_LOCKED;
2342 }
2343
2344 static struct ocelot_pgid *ocelot_pgid_alloc(struct ocelot *ocelot, int index,
2345                          unsigned long ports)
2346 {
2347     struct ocelot_pgid *pgid;
2348
2349     pgid = kzalloc(sizeof(*pgid), GFP_KERNEL);
2350     if (!pgid)
2351         return ERR_PTR(-ENOMEM);
2352
2353     pgid->ports = ports;
2354     pgid->index = index;
2355     refcount_set(&pgid->refcount, 1);
2356     list_add_tail(&pgid->list, &ocelot->pgids);
2357
2358     return pgid;
2359 }
2360
2361 static void ocelot_pgid_free(struct ocelot *ocelot, struct ocelot_pgid *pgid)
2362 {
2363     if (!refcount_dec_and_test(&pgid->refcount))
2364         return;
2365
2366     list_del(&pgid->list);
2367     kfree(pgid);
2368 }
2369
2370 static struct ocelot_pgid *ocelot_mdb_get_pgid(struct ocelot *ocelot,
2371                            const struct ocelot_multicast *mc)
2372 {
2373     struct ocelot_pgid *pgid;
2374     int index;
2375
2376     /* According to VSC7514 datasheet 3.9.1.5 IPv4 Multicast Entries and
2377      * 3.9.1.6 IPv6 Multicast Entries, "Instead of a lookup in the
2378      * destination mask table (PGID), the destination set is programmed as
2379      * part of the entry MAC address.", and the DEST_IDX is set to 0.
2380      */
2381     if (mc->entry_type == ENTRYTYPE_MACv4 ||
2382         mc->entry_type == ENTRYTYPE_MACv6)
2383         return ocelot_pgid_alloc(ocelot, 0, mc->ports);
2384
2385     list_for_each_entry(pgid, &ocelot->pgids, list) {
2386         /* When searching for a nonreserved multicast PGID, ignore the
2387          * dummy PGID of zero that we have for MACv4/MACv6 entries
2388          */
2389         if (pgid->index && pgid->ports == mc->ports) {
2390             refcount_inc(&pgid->refcount);
2391             return pgid;
2392         }
2393     }
2394
2395     /* Search for a free index in the nonreserved multicast PGID area */
2396     for_each_nonreserved_multicast_dest_pgid(ocelot, index) {
2397         bool used = false;
2398
2399         list_for_each_entry(pgid, &ocelot->pgids, list) {
2400             if (pgid->index == index) {
2401                 used = true;
2402                 break;
2403             }
2404         }
2405
2406         if (!used)
2407             return ocelot_pgid_alloc(ocelot, index, mc->ports);
2408     }
2409
2410     return ERR_PTR(-ENOSPC);
2411 }
2412
2413 static void ocelot_encode_ports_to_mdb(unsigned char *addr,
2414                        struct ocelot_multicast *mc)
2415 {
2416     ether_addr_copy(addr, mc->addr);
2417
2418     if (mc->entry_type == ENTRYTYPE_MACv4) {
2419         addr[0] = 0;
2420         addr[1] = mc->ports >> 8;
2421         addr[2] = mc->ports & 0xff;
2422     } else if (mc->entry_type == ENTRYTYPE_MACv6) {
2423         addr[0] = mc->ports >> 8;
2424         addr[1] = mc->ports & 0xff;
2425     }
2426 }
2427
2428 int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
2429             const struct switchdev_obj_port_mdb *mdb,
2430             const struct net_device *bridge)
2431 {
2432     unsigned char addr[ETH_ALEN];
2433     struct ocelot_multicast *mc;
2434     struct ocelot_pgid *pgid;
2435     u16 vid = mdb->vid;
2436
2437     if (!vid)
2438         vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2439
2440     mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
2441     if (!mc) {
2442         /* New entry */
2443         mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
2444         if (!mc)
2445             return -ENOMEM;
2446
2447         mc->entry_type = ocelot_classify_mdb(mdb->addr);
2448         ether_addr_copy(mc->addr, mdb->addr);
2449         mc->vid = vid;
2450
2451         list_add_tail(&mc->list, &ocelot->multicast);
2452     } else {
2453         /* Existing entry. Clean up the current port mask from
2454          * hardware now, because we'll be modifying it.
2455          */
2456         ocelot_pgid_free(ocelot, mc->pgid);
2457         ocelot_encode_ports_to_mdb(addr, mc);
2458         ocelot_mact_forget(ocelot, addr, vid);
2459     }
2460
2461     mc->ports |= BIT(port);
2462
2463     pgid = ocelot_mdb_get_pgid(ocelot, mc);
2464     if (IS_ERR(pgid)) {
2465         dev_err(ocelot->dev,
2466             "Cannot allocate PGID for mdb %pM vid %d\n",
2467             mc->addr, mc->vid);
2468         devm_kfree(ocelot->dev, mc);
2469         return PTR_ERR(pgid);
2470     }
2471     mc->pgid = pgid;
2472
2473     ocelot_encode_ports_to_mdb(addr, mc);
2474
2475     if (mc->entry_type != ENTRYTYPE_MACv4 &&
2476         mc->entry_type != ENTRYTYPE_MACv6)
2477         ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
2478                  pgid->index);
2479
2480     return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
2481                  mc->entry_type);
2482 }
2483 EXPORT_SYMBOL(ocelot_port_mdb_add);
2484
2485 int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
2486             const struct switchdev_obj_port_mdb *mdb,
2487             const struct net_device *bridge)
2488 {
2489     unsigned char addr[ETH_ALEN];
2490     struct ocelot_multicast *mc;
2491     struct ocelot_pgid *pgid;
2492     u16 vid = mdb->vid;
2493
2494     if (!vid)
2495         vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2496
2497     mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
2498     if (!mc)
2499         return -ENOENT;
2500
2501     ocelot_encode_ports_to_mdb(addr, mc);
2502     ocelot_mact_forget(ocelot, addr, vid);
2503
2504     ocelot_pgid_free(ocelot, mc->pgid);
2505     mc->ports &= ~BIT(port);
2506     if (!mc->ports) {
2507         list_del(&mc->list);
2508         devm_kfree(ocelot->dev, mc);
2509         return 0;
2510     }
2511
2512     /* We have a PGID with fewer ports now */
2513     pgid = ocelot_mdb_get_pgid(ocelot, mc);
2514     if (IS_ERR(pgid))
2515         return PTR_ERR(pgid);
2516     mc->pgid = pgid;
2517
2518     ocelot_encode_ports_to_mdb(addr, mc);
2519
2520     if (mc->entry_type != ENTRYTYPE_MACv4 &&
2521         mc->entry_type != ENTRYTYPE_MACv6)
2522         ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
2523                  pgid->index);
2524
2525     return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
2526                  mc->entry_type);
2527 }
2528 EXPORT_SYMBOL(ocelot_port_mdb_del);
2529
2530 int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
2531                 struct net_device *bridge, int bridge_num,
2532                 struct netlink_ext_ack *extack)
2533 {
2534     struct ocelot_port *ocelot_port = ocelot->ports[port];
2535     int err;
2536
2537     err = ocelot_single_vlan_aware_bridge(ocelot, extack);
2538     if (err)
2539         return err;
2540
2541     mutex_lock(&ocelot->fwd_domain_lock);
2542
2543     ocelot_port->bridge = bridge;
2544     ocelot_port->bridge_num = bridge_num;
2545
2546     ocelot_apply_bridge_fwd_mask(ocelot, true);
2547
2548     mutex_unlock(&ocelot->fwd_domain_lock);
2549
2550     if (br_vlan_enabled(bridge))
2551         return 0;
2552
2553     return ocelot_add_vlan_unaware_pvid(ocelot, port, bridge);
2554 }
2555 EXPORT_SYMBOL(ocelot_port_bridge_join);
2556
2557 void ocelot_port_bridge_leave(struct ocelot *ocelot, int port,
2558                   struct net_device *bridge)
2559 {
2560     struct ocelot_port *ocelot_port = ocelot->ports[port];
2561
2562     mutex_lock(&ocelot->fwd_domain_lock);
2563
2564     if (!br_vlan_enabled(bridge))
2565         ocelot_del_vlan_unaware_pvid(ocelot, port, bridge);
2566
2567     ocelot_port->bridge = NULL;
2568     ocelot_port->bridge_num = -1;
2569
2570     ocelot_port_set_pvid(ocelot, port, NULL);
2571     ocelot_port_manage_port_tag(ocelot, port);
2572     ocelot_apply_bridge_fwd_mask(ocelot, false);
2573
2574     mutex_unlock(&ocelot->fwd_domain_lock);
2575 }
2576 EXPORT_SYMBOL(ocelot_port_bridge_leave);
2577
2578 static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
2579 {
2580     unsigned long visited = GENMASK(ocelot->num_phys_ports - 1, 0);
2581     int i, port, lag;
2582
2583     /* Reset destination and aggregation PGIDS */
2584     for_each_unicast_dest_pgid(ocelot, port)
2585         ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
2586
2587     for_each_aggr_pgid(ocelot, i)
2588         ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
2589                  ANA_PGID_PGID, i);
2590
2591     /* The visited ports bitmask holds the list of ports offloading any
2592      * bonding interface. Initially we mark all these ports as unvisited,
2593      * then every time we visit a port in this bitmask, we know that it is
2594      * the lowest numbered port, i.e. the one whose logical ID == physical
2595      * port ID == LAG ID. So we mark as visited all further ports in the
2596      * bitmask that are offloading the same bonding interface. This way,
2597      * we set up the aggregation PGIDs only once per bonding interface.
2598      */
2599     for (port = 0; port < ocelot->num_phys_ports; port++) {
2600         struct ocelot_port *ocelot_port = ocelot->ports[port];
2601
2602         if (!ocelot_port || !ocelot_port->bond)
2603             continue;
2604
2605         visited &= ~BIT(port);
2606     }
2607
2608     /* Now, set PGIDs for each active LAG */
2609     for (lag = 0; lag < ocelot->num_phys_ports; lag++) {
2610         struct net_device *bond = ocelot->ports[lag]->bond;
2611         int num_active_ports = 0;
2612         unsigned long bond_mask;
2613         u8 aggr_idx[16];
2614
2615         if (!bond || (visited & BIT(lag)))
2616             continue;
2617
2618         bond_mask = ocelot_get_bond_mask(ocelot, bond);
2619
2620         for_each_set_bit(port, &bond_mask, ocelot->num_phys_ports) {
2621             struct ocelot_port *ocelot_port = ocelot->ports[port];
2622
2623             // Destination mask
2624             ocelot_write_rix(ocelot, bond_mask,
2625                      ANA_PGID_PGID, port);
2626
2627             if (ocelot_port->lag_tx_active)
2628                 aggr_idx[num_active_ports++] = port;
2629         }
2630
2631         for_each_aggr_pgid(ocelot, i) {
2632             u32 ac;
2633
2634             ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
2635             ac &= ~bond_mask;
2636             /* Don't do division by zero if there was no active
2637              * port. Just make all aggregation codes zero.
2638              */
2639             if (num_active_ports)
2640                 ac |= BIT(aggr_idx[i % num_active_ports]);
2641             ocelot_write_rix(ocelot, ac, ANA_PGID_PGID, i);
2642         }
2643
2644         /* Mark all ports in the same LAG as visited to avoid applying
2645          * the same config again.
2646          */
2647         for (port = lag; port < ocelot->num_phys_ports; port++) {
2648             struct ocelot_port *ocelot_port = ocelot->ports[port];
2649
2650             if (!ocelot_port)
2651                 continue;
2652
2653             if (ocelot_port->bond == bond)
2654                 visited |= BIT(port);
2655         }
2656     }
2657 }
2658
2659 /* When offloading a bonding interface, the switch ports configured under the
2660  * same bond must have the same logical port ID, equal to the physical port ID
2661  * of the lowest numbered physical port in that bond. Otherwise, in standalone/
2662  * bridged mode, each port has a logical port ID equal to its physical port ID.
2663  */
2664 static void ocelot_setup_logical_port_ids(struct ocelot *ocelot)
2665 {
2666     int port;
2667
2668     for (port = 0; port < ocelot->num_phys_ports; port++) {
2669         struct ocelot_port *ocelot_port = ocelot->ports[port];
2670         struct net_device *bond;
2671
2672         if (!ocelot_port)
2673             continue;
2674
2675         bond = ocelot_port->bond;
2676         if (bond) {
2677             int lag = ocelot_bond_get_id(ocelot, bond);
2678
2679             ocelot_rmw_gix(ocelot,
2680                        ANA_PORT_PORT_CFG_PORTID_VAL(lag),
2681                        ANA_PORT_PORT_CFG_PORTID_VAL_M,
2682                        ANA_PORT_PORT_CFG, port);
2683         } else {
2684             ocelot_rmw_gix(ocelot,
2685                        ANA_PORT_PORT_CFG_PORTID_VAL(port),
2686                        ANA_PORT_PORT_CFG_PORTID_VAL_M,
2687                        ANA_PORT_PORT_CFG, port);
2688         }
2689     }
2690 }
2691
2692 static int ocelot_migrate_mc(struct ocelot *ocelot, struct ocelot_multicast *mc,
2693                  unsigned long from_mask, unsigned long to_mask)
2694 {
2695     unsigned char addr[ETH_ALEN];
2696     struct ocelot_pgid *pgid;
2697     u16 vid = mc->vid;
2698
2699     dev_dbg(ocelot->dev,
2700         "Migrating multicast %pM vid %d from port mask 0x%lx to 0x%lx\n",
2701         mc->addr, mc->vid, from_mask, to_mask);
2702
2703     /* First clean up the current port mask from hardware, because
2704      * we'll be modifying it.
2705      */
2706     ocelot_pgid_free(ocelot, mc->pgid);
2707     ocelot_encode_ports_to_mdb(addr, mc);
2708     ocelot_mact_forget(ocelot, addr, vid);
2709
2710     mc->ports &= ~from_mask;
2711     mc->ports |= to_mask;
2712
2713     pgid = ocelot_mdb_get_pgid(ocelot, mc);
2714     if (IS_ERR(pgid)) {
2715         dev_err(ocelot->dev,
2716             "Cannot allocate PGID for mdb %pM vid %d\n",
2717             mc->addr, mc->vid);
2718         devm_kfree(ocelot->dev, mc);
2719         return PTR_ERR(pgid);
2720     }
2721     mc->pgid = pgid;
2722
2723     ocelot_encode_ports_to_mdb(addr, mc);
2724
2725     if (mc->entry_type != ENTRYTYPE_MACv4 &&
2726         mc->entry_type != ENTRYTYPE_MACv6)
2727         ocelot_write_rix(ocelot, pgid->ports, ANA_PGID_PGID,
2728                  pgid->index);
2729
2730     return ocelot_mact_learn(ocelot, pgid->index, addr, vid,
2731                  mc->entry_type);
2732 }
2733
2734 int ocelot_migrate_mdbs(struct ocelot *ocelot, unsigned long from_mask,
2735             unsigned long to_mask)
2736 {
2737     struct ocelot_multicast *mc;
2738     int err;
2739
2740     list_for_each_entry(mc, &ocelot->multicast, list) {
2741         if (!(mc->ports & from_mask))
2742             continue;
2743
2744         err = ocelot_migrate_mc(ocelot, mc, from_mask, to_mask);
2745         if (err)
2746             return err;
2747     }
2748
2749     return 0;
2750 }
2751 EXPORT_SYMBOL_GPL(ocelot_migrate_mdbs);
2752
2753 /* Documentation for PORTID_VAL says:
2754  *     Logical port number for front port. If port is not a member of a LLAG,
2755  *     then PORTID must be set to the physical port number.
2756  *     If port is a member of a LLAG, then PORTID must be set to the common
2757  *     PORTID_VAL used for all member ports of the LLAG.
2758  *     The value must not exceed the number of physical ports on the device.
2759  *
2760  * This means we have little choice but to migrate FDB entries pointing towards
2761  * a logical port when that changes.
2762  */
2763 static void ocelot_migrate_lag_fdbs(struct ocelot *ocelot,
2764                     struct net_device *bond,
2765                     int lag)
2766 {
2767     struct ocelot_lag_fdb *fdb;
2768     int err;
2769
2770     lockdep_assert_held(&ocelot->fwd_domain_lock);
2771
2772     list_for_each_entry(fdb, &ocelot->lag_fdbs, list) {
2773         if (fdb->bond != bond)
2774             continue;
2775
2776         err = ocelot_mact_forget(ocelot, fdb->addr, fdb->vid);
2777         if (err) {
2778             dev_err(ocelot->dev,
2779                 "failed to delete LAG %s FDB %pM vid %d: %pe\n",
2780                 bond->name, fdb->addr, fdb->vid, ERR_PTR(err));
2781         }
2782
2783         err = ocelot_mact_learn(ocelot, lag, fdb->addr, fdb->vid,
2784                     ENTRYTYPE_LOCKED);
2785         if (err) {
2786             dev_err(ocelot->dev,
2787                 "failed to migrate LAG %s FDB %pM vid %d: %pe\n",
2788                 bond->name, fdb->addr, fdb->vid, ERR_PTR(err));
2789         }
2790     }
2791 }
2792
2793 int ocelot_port_lag_join(struct ocelot *ocelot, int port,
2794              struct net_device *bond,
2795              struct netdev_lag_upper_info *info)
2796 {
2797     if (info->tx_type != NETDEV_LAG_TX_TYPE_HASH)
2798         return -EOPNOTSUPP;
2799
2800     mutex_lock(&ocelot->fwd_domain_lock);
2801
2802     ocelot->ports[port]->bond = bond;
2803
2804     ocelot_setup_logical_port_ids(ocelot);
2805     ocelot_apply_bridge_fwd_mask(ocelot, true);
2806     ocelot_set_aggr_pgids(ocelot);
2807
2808     mutex_unlock(&ocelot->fwd_domain_lock);
2809
2810     return 0;
2811 }
2812 EXPORT_SYMBOL(ocelot_port_lag_join);
2813
2814 void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
2815                struct net_device *bond)
2816 {
2817     int old_lag_id, new_lag_id;
2818
2819     mutex_lock(&ocelot->fwd_domain_lock);
2820
2821     old_lag_id = ocelot_bond_get_id(ocelot, bond);
2822
2823     ocelot->ports[port]->bond = NULL;
2824
2825     ocelot_setup_logical_port_ids(ocelot);
2826     ocelot_apply_bridge_fwd_mask(ocelot, false);
2827     ocelot_set_aggr_pgids(ocelot);
2828
2829     new_lag_id = ocelot_bond_get_id(ocelot, bond);
2830
2831     if (new_lag_id >= 0 && old_lag_id != new_lag_id)
2832         ocelot_migrate_lag_fdbs(ocelot, bond, new_lag_id);
2833
2834     mutex_unlock(&ocelot->fwd_domain_lock);
2835 }
2836 EXPORT_SYMBOL(ocelot_port_lag_leave);
2837
2838 void ocelot_port_lag_change(struct ocelot *ocelot, int port, bool lag_tx_active)
2839 {
2840     struct ocelot_port *ocelot_port = ocelot->ports[port];
2841
2842     mutex_lock(&ocelot->fwd_domain_lock);
2843
2844     ocelot_port->lag_tx_active = lag_tx_active;
2845
2846     /* Rebalance the LAGs */
2847     ocelot_set_aggr_pgids(ocelot);
2848
2849     mutex_unlock(&ocelot->fwd_domain_lock);
2850 }
2851 EXPORT_SYMBOL(ocelot_port_lag_change);
2852
2853 int ocelot_lag_fdb_add(struct ocelot *ocelot, struct net_device *bond,
2854                const unsigned char *addr, u16 vid,
2855                const struct net_device *bridge)
2856 {
2857     struct ocelot_lag_fdb *fdb;
2858     int lag, err;
2859
2860     fdb = kzalloc(sizeof(*fdb), GFP_KERNEL);
2861     if (!fdb)
2862         return -ENOMEM;
2863
2864     mutex_lock(&ocelot->fwd_domain_lock);
2865
2866     if (!vid)
2867         vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2868
2869     ether_addr_copy(fdb->addr, addr);
2870     fdb->vid = vid;
2871     fdb->bond = bond;
2872
2873     lag = ocelot_bond_get_id(ocelot, bond);
2874
2875     err = ocelot_mact_learn(ocelot, lag, addr, vid, ENTRYTYPE_LOCKED);
2876     if (err) {
2877         mutex_unlock(&ocelot->fwd_domain_lock);
2878         kfree(fdb);
2879         return err;
2880     }
2881
2882     list_add_tail(&fdb->list, &ocelot->lag_fdbs);
2883     mutex_unlock(&ocelot->fwd_domain_lock);
2884
2885     return 0;
2886 }
2887 EXPORT_SYMBOL_GPL(ocelot_lag_fdb_add);
2888
2889 int ocelot_lag_fdb_del(struct ocelot *ocelot, struct net_device *bond,
2890                const unsigned char *addr, u16 vid,
2891                const struct net_device *bridge)
2892 {
2893     struct ocelot_lag_fdb *fdb, *tmp;
2894
2895     mutex_lock(&ocelot->fwd_domain_lock);
2896
2897     if (!vid)
2898         vid = ocelot_vlan_unaware_pvid(ocelot, bridge);
2899
2900     list_for_each_entry_safe(fdb, tmp, &ocelot->lag_fdbs, list) {
2901         if (!ether_addr_equal(fdb->addr, addr) || fdb->vid != vid ||
2902             fdb->bond != bond)
2903             continue;
2904
2905         ocelot_mact_forget(ocelot, addr, vid);
2906         list_del(&fdb->list);
2907         mutex_unlock(&ocelot->fwd_domain_lock);
2908         kfree(fdb);
2909
2910         return 0;
2911     }
2912
2913     mutex_unlock(&ocelot->fwd_domain_lock);
2914
2915     return -ENOENT;
2916 }
2917 EXPORT_SYMBOL_GPL(ocelot_lag_fdb_del);
2918
2919 /* Configure the maximum SDU (L2 payload) on RX to the value specified in @sdu.
2920  * The length of VLAN tags is accounted for automatically via DEV_MAC_TAGS_CFG.
2921  * In the special case that it's the NPI port that we're configuring, the
2922  * length of the tag and optional prefix needs to be accounted for privately,
2923  * in order to be able to sustain communication at the requested @sdu.
2924  */
2925 void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
2926 {
2927     struct ocelot_port *ocelot_port = ocelot->ports[port];
2928     int maxlen = sdu + ETH_HLEN + ETH_FCS_LEN;
2929     int pause_start, pause_stop;
2930     int atop, atop_tot;
2931
2932     if (port == ocelot->npi) {
2933         maxlen += OCELOT_TAG_LEN;
2934
2935         if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_SHORT)
2936             maxlen += OCELOT_SHORT_PREFIX_LEN;
2937         else if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_LONG)
2938             maxlen += OCELOT_LONG_PREFIX_LEN;
2939     }
2940
2941     ocelot_port_writel(ocelot_port, maxlen, DEV_MAC_MAXLEN_CFG);
2942
2943     /* Set Pause watermark hysteresis */
2944     pause_start = 6 * maxlen / OCELOT_BUFFER_CELL_SZ;
2945     pause_stop = 4 * maxlen / OCELOT_BUFFER_CELL_SZ;
2946     ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_START,
2947                 pause_start);
2948     ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_STOP,
2949                 pause_stop);
2950
2951     /* Tail dropping watermarks */
2952     atop_tot = (ocelot->packet_buffer_size - 9 * maxlen) /
2953            OCELOT_BUFFER_CELL_SZ;
2954     atop = (9 * maxlen) / OCELOT_BUFFER_CELL_SZ;
2955     ocelot_write_rix(ocelot, ocelot->ops->wm_enc(atop), SYS_ATOP, port);
2956     ocelot_write(ocelot, ocelot->ops->wm_enc(atop_tot), SYS_ATOP_TOT_CFG);
2957 }
2958 EXPORT_SYMBOL(ocelot_port_set_maxlen);
2959
2960 int ocelot_get_max_mtu(struct ocelot *ocelot, int port)
2961 {
2962     int max_mtu = 65535 - ETH_HLEN - ETH_FCS_LEN;
2963
2964     if (port == ocelot->npi) {
2965         max_mtu -= OCELOT_TAG_LEN;
2966
2967         if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_SHORT)
2968             max_mtu -= OCELOT_SHORT_PREFIX_LEN;
2969         else if (ocelot->npi_inj_prefix == OCELOT_TAG_PREFIX_LONG)
2970             max_mtu -= OCELOT_LONG_PREFIX_LEN;
2971     }
2972
2973     return max_mtu;
2974 }
2975 EXPORT_SYMBOL(ocelot_get_max_mtu);
2976
2977 static void ocelot_port_set_learning(struct ocelot *ocelot, int port,
2978                      bool enabled)
2979 {
2980     struct ocelot_port *ocelot_port = ocelot->ports[port];
2981     u32 val = 0;
2982
2983     if (enabled)
2984         val = ANA_PORT_PORT_CFG_LEARN_ENA;
2985
2986     ocelot_rmw_gix(ocelot, val, ANA_PORT_PORT_CFG_LEARN_ENA,
2987                ANA_PORT_PORT_CFG, port);
2988
2989     ocelot_port->learn_ena = enabled;
2990 }
2991
2992 static void ocelot_port_set_ucast_flood(struct ocelot *ocelot, int port,
2993                     bool enabled)
2994 {
2995     u32 val = 0;
2996
2997     if (enabled)
2998         val = BIT(port);
2999
3000     ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_UC);
3001 }
3002
3003 static void ocelot_port_set_mcast_flood(struct ocelot *ocelot, int port,
3004                     bool enabled)
3005 {
3006     u32 val = 0;
3007
3008     if (enabled)
3009         val = BIT(port);
3010
3011     ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MC);
3012     ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MCIPV4);
3013     ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_MCIPV6);
3014 }
3015
3016 static void ocelot_port_set_bcast_flood(struct ocelot *ocelot, int port,
3017                     bool enabled)
3018 {
3019     u32 val = 0;
3020
3021     if (enabled)
3022         val = BIT(port);
3023
3024     ocelot_rmw_rix(ocelot, val, BIT(port), ANA_PGID_PGID, PGID_BC);
3025 }
3026
3027 int ocelot_port_pre_bridge_flags(struct ocelot *ocelot, int port,
3028                  struct switchdev_brport_flags flags)
3029 {
3030     if (flags.mask & ~(BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD |
3031                BR_BCAST_FLOOD))
3032         return -EINVAL;
3033
3034     return 0;
3035 }
3036 EXPORT_SYMBOL(ocelot_port_pre_bridge_flags);
3037
3038 void ocelot_port_bridge_flags(struct ocelot *ocelot, int port,
3039                   struct switchdev_brport_flags flags)
3040 {
3041     if (flags.mask & BR_LEARNING)
3042         ocelot_port_set_learning(ocelot, port,
3043                      !!(flags.val & BR_LEARNING));
3044
3045     if (flags.mask & BR_FLOOD)
3046         ocelot_port_set_ucast_flood(ocelot, port,
3047                         !!(flags.val & BR_FLOOD));
3048
3049     if (flags.mask & BR_MCAST_FLOOD)
3050         ocelot_port_set_mcast_flood(ocelot, port,
3051                         !!(flags.val & BR_MCAST_FLOOD));
3052
3053     if (flags.mask & BR_BCAST_FLOOD)
3054         ocelot_port_set_bcast_flood(ocelot, port,
3055                         !!(flags.val & BR_BCAST_FLOOD));
3056 }
3057 EXPORT_SYMBOL(ocelot_port_bridge_flags);
3058
3059 int ocelot_port_get_default_prio(struct ocelot *ocelot, int port)
3060 {
3061     int val = ocelot_read_gix(ocelot, ANA_PORT_QOS_CFG, port);
3062
3063     return ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL_X(val);
3064 }
3065 EXPORT_SYMBOL_GPL(ocelot_port_get_default_prio);
3066
3067 int ocelot_port_set_default_prio(struct ocelot *ocelot, int port, u8 prio)
3068 {
3069     if (prio >= OCELOT_NUM_TC)
3070         return -ERANGE;
3071
3072     ocelot_rmw_gix(ocelot,
3073                ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL(prio),
3074                ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL_M,
3075                ANA_PORT_QOS_CFG,
3076                port);
3077
3078     return 0;
3079 }
3080 EXPORT_SYMBOL_GPL(ocelot_port_set_default_prio);
3081
3082 int ocelot_port_get_dscp_prio(struct ocelot *ocelot, int port, u8 dscp)
3083 {
3084     int qos_cfg = ocelot_read_gix(ocelot, ANA_PORT_QOS_CFG, port);
3085     int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp);
3086
3087     /* Return error if DSCP prioritization isn't enabled */
3088     if (!(qos_cfg & ANA_PORT_QOS_CFG_QOS_DSCP_ENA))
3089         return -EOPNOTSUPP;
3090
3091     if (qos_cfg & ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA) {
3092         dscp = ANA_DSCP_CFG_DSCP_TRANSLATE_VAL_X(dscp_cfg);
3093         /* Re-read ANA_DSCP_CFG for the translated DSCP */
3094         dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp);
3095     }
3096
3097     /* If the DSCP value is not trusted, the QoS classification falls back
3098      * to VLAN PCP or port-based default.
3099      */
3100     if (!(dscp_cfg & ANA_DSCP_CFG_DSCP_TRUST_ENA))
3101         return -EOPNOTSUPP;
3102
3103     return ANA_DSCP_CFG_QOS_DSCP_VAL_X(dscp_cfg);
3104 }
3105 EXPORT_SYMBOL_GPL(ocelot_port_get_dscp_prio);
3106
3107 int ocelot_port_add_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio)
3108 {
3109     int mask, val;
3110
3111     if (prio >= OCELOT_NUM_TC)
3112         return -ERANGE;
3113
3114     /* There is at least one app table priority (this one), so we need to
3115      * make sure DSCP prioritization is enabled on the port.
3116      * Also make sure DSCP translation is disabled
3117      * (dcbnl doesn't support it).
3118      */
3119     mask = ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
3120            ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA;
3121
3122     ocelot_rmw_gix(ocelot, ANA_PORT_QOS_CFG_QOS_DSCP_ENA, mask,
3123                ANA_PORT_QOS_CFG, port);
3124
3125     /* Trust this DSCP value and map it to the given QoS class */
3126     val = ANA_DSCP_CFG_DSCP_TRUST_ENA | ANA_DSCP_CFG_QOS_DSCP_VAL(prio);
3127
3128     ocelot_write_rix(ocelot, val, ANA_DSCP_CFG, dscp);
3129
3130     return 0;
3131 }
3132 EXPORT_SYMBOL_GPL(ocelot_port_add_dscp_prio);
3133
3134 int ocelot_port_del_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio)
3135 {
3136     int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp);
3137     int mask, i;
3138
3139     /* During a "dcb app replace" command, the new app table entry will be
3140      * added first, then the old one will be deleted. But the hardware only
3141      * supports one QoS class per DSCP value (duh), so if we blindly delete
3142      * the app table entry for this DSCP value, we end up deleting the
3143      * entry with the new priority. Avoid that by checking whether user
3144      * space wants to delete the priority which is currently configured, or
3145      * something else which is no longer current.
3146      */
3147     if (ANA_DSCP_CFG_QOS_DSCP_VAL_X(dscp_cfg) != prio)
3148         return 0;
3149
3150     /* Untrust this DSCP value */
3151     ocelot_write_rix(ocelot, 0, ANA_DSCP_CFG, dscp);
3152
3153     for (i = 0; i < 64; i++) {
3154         int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, i);
3155
3156         /* There are still app table entries on the port, so we need to
3157          * keep DSCP enabled, nothing to do.
3158          */
3159         if (dscp_cfg & ANA_DSCP_CFG_DSCP_TRUST_ENA)
3160             return 0;
3161     }
3162
3163     /* Disable DSCP QoS classification if there isn't any trusted
3164      * DSCP value left.
3165      */
3166     mask = ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
3167            ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA;
3168
3169     ocelot_rmw_gix(ocelot, 0, mask, ANA_PORT_QOS_CFG, port);
3170
3171     return 0;
3172 }
3173 EXPORT_SYMBOL_GPL(ocelot_port_del_dscp_prio);
3174
3175 struct ocelot_mirror *ocelot_mirror_get(struct ocelot *ocelot, int to,
3176                     struct netlink_ext_ack *extack)
3177 {
3178     struct ocelot_mirror *m = ocelot->mirror;
3179
3180     if (m) {
3181         if (m->to != to) {
3182             NL_SET_ERR_MSG_MOD(extack,
3183                        "Mirroring already configured towards different egress port");
3184             return ERR_PTR(-EBUSY);
3185         }
3186
3187         refcount_inc(&m->refcount);
3188         return m;
3189     }
3190
3191     m = kzalloc(sizeof(*m), GFP_KERNEL);
3192     if (!m)
3193         return ERR_PTR(-ENOMEM);
3194
3195     m->to = to;
3196     refcount_set(&m->refcount, 1);
3197     ocelot->mirror = m;
3198
3199     /* Program the mirror port to hardware */
3200     ocelot_write(ocelot, BIT(to), ANA_MIRRORPORTS);
3201
3202     return m;
3203 }
3204
3205 void ocelot_mirror_put(struct ocelot *ocelot)
3206 {
3207     struct ocelot_mirror *m = ocelot->mirror;
3208
3209     if (!refcount_dec_and_test(&m->refcount))
3210         return;
3211
3212     ocelot_write(ocelot, 0, ANA_MIRRORPORTS);
3213     ocelot->mirror = NULL;
3214     kfree(m);
3215 }
3216
3217 int ocelot_port_mirror_add(struct ocelot *ocelot, int from, int to,
3218                bool ingress, struct netlink_ext_ack *extack)
3219 {
3220     struct ocelot_mirror *m = ocelot_mirror_get(ocelot, to, extack);
3221
3222     if (IS_ERR(m))
3223         return PTR_ERR(m);
3224
3225     if (ingress) {
3226         ocelot_rmw_gix(ocelot, ANA_PORT_PORT_CFG_SRC_MIRROR_ENA,
3227                    ANA_PORT_PORT_CFG_SRC_MIRROR_ENA,
3228                    ANA_PORT_PORT_CFG, from);
3229     } else {
3230         ocelot_rmw(ocelot, BIT(from), BIT(from),
3231                ANA_EMIRRORPORTS);
3232     }
3233
3234     return 0;
3235 }
3236 EXPORT_SYMBOL_GPL(ocelot_port_mirror_add);
3237
3238 void ocelot_port_mirror_del(struct ocelot *ocelot, int from, bool ingress)
3239 {
3240     if (ingress) {
3241         ocelot_rmw_gix(ocelot, 0, ANA_PORT_PORT_CFG_SRC_MIRROR_ENA,
3242                    ANA_PORT_PORT_CFG, from);
3243     } else {
3244         ocelot_rmw(ocelot, 0, BIT(from), ANA_EMIRRORPORTS);
3245     }
3246
3247     ocelot_mirror_put(ocelot);
3248 }
3249 EXPORT_SYMBOL_GPL(ocelot_port_mirror_del);
3250
3251 void ocelot_init_port(struct ocelot *ocelot, int port)
3252 {
3253     struct ocelot_port *ocelot_port = ocelot->ports[port];
3254
3255     skb_queue_head_init(&ocelot_port->tx_skbs);
3256
3257     /* Basic L2 initialization */
3258
3259     /* Set MAC IFG Gaps
3260      * FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 0
3261      * !FDX: TX_IFG = 5, RX_IFG1 = RX_IFG2 = 5
3262      */
3263     ocelot_port_writel(ocelot_port, DEV_MAC_IFG_CFG_TX_IFG(5),
3264                DEV_MAC_IFG_CFG);
3265
3266     /* Load seed (0) and set MAC HDX late collision  */
3267     ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67) |
3268                DEV_MAC_HDX_CFG_SEED_LOAD,
3269                DEV_MAC_HDX_CFG);
3270     mdelay(1);
3271     ocelot_port_writel(ocelot_port, DEV_MAC_HDX_CFG_LATE_COL_POS(67),
3272                DEV_MAC_HDX_CFG);
3273
3274     /* Set Max Length and maximum tags allowed */
3275     ocelot_port_set_maxlen(ocelot, port, ETH_DATA_LEN);
3276     ocelot_port_writel(ocelot_port, DEV_MAC_TAGS_CFG_TAG_ID(ETH_P_8021AD) |
3277                DEV_MAC_TAGS_CFG_VLAN_AWR_ENA |
3278                DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA |
3279                DEV_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA,
3280                DEV_MAC_TAGS_CFG);
3281
3282     /* Set SMAC of Pause frame (00:00:00:00:00:00) */
3283     ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
3284     ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
3285
3286     /* Enable transmission of pause frames */
3287     ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
3288
3289     /* Drop frames with multicast source address */
3290     ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
3291                ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
3292                ANA_PORT_DROP_CFG, port);
3293
3294     /* Set default VLAN and tag type to 8021Q. */
3295     ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q),
3296                REW_PORT_VLAN_CFG_PORT_TPID_M,
3297                REW_PORT_VLAN_CFG, port);
3298
3299     /* Disable source address learning for standalone mode */
3300     ocelot_port_set_learning(ocelot, port, false);
3301
3302     /* Set the port's initial logical port ID value, enable receiving
3303      * frames on it, and configure the MAC address learning type to
3304      * automatic.
3305      */
3306     ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_LEARNAUTO |
3307              ANA_PORT_PORT_CFG_RECV_ENA |
3308              ANA_PORT_PORT_CFG_PORTID_VAL(port),
3309              ANA_PORT_PORT_CFG, port);
3310
3311     /* Enable vcap lookups */
3312     ocelot_vcap_enable(ocelot, port);
3313 }
3314 EXPORT_SYMBOL(ocelot_init_port);
3315
3316 /* Configure and enable the CPU port module, which is a set of queues
3317  * accessible through register MMIO, frame DMA or Ethernet (in case
3318  * NPI mode is used).
3319  */
3320 static void ocelot_cpu_port_init(struct ocelot *ocelot)
3321 {
3322     int cpu = ocelot->num_phys_ports;
3323
3324     /* The unicast destination PGID for the CPU port module is unused */
3325     ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, cpu);
3326     /* Instead set up a multicast destination PGID for traffic copied to
3327      * the CPU. Whitelisted MAC addresses like the port netdevice MAC
3328      * addresses will be copied to the CPU via this PGID.
3329      */
3330     ocelot_write_rix(ocelot, BIT(cpu), ANA_PGID_PGID, PGID_CPU);
3331     ocelot_write_gix(ocelot, ANA_PORT_PORT_CFG_RECV_ENA |
3332              ANA_PORT_PORT_CFG_PORTID_VAL(cpu),
3333              ANA_PORT_PORT_CFG, cpu);
3334
3335     /* Enable CPU port module */
3336     ocelot_fields_write(ocelot, cpu, QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
3337     /* CPU port Injection/Extraction configuration */
3338     ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_XTR_HDR,
3339                 OCELOT_TAG_PREFIX_NONE);
3340     ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_INJ_HDR,
3341                 OCELOT_TAG_PREFIX_NONE);
3342
3343     /* Configure the CPU port to be VLAN aware */
3344     ocelot_write_gix(ocelot,
3345              ANA_PORT_VLAN_CFG_VLAN_VID(OCELOT_STANDALONE_PVID) |
3346              ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
3347              ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1),
3348              ANA_PORT_VLAN_CFG, cpu);
3349 }
3350
3351 static void ocelot_detect_features(struct ocelot *ocelot)
3352 {
3353     int mmgt, eq_ctrl;
3354
3355     /* For Ocelot, Felix, Seville, Serval etc, SYS:MMGT:MMGT:FREECNT holds
3356      * the number of 240-byte free memory words (aka 4-cell chunks) and not
3357      * 192 bytes as the documentation incorrectly says.
3358      */
3359     mmgt = ocelot_read(ocelot, SYS_MMGT);
3360     ocelot->packet_buffer_size = 240 * SYS_MMGT_FREECNT(mmgt);
3361
3362     eq_ctrl = ocelot_read(ocelot, QSYS_EQ_CTRL);
3363     ocelot->num_frame_refs = QSYS_MMGT_EQ_CTRL_FP_FREE_CNT(eq_ctrl);
3364 }
3365
3366 int ocelot_init(struct ocelot *ocelot)
3367 {
3368     char queue_name[32];
3369     int i, ret;
3370     u32 port;
3371
3372     if (ocelot->ops->reset) {
3373         ret = ocelot->ops->reset(ocelot);
3374         if (ret) {
3375             dev_err(ocelot->dev, "Switch reset failed\n");
3376             return ret;
3377         }
3378     }
3379
3380     ocelot->stats = devm_kcalloc(ocelot->dev,
3381                      ocelot->num_phys_ports * OCELOT_NUM_STATS,
3382                      sizeof(u64), GFP_KERNEL);
3383     if (!ocelot->stats)
3384         return -ENOMEM;
3385
3386     spin_lock_init(&ocelot->stats_lock);
3387     mutex_init(&ocelot->ptp_lock);
3388     mutex_init(&ocelot->mact_lock);
3389     mutex_init(&ocelot->fwd_domain_lock);
3390     mutex_init(&ocelot->tas_lock);
3391     spin_lock_init(&ocelot->ptp_clock_lock);
3392     spin_lock_init(&ocelot->ts_id_lock);
3393     snprintf(queue_name, sizeof(queue_name), "%s-stats",
3394          dev_name(ocelot->dev));
3395     ocelot->stats_queue = create_singlethread_workqueue(queue_name);
3396     if (!ocelot->stats_queue)
3397         return -ENOMEM;
3398
3399     ocelot->owq = alloc_ordered_workqueue("ocelot-owq", 0);
3400     if (!ocelot->owq) {
3401         destroy_workqueue(ocelot->stats_queue);
3402         return -ENOMEM;
3403     }
3404
3405     INIT_LIST_HEAD(&ocelot->multicast);
3406     INIT_LIST_HEAD(&ocelot->pgids);
3407     INIT_LIST_HEAD(&ocelot->vlans);
3408     INIT_LIST_HEAD(&ocelot->lag_fdbs);
3409     ocelot_detect_features(ocelot);
3410     ocelot_mact_init(ocelot);
3411     ocelot_vlan_init(ocelot);
3412     ocelot_vcap_init(ocelot);
3413     ocelot_cpu_port_init(ocelot);
3414
3415     if (ocelot->ops->psfp_init)
3416         ocelot->ops->psfp_init(ocelot);
3417
3418     for (port = 0; port < ocelot->num_phys_ports; port++) {
3419         /* Clear all counters (5 groups) */
3420         ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
3421                      SYS_STAT_CFG_STAT_CLEAR_SHOT(0x7f),
3422                  SYS_STAT_CFG);
3423     }
3424
3425     /* Only use S-Tag */
3426     ocelot_write(ocelot, ETH_P_8021AD, SYS_VLAN_ETYPE_CFG);
3427
3428     /* Aggregation mode */
3429     ocelot_write(ocelot, ANA_AGGR_CFG_AC_SMAC_ENA |
3430                  ANA_AGGR_CFG_AC_DMAC_ENA |
3431                  ANA_AGGR_CFG_AC_IP4_SIPDIP_ENA |
3432                  ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA |
3433                  ANA_AGGR_CFG_AC_IP6_FLOW_LBL_ENA |
3434                  ANA_AGGR_CFG_AC_IP6_TCPUDP_ENA,
3435                  ANA_AGGR_CFG);
3436
3437     /* Set MAC age time to default value. The entry is aged after
3438      * 2*AGE_PERIOD
3439      */
3440     ocelot_write(ocelot,
3441              ANA_AUTOAGE_AGE_PERIOD(BR_DEFAULT_AGEING_TIME / 2 / HZ),
3442              ANA_AUTOAGE);
3443
3444     /* Disable learning for frames discarded by VLAN ingress filtering */
3445     regmap_field_write(ocelot->regfields[ANA_ADVLEARN_VLAN_CHK], 1);
3446
3447     /* Setup frame ageing - fixed value "2 sec" - in 6.5 us units */
3448     ocelot_write(ocelot, SYS_FRM_AGING_AGE_TX_ENA |
3449              SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
3450
3451     /* Setup flooding PGIDs */
3452     for (i = 0; i < ocelot->num_flooding_pgids; i++)
3453         ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
3454                  ANA_FLOODING_FLD_BROADCAST(PGID_BC) |
3455                  ANA_FLOODING_FLD_UNICAST(PGID_UC),
3456                  ANA_FLOODING, i);
3457     ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
3458              ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
3459              ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
3460              ANA_FLOODING_IPMC_FLD_MC4_CTRL(PGID_MC),
3461              ANA_FLOODING_IPMC);
3462
3463     for (port = 0; port < ocelot->num_phys_ports; port++) {
3464         /* Transmit the frame to the local port. */
3465         ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
3466         /* Do not forward BPDU frames to the front ports. */
3467         ocelot_write_gix(ocelot,
3468                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
3469                  ANA_PORT_CPU_FWD_BPDU_CFG,
3470                  port);
3471         /* Ensure bridging is disabled */
3472         ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_SRC + port);
3473     }
3474
3475     for_each_nonreserved_multicast_dest_pgid(ocelot, i) {
3476         u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
3477
3478         ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
3479     }
3480
3481     ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_BLACKHOLE);
3482
3483     /* Allow broadcast and unknown L2 multicast to the CPU. */
3484     ocelot_rmw_rix(ocelot, ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3485                ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3486                ANA_PGID_PGID, PGID_MC);
3487     ocelot_rmw_rix(ocelot, ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3488                ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports)),
3489                ANA_PGID_PGID, PGID_BC);
3490     ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV4);
3491     ocelot_write_rix(ocelot, 0, ANA_PGID_PGID, PGID_MCIPV6);
3492
3493     /* Allow manual injection via DEVCPU_QS registers, and byte swap these
3494      * registers endianness.
3495      */
3496     ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_BYTE_SWAP |
3497              QS_INJ_GRP_CFG_MODE(1), QS_INJ_GRP_CFG, 0);
3498     ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_BYTE_SWAP |
3499              QS_XTR_GRP_CFG_MODE(1), QS_XTR_GRP_CFG, 0);
3500     ocelot_write(ocelot, ANA_CPUQ_CFG_CPUQ_MIRROR(2) |
3501              ANA_CPUQ_CFG_CPUQ_LRN(2) |
3502              ANA_CPUQ_CFG_CPUQ_MAC_COPY(2) |
3503              ANA_CPUQ_CFG_CPUQ_SRC_COPY(2) |
3504              ANA_CPUQ_CFG_CPUQ_LOCKED_PORTMOVE(2) |
3505              ANA_CPUQ_CFG_CPUQ_ALLBRIDGE(6) |
3506              ANA_CPUQ_CFG_CPUQ_IPMC_CTRL(6) |
3507              ANA_CPUQ_CFG_CPUQ_IGMP(6) |
3508              ANA_CPUQ_CFG_CPUQ_MLD(6), ANA_CPUQ_CFG);
3509     for (i = 0; i < 16; i++)
3510         ocelot_write_rix(ocelot, ANA_CPUQ_8021_CFG_CPUQ_GARP_VAL(6) |
3511                  ANA_CPUQ_8021_CFG_CPUQ_BPDU_VAL(6),
3512                  ANA_CPUQ_8021_CFG, i);
3513
3514     ret = ocelot_prepare_stats_regions(ocelot);
3515     if (ret) {
3516         destroy_workqueue(ocelot->stats_queue);
3517         destroy_workqueue(ocelot->owq);
3518         return ret;
3519     }
3520
3521     INIT_DELAYED_WORK(&ocelot->stats_work, ocelot_check_stats_work);
3522     queue_delayed_work(ocelot->stats_queue, &ocelot->stats_work,
3523                OCELOT_STATS_CHECK_DELAY);
3524
3525     return 0;
3526 }
3527 EXPORT_SYMBOL(ocelot_init);
3528
3529 void ocelot_deinit(struct ocelot *ocelot)
3530 {
3531     cancel_delayed_work(&ocelot->stats_work);
3532     destroy_workqueue(ocelot->stats_queue);
3533     destroy_workqueue(ocelot->owq);
3534 }
3535 EXPORT_SYMBOL(ocelot_deinit);
3536
3537 void ocelot_deinit_port(struct ocelot *ocelot, int port)
3538 {
3539     struct ocelot_port *ocelot_port = ocelot->ports[port];
3540
3541     skb_queue_purge(&ocelot_port->tx_skbs);
3542 }
3543 EXPORT_SYMBOL(ocelot_deinit_port);
3544
3545 MODULE_LICENSE("Dual MIT/GPL");