drivers/net/dsa/ocelot/felix.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright 2019-2021 NXP Semiconductors
   3  *
   4  * This is an umbrella module for all network switches that are
   5  * register-compatible with Ocelot and that perform I/O to their host CPU
   6  * through an NPI (Node Processor Interface) Ethernet port.
   7  */
   8 #include <uapi/linux/if_bridge.h>
   9 #include <soc/mscc/ocelot_vcap.h>
  10 #include <soc/mscc/ocelot_qsys.h>
  11 #include <soc/mscc/ocelot_sys.h>
  12 #include <soc/mscc/ocelot_dev.h>
  13 #include <soc/mscc/ocelot_ana.h>
  14 #include <soc/mscc/ocelot_ptp.h>
  15 #include <soc/mscc/ocelot.h>
  16 #include <linux/dsa/8021q.h>
  17 #include <linux/dsa/ocelot.h>
  18 #include <linux/platform_device.h>
  19 #include <linux/ptp_classify.h>
  20 #include <linux/module.h>
  21 #include <linux/of_net.h>
  22 #include <linux/pci.h>
  23 #include <linux/of.h>
  24 #include <linux/pcs-lynx.h>
  25 #include <net/pkt_sched.h>
  26 #include <net/dsa.h>
  27 #include "felix.h"
  28
  29 static int felix_tag_8021q_rxvlan_add(struct felix *felix, int port, u16 vid,
  30                                       bool pvid, bool untagged)
  31 {
  32         struct ocelot_vcap_filter *outer_tagging_rule;
  33         struct ocelot *ocelot = &felix->ocelot;
  34         struct dsa_switch *ds = felix->ds;
  35         int key_length, upstream, err;
  36
  37         /* We don't need to install the rxvlan into the other ports' filtering
  38          * tables, because we're just pushing the rxvlan when sending towards
  39          * the CPU
  40          */
  41         if (!pvid)
  42                 return 0;
  43
  44         key_length = ocelot->vcap[VCAP_ES0].keys[VCAP_ES0_IGR_PORT].length;
  45         upstream = dsa_upstream_port(ds, port);
  46
  47         outer_tagging_rule = kzalloc(sizeof(struct ocelot_vcap_filter),
  48                                      GFP_KERNEL);
  49         if (!outer_tagging_rule)
  50                 return -ENOMEM;
  51
  52         outer_tagging_rule->key_type = OCELOT_VCAP_KEY_ANY;
  53         outer_tagging_rule->prio = 1;
  54         outer_tagging_rule->id.cookie = port;
  55         outer_tagging_rule->id.tc_offload = false;
  56         outer_tagging_rule->block_id = VCAP_ES0;
  57         outer_tagging_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
  58         outer_tagging_rule->lookup = 0;
  59         outer_tagging_rule->ingress_port.value = port;
  60         outer_tagging_rule->ingress_port.mask = GENMASK(key_length - 1, 0);
  61         outer_tagging_rule->egress_port.value = upstream;
  62         outer_tagging_rule->egress_port.mask = GENMASK(key_length - 1, 0);
  63         outer_tagging_rule->action.push_outer_tag = OCELOT_ES0_TAG;
  64         outer_tagging_rule->action.tag_a_tpid_sel = OCELOT_TAG_TPID_SEL_8021AD;
  65         outer_tagging_rule->action.tag_a_vid_sel = 1;
  66         outer_tagging_rule->action.vid_a_val = vid;
  67
  68         err = ocelot_vcap_filter_add(ocelot, outer_tagging_rule, NULL);
  69         if (err)
  70                 kfree(outer_tagging_rule);
  71
  72         return err;
  73 }
  74
  75 static int felix_tag_8021q_txvlan_add(struct felix *felix, int port, u16 vid,
  76                                       bool pvid, bool untagged)
  77 {
  78         struct ocelot_vcap_filter *untagging_rule, *redirect_rule;
  79         struct ocelot *ocelot = &felix->ocelot;
  80         struct dsa_switch *ds = felix->ds;
  81         int upstream, err;
  82
  83         /* tag_8021q.c assumes we are implementing this via port VLAN
  84          * membership, which we aren't. So we don't need to add any VCAP filter
  85          * for the CPU port.
  86          */
  87         if (ocelot->ports[port]->is_dsa_8021q_cpu)
  88                 return 0;
  89
  90         untagging_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
  91         if (!untagging_rule)
  92                 return -ENOMEM;
  93
  94         redirect_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
  95         if (!redirect_rule) {
  96                 kfree(untagging_rule);
  97                 return -ENOMEM;
  98         }
  99
 100         upstream = dsa_upstream_port(ds, port);
 101
 102         untagging_rule->key_type = OCELOT_VCAP_KEY_ANY;
 103         untagging_rule->ingress_port_mask = BIT(upstream);
 104         untagging_rule->vlan.vid.value = vid;
 105         untagging_rule->vlan.vid.mask = VLAN_VID_MASK;
 106         untagging_rule->prio = 1;
 107         untagging_rule->id.cookie = port;
 108         untagging_rule->id.tc_offload = false;
 109         untagging_rule->block_id = VCAP_IS1;
 110         untagging_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
 111         untagging_rule->lookup = 0;
 112         untagging_rule->action.vlan_pop_cnt_ena = true;
 113         untagging_rule->action.vlan_pop_cnt = 1;
 114         untagging_rule->action.pag_override_mask = 0xff;
 115         untagging_rule->action.pag_val = port;
 116
 117         err = ocelot_vcap_filter_add(ocelot, untagging_rule, NULL);
 118         if (err) {
 119                 kfree(untagging_rule);
 120                 kfree(redirect_rule);
 121                 return err;
 122         }
 123
 124         redirect_rule->key_type = OCELOT_VCAP_KEY_ANY;
 125         redirect_rule->ingress_port_mask = BIT(upstream);
 126         redirect_rule->pag = port;
 127         redirect_rule->prio = 1;
 128         redirect_rule->id.cookie = port;
 129         redirect_rule->id.tc_offload = false;
 130         redirect_rule->block_id = VCAP_IS2;
 131         redirect_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
 132         redirect_rule->lookup = 0;
 133         redirect_rule->action.mask_mode = OCELOT_MASK_MODE_REDIRECT;
 134         redirect_rule->action.port_mask = BIT(port);
 135
 136         err = ocelot_vcap_filter_add(ocelot, redirect_rule, NULL);
 137         if (err) {
 138                 ocelot_vcap_filter_del(ocelot, untagging_rule);
 139                 kfree(redirect_rule);
 140                 return err;
 141         }
 142
 143         return 0;
 144 }
 145
 146 static int felix_tag_8021q_vlan_add(struct dsa_switch *ds, int port, u16 vid,
 147                                     u16 flags)
 148 {
 149         bool untagged = flags & BRIDGE_VLAN_INFO_UNTAGGED;
 150         bool pvid = flags & BRIDGE_VLAN_INFO_PVID;
 151         struct ocelot *ocelot = ds->priv;
 152
 153         if (vid_is_dsa_8021q_rxvlan(vid))
 154                 return felix_tag_8021q_rxvlan_add(ocelot_to_felix(ocelot),
 155                                                   port, vid, pvid, untagged);
 156
 157         if (vid_is_dsa_8021q_txvlan(vid))
 158                 return felix_tag_8021q_txvlan_add(ocelot_to_felix(ocelot),
 159                                                   port, vid, pvid, untagged);
 160
 161         return 0;
 162 }
 163
 164 static int felix_tag_8021q_rxvlan_del(struct felix *felix, int port, u16 vid)
 165 {
 166         struct ocelot_vcap_filter *outer_tagging_rule;
 167         struct ocelot_vcap_block *block_vcap_es0;
 168         struct ocelot *ocelot = &felix->ocelot;
 169
 170         block_vcap_es0 = &ocelot->block[VCAP_ES0];
 171
 172         outer_tagging_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_es0,
 173                                                                  port, false);
 174         /* In rxvlan_add, we had the "if (!pvid) return 0" logic to avoid
 175          * installing outer tagging ES0 rules where they weren't needed.
 176          * But in rxvlan_del, the API doesn't give us the "flags" anymore,
 177          * so that forces us to be slightly sloppy here, and just assume that
 178          * if we didn't find an outer_tagging_rule it means that there was
 179          * none in the first place, i.e. rxvlan_del is called on a non-pvid
 180          * port. This is most probably true though.
 181          */
 182         if (!outer_tagging_rule)
 183                 return 0;
 184
 185         return ocelot_vcap_filter_del(ocelot, outer_tagging_rule);
 186 }
 187
 188 static int felix_tag_8021q_txvlan_del(struct felix *felix, int port, u16 vid)
 189 {
 190         struct ocelot_vcap_filter *untagging_rule, *redirect_rule;
 191         struct ocelot_vcap_block *block_vcap_is1;
 192         struct ocelot_vcap_block *block_vcap_is2;
 193         struct ocelot *ocelot = &felix->ocelot;
 194         int err;
 195
 196         if (ocelot->ports[port]->is_dsa_8021q_cpu)
 197                 return 0;
 198
 199         block_vcap_is1 = &ocelot->block[VCAP_IS1];
 200         block_vcap_is2 = &ocelot->block[VCAP_IS2];
 201
 202         untagging_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is1,
 203                                                              port, false);
 204         if (!untagging_rule)
 205                 return 0;
 206
 207         err = ocelot_vcap_filter_del(ocelot, untagging_rule);
 208         if (err)
 209                 return err;
 210
 211         redirect_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is2,
 212                                                             port, false);
 213         if (!redirect_rule)
 214                 return 0;
 215
 216         return ocelot_vcap_filter_del(ocelot, redirect_rule);
 217 }
 218
 219 static int felix_tag_8021q_vlan_del(struct dsa_switch *ds, int port, u16 vid)
 220 {
 221         struct ocelot *ocelot = ds->priv;
 222
 223         if (vid_is_dsa_8021q_rxvlan(vid))
 224                 return felix_tag_8021q_rxvlan_del(ocelot_to_felix(ocelot),
 225                                                   port, vid);
 226
 227         if (vid_is_dsa_8021q_txvlan(vid))
 228                 return felix_tag_8021q_txvlan_del(ocelot_to_felix(ocelot),
 229                                                   port, vid);
 230
 231         return 0;
 232 }
 233
 234 /* Alternatively to using the NPI functionality, that same hardware MAC
 235  * connected internally to the enetc or fman DSA master can be configured to
 236  * use the software-defined tag_8021q frame format. As far as the hardware is
 237  * concerned, it thinks it is a "dumb switch" - the queues of the CPU port
 238  * module are now disconnected from it, but can still be accessed through
 239  * register-based MMIO.
 240  */
 241 static void felix_8021q_cpu_port_init(struct ocelot *ocelot, int port)
 242 {
 243         ocelot->ports[port]->is_dsa_8021q_cpu = true;
 244         ocelot->npi = -1;
 245
 246         /* Overwrite PGID_CPU with the non-tagging port */
 247         ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, PGID_CPU);
 248
 249         ocelot_apply_bridge_fwd_mask(ocelot);
 250 }
 251
 252 static void felix_8021q_cpu_port_deinit(struct ocelot *ocelot, int port)
 253 {
 254         ocelot->ports[port]->is_dsa_8021q_cpu = false;
 255
 256         /* Restore PGID_CPU */
 257         ocelot_write_rix(ocelot, BIT(ocelot->num_phys_ports), ANA_PGID_PGID,
 258                          PGID_CPU);
 259
 260         ocelot_apply_bridge_fwd_mask(ocelot);
 261 }
 262
 263 /* Set up a VCAP IS2 rule for delivering PTP frames to the CPU port module.
 264  * If the quirk_no_xtr_irq is in place, then also copy those PTP frames to the
 265  * tag_8021q CPU port.
 266  */
 267 static int felix_setup_mmio_filtering(struct felix *felix)
 268 {
 269         unsigned long user_ports = 0, cpu_ports = 0;
 270         struct ocelot_vcap_filter *redirect_rule;
 271         struct ocelot_vcap_filter *tagging_rule;
 272         struct ocelot *ocelot = &felix->ocelot;
 273         struct dsa_switch *ds = felix->ds;
 274         int port, ret;
 275
 276         tagging_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
 277         if (!tagging_rule)
 278                 return -ENOMEM;
 279
 280         redirect_rule = kzalloc(sizeof(struct ocelot_vcap_filter), GFP_KERNEL);
 281         if (!redirect_rule) {
 282                 kfree(tagging_rule);
 283                 return -ENOMEM;
 284         }
 285
 286         for (port = 0; port < ocelot->num_phys_ports; port++) {
 287                 if (dsa_is_user_port(ds, port))
 288                         user_ports |= BIT(port);
 289                 if (dsa_is_cpu_port(ds, port))
 290                         cpu_ports |= BIT(port);
 291         }
 292
 293         tagging_rule->key_type = OCELOT_VCAP_KEY_ETYPE;
 294         *(__be16 *)tagging_rule->key.etype.etype.value = htons(ETH_P_1588);
 295         *(__be16 *)tagging_rule->key.etype.etype.mask = htons(0xffff);
 296         tagging_rule->ingress_port_mask = user_ports;
 297         tagging_rule->prio = 1;
 298         tagging_rule->id.cookie = ocelot->num_phys_ports;
 299         tagging_rule->id.tc_offload = false;
 300         tagging_rule->block_id = VCAP_IS1;
 301         tagging_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
 302         tagging_rule->lookup = 0;
 303         tagging_rule->action.pag_override_mask = 0xff;
 304         tagging_rule->action.pag_val = ocelot->num_phys_ports;
 305
 306         ret = ocelot_vcap_filter_add(ocelot, tagging_rule, NULL);
 307         if (ret) {
 308                 kfree(tagging_rule);
 309                 kfree(redirect_rule);
 310                 return ret;
 311         }
 312
 313         redirect_rule->key_type = OCELOT_VCAP_KEY_ANY;
 314         redirect_rule->ingress_port_mask = user_ports;
 315         redirect_rule->pag = ocelot->num_phys_ports;
 316         redirect_rule->prio = 1;
 317         redirect_rule->id.cookie = ocelot->num_phys_ports;
 318         redirect_rule->id.tc_offload = false;
 319         redirect_rule->block_id = VCAP_IS2;
 320         redirect_rule->type = OCELOT_VCAP_FILTER_OFFLOAD;
 321         redirect_rule->lookup = 0;
 322         redirect_rule->action.cpu_copy_ena = true;
 323         if (felix->info->quirk_no_xtr_irq) {
 324                 /* Redirect to the tag_8021q CPU but also copy PTP packets to
 325                  * the CPU port module
 326                  */
 327                 redirect_rule->action.mask_mode = OCELOT_MASK_MODE_REDIRECT;
 328                 redirect_rule->action.port_mask = cpu_ports;
 329         } else {
 330                 /* Trap PTP packets only to the CPU port module (which is
 331                  * redirected to the NPI port)
 332                  */
 333                 redirect_rule->action.mask_mode = OCELOT_MASK_MODE_PERMIT_DENY;
 334                 redirect_rule->action.port_mask = 0;
 335         }
 336
 337         ret = ocelot_vcap_filter_add(ocelot, redirect_rule, NULL);
 338         if (ret) {
 339                 ocelot_vcap_filter_del(ocelot, tagging_rule);
 340                 kfree(redirect_rule);
 341                 return ret;
 342         }
 343
 344         /* The ownership of the CPU port module's queues might have just been
 345          * transferred to the tag_8021q tagger from the NPI-based tagger.
 346          * So there might still be all sorts of crap in the queues. On the
 347          * other hand, the MMIO-based matching of PTP frames is very brittle,
 348          * so we need to be careful that there are no extra frames to be
 349          * dequeued over MMIO, since we would never know to discard them.
 350          */
 351         ocelot_drain_cpu_queue(ocelot, 0);
 352
 353         return 0;
 354 }
 355
 356 static int felix_teardown_mmio_filtering(struct felix *felix)
 357 {
 358         struct ocelot_vcap_filter *tagging_rule, *redirect_rule;
 359         struct ocelot_vcap_block *block_vcap_is1;
 360         struct ocelot_vcap_block *block_vcap_is2;
 361         struct ocelot *ocelot = &felix->ocelot;
 362         int err;
 363
 364         block_vcap_is1 = &ocelot->block[VCAP_IS1];
 365         block_vcap_is2 = &ocelot->block[VCAP_IS2];
 366
 367         tagging_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is1,
 368                                                            ocelot->num_phys_ports,
 369                                                            false);
 370         if (!tagging_rule)
 371                 return -ENOENT;
 372
 373         err = ocelot_vcap_filter_del(ocelot, tagging_rule);
 374         if (err)
 375                 return err;
 376
 377         redirect_rule = ocelot_vcap_block_find_filter_by_id(block_vcap_is2,
 378                                                             ocelot->num_phys_ports,
 379                                                             false);
 380         if (!redirect_rule)
 381                 return -ENOENT;
 382
 383         return ocelot_vcap_filter_del(ocelot, redirect_rule);
 384 }
 385
 386 static int felix_setup_tag_8021q(struct dsa_switch *ds, int cpu)
 387 {
 388         struct ocelot *ocelot = ds->priv;
 389         struct felix *felix = ocelot_to_felix(ocelot);
 390         unsigned long cpu_flood;
 391         int port, err;
 392
 393         felix_8021q_cpu_port_init(ocelot, cpu);
 394
 395         for (port = 0; port < ds->num_ports; port++) {
 396                 if (dsa_is_unused_port(ds, port))
 397                         continue;
 398
 399                 /* This overwrites ocelot_init():
 400                  * Do not forward BPDU frames to the CPU port module,
 401                  * for 2 reasons:
 402                  * - When these packets are injected from the tag_8021q
 403                  *   CPU port, we want them to go out, not loop back
 404                  *   into the system.
 405                  * - STP traffic ingressing on a user port should go to
 406                  *   the tag_8021q CPU port, not to the hardware CPU
 407                  *   port module.
 408                  */
 409                 ocelot_write_gix(ocelot,
 410                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0),
 411                                  ANA_PORT_CPU_FWD_BPDU_CFG, port);
 412         }
 413
 414         /* In tag_8021q mode, the CPU port module is unused, except for PTP
 415          * frames. So we want to disable flooding of any kind to the CPU port
 416          * module, since packets going there will end in a black hole.
 417          */
 418         cpu_flood = ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports));
 419         ocelot_rmw_rix(ocelot, 0, cpu_flood, ANA_PGID_PGID, PGID_UC);
 420         ocelot_rmw_rix(ocelot, 0, cpu_flood, ANA_PGID_PGID, PGID_MC);
 421         ocelot_rmw_rix(ocelot, 0, cpu_flood, ANA_PGID_PGID, PGID_BC);
 422
 423         err = dsa_tag_8021q_register(ds, htons(ETH_P_8021AD));
 424         if (err)
 425                 return err;
 426
 427         err = felix_setup_mmio_filtering(felix);
 428         if (err)
 429                 goto out_tag_8021q_unregister;
 430
 431         return 0;
 432
 433 out_tag_8021q_unregister:
 434         dsa_tag_8021q_unregister(ds);
 435         return err;
 436 }
 437
 438 static void felix_teardown_tag_8021q(struct dsa_switch *ds, int cpu)
 439 {
 440         struct ocelot *ocelot = ds->priv;
 441         struct felix *felix = ocelot_to_felix(ocelot);
 442         int err, port;
 443
 444         err = felix_teardown_mmio_filtering(felix);
 445         if (err)
 446                 dev_err(ds->dev, "felix_teardown_mmio_filtering returned %d",
 447                         err);
 448
 449         dsa_tag_8021q_unregister(ds);
 450
 451         for (port = 0; port < ds->num_ports; port++) {
 452                 if (dsa_is_unused_port(ds, port))
 453                         continue;
 454
 455                 /* Restore the logic from ocelot_init:
 456                  * do not forward BPDU frames to the front ports.
 457                  */
 458                 ocelot_write_gix(ocelot,
 459                                  ANA_PORT_CPU_FWD_BPDU_CFG_BPDU_REDIR_ENA(0xffff),
 460                                  ANA_PORT_CPU_FWD_BPDU_CFG,
 461                                  port);
 462         }
 463
 464         felix_8021q_cpu_port_deinit(ocelot, cpu);
 465 }
 466
 467 /* The CPU port module is connected to the Node Processor Interface (NPI). This
 468  * is the mode through which frames can be injected from and extracted to an
 469  * external CPU, over Ethernet. In NXP SoCs, the "external CPU" is the ARM CPU
 470  * running Linux, and this forms a DSA setup together with the enetc or fman
 471  * DSA master.
 472  */
 473 static void felix_npi_port_init(struct ocelot *ocelot, int port)
 474 {
 475         ocelot->npi = port;
 476
 477         ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPUQ_MSK_M |
 478                      QSYS_EXT_CPU_CFG_EXT_CPU_PORT(port),
 479                      QSYS_EXT_CPU_CFG);
 480
 481         /* NPI port Injection/Extraction configuration */
 482         ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_XTR_HDR,
 483                             ocelot->npi_xtr_prefix);
 484         ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_INJ_HDR,
 485                             ocelot->npi_inj_prefix);
 486
 487         /* Disable transmission of pause frames */
 488         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 0);
 489 }
 490
 491 static void felix_npi_port_deinit(struct ocelot *ocelot, int port)
 492 {
 493         /* Restore hardware defaults */
 494         int unused_port = ocelot->num_phys_ports + 2;
 495
 496         ocelot->npi = -1;
 497
 498         ocelot_write(ocelot, QSYS_EXT_CPU_CFG_EXT_CPU_PORT(unused_port),
 499                      QSYS_EXT_CPU_CFG);
 500
 501         ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_XTR_HDR,
 502                             OCELOT_TAG_PREFIX_DISABLED);
 503         ocelot_fields_write(ocelot, port, SYS_PORT_MODE_INCL_INJ_HDR,
 504                             OCELOT_TAG_PREFIX_DISABLED);
 505
 506         /* Enable transmission of pause frames */
 507         ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
 508 }
 509
 510 static int felix_setup_tag_npi(struct dsa_switch *ds, int cpu)
 511 {
 512         struct ocelot *ocelot = ds->priv;
 513         unsigned long cpu_flood;
 514
 515         felix_npi_port_init(ocelot, cpu);
 516
 517         /* Include the CPU port module (and indirectly, the NPI port)
 518          * in the forwarding mask for unknown unicast - the hardware
 519          * default value for ANA_FLOODING_FLD_UNICAST excludes
 520          * BIT(ocelot->num_phys_ports), and so does ocelot_init,
 521          * since Ocelot relies on whitelisting MAC addresses towards
 522          * PGID_CPU.
 523          * We do this because DSA does not yet perform RX filtering,
 524          * and the NPI port does not perform source address learning,
 525          * so traffic sent to Linux is effectively unknown from the
 526          * switch's perspective.
 527          */
 528         cpu_flood = ANA_PGID_PGID_PGID(BIT(ocelot->num_phys_ports));
 529         ocelot_rmw_rix(ocelot, cpu_flood, cpu_flood, ANA_PGID_PGID, PGID_UC);
 530         ocelot_rmw_rix(ocelot, cpu_flood, cpu_flood, ANA_PGID_PGID, PGID_MC);
 531         ocelot_rmw_rix(ocelot, cpu_flood, cpu_flood, ANA_PGID_PGID, PGID_BC);
 532
 533         return 0;
 534 }
 535
 536 static void felix_teardown_tag_npi(struct dsa_switch *ds, int cpu)
 537 {
 538         struct ocelot *ocelot = ds->priv;
 539
 540         felix_npi_port_deinit(ocelot, cpu);
 541 }
 542
 543 static int felix_set_tag_protocol(struct dsa_switch *ds, int cpu,
 544                                   enum dsa_tag_protocol proto)
 545 {
 546         int err;
 547
 548         switch (proto) {
 549         case DSA_TAG_PROTO_SEVILLE:
 550         case DSA_TAG_PROTO_OCELOT:
 551                 err = felix_setup_tag_npi(ds, cpu);
 552                 break;
 553         case DSA_TAG_PROTO_OCELOT_8021Q:
 554                 err = felix_setup_tag_8021q(ds, cpu);
 555                 break;
 556         default:
 557                 err = -EPROTONOSUPPORT;
 558         }
 559
 560         return err;
 561 }
 562
 563 static void felix_del_tag_protocol(struct dsa_switch *ds, int cpu,
 564                                    enum dsa_tag_protocol proto)
 565 {
 566         switch (proto) {
 567         case DSA_TAG_PROTO_SEVILLE:
 568         case DSA_TAG_PROTO_OCELOT:
 569                 felix_teardown_tag_npi(ds, cpu);
 570                 break;
 571         case DSA_TAG_PROTO_OCELOT_8021Q:
 572                 felix_teardown_tag_8021q(ds, cpu);
 573                 break;
 574         default:
 575                 break;
 576         }
 577 }
 578
 579 /* This always leaves the switch in a consistent state, because although the
 580  * tag_8021q setup can fail, the NPI setup can't. So either the change is made,
 581  * or the restoration is guaranteed to work.
 582  */
 583 static int felix_change_tag_protocol(struct dsa_switch *ds, int cpu,
 584                                      enum dsa_tag_protocol proto)
 585 {
 586         struct ocelot *ocelot = ds->priv;
 587         struct felix *felix = ocelot_to_felix(ocelot);
 588         enum dsa_tag_protocol old_proto = felix->tag_proto;
 589         int err;
 590
 591         if (proto != DSA_TAG_PROTO_SEVILLE &&
 592             proto != DSA_TAG_PROTO_OCELOT &&
 593             proto != DSA_TAG_PROTO_OCELOT_8021Q)
 594                 return -EPROTONOSUPPORT;
 595
 596         felix_del_tag_protocol(ds, cpu, old_proto);
 597
 598         err = felix_set_tag_protocol(ds, cpu, proto);
 599         if (err) {
 600                 felix_set_tag_protocol(ds, cpu, old_proto);
 601                 return err;
 602         }
 603
 604         felix->tag_proto = proto;
 605
 606         return 0;
 607 }
 608
 609 static enum dsa_tag_protocol felix_get_tag_protocol(struct dsa_switch *ds,
 610                                                     int port,
 611                                                     enum dsa_tag_protocol mp)
 612 {
 613         struct ocelot *ocelot = ds->priv;
 614         struct felix *felix = ocelot_to_felix(ocelot);
 615
 616         return felix->tag_proto;
 617 }
 618
 619 static int felix_set_ageing_time(struct dsa_switch *ds,
 620                                  unsigned int ageing_time)
 621 {
 622         struct ocelot *ocelot = ds->priv;
 623
 624         ocelot_set_ageing_time(ocelot, ageing_time);
 625
 626         return 0;
 627 }
 628
 629 static int felix_fdb_dump(struct dsa_switch *ds, int port,
 630                           dsa_fdb_dump_cb_t *cb, void *data)
 631 {
 632         struct ocelot *ocelot = ds->priv;
 633
 634         return ocelot_fdb_dump(ocelot, port, cb, data);
 635 }
 636
 637 static int felix_fdb_add(struct dsa_switch *ds, int port,
 638                          const unsigned char *addr, u16 vid)
 639 {
 640         struct ocelot *ocelot = ds->priv;
 641
 642         return ocelot_fdb_add(ocelot, port, addr, vid);
 643 }
 644
 645 static int felix_fdb_del(struct dsa_switch *ds, int port,
 646                          const unsigned char *addr, u16 vid)
 647 {
 648         struct ocelot *ocelot = ds->priv;
 649
 650         return ocelot_fdb_del(ocelot, port, addr, vid);
 651 }
 652
 653 static int felix_mdb_add(struct dsa_switch *ds, int port,
 654                          const struct switchdev_obj_port_mdb *mdb)
 655 {
 656         struct ocelot *ocelot = ds->priv;
 657
 658         return ocelot_port_mdb_add(ocelot, port, mdb);
 659 }
 660
 661 static int felix_mdb_del(struct dsa_switch *ds, int port,
 662                          const struct switchdev_obj_port_mdb *mdb)
 663 {
 664         struct ocelot *ocelot = ds->priv;
 665
 666         return ocelot_port_mdb_del(ocelot, port, mdb);
 667 }
 668
 669 static void felix_bridge_stp_state_set(struct dsa_switch *ds, int port,
 670                                        u8 state)
 671 {
 672         struct ocelot *ocelot = ds->priv;
 673
 674         return ocelot_bridge_stp_state_set(ocelot, port, state);
 675 }
 676
 677 static int felix_pre_bridge_flags(struct dsa_switch *ds, int port,
 678                                   struct switchdev_brport_flags val,
 679                                   struct netlink_ext_ack *extack)
 680 {
 681         struct ocelot *ocelot = ds->priv;
 682
 683         return ocelot_port_pre_bridge_flags(ocelot, port, val);
 684 }
 685
 686 static int felix_bridge_flags(struct dsa_switch *ds, int port,
 687                               struct switchdev_brport_flags val,
 688                               struct netlink_ext_ack *extack)
 689 {
 690         struct ocelot *ocelot = ds->priv;
 691
 692         ocelot_port_bridge_flags(ocelot, port, val);
 693
 694         return 0;
 695 }
 696
 697 static int felix_bridge_join(struct dsa_switch *ds, int port,
 698                              struct net_device *br)
 699 {
 700         struct ocelot *ocelot = ds->priv;
 701
 702         ocelot_port_bridge_join(ocelot, port, br);
 703
 704         return 0;
 705 }
 706
 707 static void felix_bridge_leave(struct dsa_switch *ds, int port,
 708                                struct net_device *br)
 709 {
 710         struct ocelot *ocelot = ds->priv;
 711
 712         ocelot_port_bridge_leave(ocelot, port, br);
 713 }
 714
 715 static int felix_lag_join(struct dsa_switch *ds, int port,
 716                           struct net_device *bond,
 717                           struct netdev_lag_upper_info *info)
 718 {
 719         struct ocelot *ocelot = ds->priv;
 720
 721         return ocelot_port_lag_join(ocelot, port, bond, info);
 722 }
 723
 724 static int felix_lag_leave(struct dsa_switch *ds, int port,
 725                            struct net_device *bond)
 726 {
 727         struct ocelot *ocelot = ds->priv;
 728
 729         ocelot_port_lag_leave(ocelot, port, bond);
 730
 731         return 0;
 732 }
 733
 734 static int felix_lag_change(struct dsa_switch *ds, int port)
 735 {
 736         struct dsa_port *dp = dsa_to_port(ds, port);
 737         struct ocelot *ocelot = ds->priv;
 738
 739         ocelot_port_lag_change(ocelot, port, dp->lag_tx_enabled);
 740
 741         return 0;
 742 }
 743
 744 static int felix_vlan_prepare(struct dsa_switch *ds, int port,
 745                               const struct switchdev_obj_port_vlan *vlan,
 746                               struct netlink_ext_ack *extack)
 747 {
 748         struct ocelot *ocelot = ds->priv;
 749         u16 flags = vlan->flags;
 750
 751         /* Ocelot switches copy frames as-is to the CPU, so the flags:
 752          * egress-untagged or not, pvid or not, make no difference. This
 753          * behavior is already better than what DSA just tries to approximate
 754          * when it installs the VLAN with the same flags on the CPU port.
 755          * Just accept any configuration, and don't let ocelot deny installing
 756          * multiple native VLANs on the NPI port, because the switch doesn't
 757          * look at the port tag settings towards the NPI interface anyway.
 758          */
 759         if (port == ocelot->npi)
 760                 return 0;
 761
 762         return ocelot_vlan_prepare(ocelot, port, vlan->vid,
 763                                    flags & BRIDGE_VLAN_INFO_PVID,
 764                                    flags & BRIDGE_VLAN_INFO_UNTAGGED,
 765                                    extack);
 766 }
 767
 768 static int felix_vlan_filtering(struct dsa_switch *ds, int port, bool enabled,
 769                                 struct netlink_ext_ack *extack)
 770 {
 771         struct ocelot *ocelot = ds->priv;
 772
 773         return ocelot_port_vlan_filtering(ocelot, port, enabled, extack);
 774 }
 775
 776 static int felix_vlan_add(struct dsa_switch *ds, int port,
 777                           const struct switchdev_obj_port_vlan *vlan,
 778                           struct netlink_ext_ack *extack)
 779 {
 780         struct ocelot *ocelot = ds->priv;
 781         u16 flags = vlan->flags;
 782         int err;
 783
 784         err = felix_vlan_prepare(ds, port, vlan, extack);
 785         if (err)
 786                 return err;
 787
 788         return ocelot_vlan_add(ocelot, port, vlan->vid,
 789                                flags & BRIDGE_VLAN_INFO_PVID,
 790                                flags & BRIDGE_VLAN_INFO_UNTAGGED);
 791 }
 792
 793 static int felix_vlan_del(struct dsa_switch *ds, int port,
 794                           const struct switchdev_obj_port_vlan *vlan)
 795 {
 796         struct ocelot *ocelot = ds->priv;
 797
 798         return ocelot_vlan_del(ocelot, port, vlan->vid);
 799 }
 800
 801 static void felix_phylink_validate(struct dsa_switch *ds, int port,
 802                                    unsigned long *supported,
 803                                    struct phylink_link_state *state)
 804 {
 805         struct ocelot *ocelot = ds->priv;
 806         struct felix *felix = ocelot_to_felix(ocelot);
 807
 808         if (felix->info->phylink_validate)
 809                 felix->info->phylink_validate(ocelot, port, supported, state);
 810 }
 811
 812 static void felix_phylink_mac_config(struct dsa_switch *ds, int port,
 813                                      unsigned int link_an_mode,
 814                                      const struct phylink_link_state *state)
 815 {
 816         struct ocelot *ocelot = ds->priv;
 817         struct felix *felix = ocelot_to_felix(ocelot);
 818         struct dsa_port *dp = dsa_to_port(ds, port);
 819
 820         if (felix->pcs[port])
 821                 phylink_set_pcs(dp->pl, &felix->pcs[port]->pcs);
 822 }
 823
 824 static void felix_phylink_mac_link_down(struct dsa_switch *ds, int port,
 825                                         unsigned int link_an_mode,
 826                                         phy_interface_t interface)
 827 {
 828         struct ocelot *ocelot = ds->priv;
 829
 830         ocelot_phylink_mac_link_down(ocelot, port, link_an_mode, interface,
 831                                      FELIX_MAC_QUIRKS);
 832 }
 833
 834 static void felix_phylink_mac_link_up(struct dsa_switch *ds, int port,
 835                                       unsigned int link_an_mode,
 836                                       phy_interface_t interface,
 837                                       struct phy_device *phydev,
 838                                       int speed, int duplex,
 839                                       bool tx_pause, bool rx_pause)
 840 {
 841         struct ocelot *ocelot = ds->priv;
 842         struct felix *felix = ocelot_to_felix(ocelot);
 843
 844         ocelot_phylink_mac_link_up(ocelot, port, phydev, link_an_mode,
 845                                    interface, speed, duplex, tx_pause, rx_pause,
 846                                    FELIX_MAC_QUIRKS);
 847
 848         if (felix->info->port_sched_speed_set)
 849                 felix->info->port_sched_speed_set(ocelot, port, speed);
 850 }
 851
 852 static void felix_port_qos_map_init(struct ocelot *ocelot, int port)
 853 {
 854         int i;
 855
 856         ocelot_rmw_gix(ocelot,
 857                        ANA_PORT_QOS_CFG_QOS_PCP_ENA,
 858                        ANA_PORT_QOS_CFG_QOS_PCP_ENA,
 859                        ANA_PORT_QOS_CFG,
 860                        port);
 861
 862         for (i = 0; i < OCELOT_NUM_TC * 2; i++) {
 863                 ocelot_rmw_ix(ocelot,
 864                               (ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL & i) |
 865                               ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL(i),
 866                               ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL |
 867                               ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL_M,
 868                               ANA_PORT_PCP_DEI_MAP,
 869                               port, i);
 870         }
 871 }
 872
 873 static void felix_get_strings(struct dsa_switch *ds, int port,
 874                               u32 stringset, u8 *data)
 875 {
 876         struct ocelot *ocelot = ds->priv;
 877
 878         return ocelot_get_strings(ocelot, port, stringset, data);
 879 }
 880
 881 static void felix_get_ethtool_stats(struct dsa_switch *ds, int port, u64 *data)
 882 {
 883         struct ocelot *ocelot = ds->priv;
 884
 885         ocelot_get_ethtool_stats(ocelot, port, data);
 886 }
 887
 888 static int felix_get_sset_count(struct dsa_switch *ds, int port, int sset)
 889 {
 890         struct ocelot *ocelot = ds->priv;
 891
 892         return ocelot_get_sset_count(ocelot, port, sset);
 893 }
 894
 895 static int felix_get_ts_info(struct dsa_switch *ds, int port,
 896                              struct ethtool_ts_info *info)
 897 {
 898         struct ocelot *ocelot = ds->priv;
 899
 900         return ocelot_get_ts_info(ocelot, port, info);
 901 }
 902
 903 static int felix_parse_ports_node(struct felix *felix,
 904                                   struct device_node *ports_node,
 905                                   phy_interface_t *port_phy_modes)
 906 {
 907         struct ocelot *ocelot = &felix->ocelot;
 908         struct device *dev = felix->ocelot.dev;
 909         struct device_node *child;
 910
 911         for_each_available_child_of_node(ports_node, child) {
 912                 phy_interface_t phy_mode;
 913                 u32 port;
 914                 int err;
 915
 916                 /* Get switch port number from DT */
 917                 if (of_property_read_u32(child, "reg", &port) < 0) {
 918                         dev_err(dev, "Port number not defined in device tree "
 919                                 "(property \"reg\")\n");
 920                         of_node_put(child);
 921                         return -ENODEV;
 922                 }
 923
 924                 /* Get PHY mode from DT */
 925                 err = of_get_phy_mode(child, &phy_mode);
 926                 if (err) {
 927                         dev_err(dev, "Failed to read phy-mode or "
 928                                 "phy-interface-type property for port %d\n",
 929                                 port);
 930                         of_node_put(child);
 931                         return -ENODEV;
 932                 }
 933
 934                 err = felix->info->prevalidate_phy_mode(ocelot, port, phy_mode);
 935                 if (err < 0) {
 936                         dev_err(dev, "Unsupported PHY mode %s on port %d\n",
 937                                 phy_modes(phy_mode), port);
 938                         of_node_put(child);
 939                         return err;
 940                 }
 941
 942                 port_phy_modes[port] = phy_mode;
 943         }
 944
 945         return 0;
 946 }
 947
 948 static int felix_parse_dt(struct felix *felix, phy_interface_t *port_phy_modes)
 949 {
 950         struct device *dev = felix->ocelot.dev;
 951         struct device_node *switch_node;
 952         struct device_node *ports_node;
 953         int err;
 954
 955         switch_node = dev->of_node;
 956
 957         ports_node = of_get_child_by_name(switch_node, "ports");
 958         if (!ports_node) {
 959                 dev_err(dev, "Incorrect bindings: absent \"ports\" node\n");
 960                 return -ENODEV;
 961         }
 962
 963         err = felix_parse_ports_node(felix, ports_node, port_phy_modes);
 964         of_node_put(ports_node);
 965
 966         return err;
 967 }
 968
 969 static int felix_init_structs(struct felix *felix, int num_phys_ports)
 970 {
 971         struct ocelot *ocelot = &felix->ocelot;
 972         phy_interface_t *port_phy_modes;
 973         struct resource res;
 974         int port, i, err;
 975
 976         ocelot->num_phys_ports = num_phys_ports;
 977         ocelot->ports = devm_kcalloc(ocelot->dev, num_phys_ports,
 978                                      sizeof(struct ocelot_port *), GFP_KERNEL);
 979         if (!ocelot->ports)
 980                 return -ENOMEM;
 981
 982         ocelot->map             = felix->info->map;
 983         ocelot->stats_layout    = felix->info->stats_layout;
 984         ocelot->num_stats       = felix->info->num_stats;
 985         ocelot->num_mact_rows   = felix->info->num_mact_rows;
 986         ocelot->vcap            = felix->info->vcap;
 987         ocelot->ops             = felix->info->ops;
 988         ocelot->npi_inj_prefix  = OCELOT_TAG_PREFIX_SHORT;
 989         ocelot->npi_xtr_prefix  = OCELOT_TAG_PREFIX_SHORT;
 990         ocelot->devlink         = felix->ds->devlink;
 991
 992         port_phy_modes = kcalloc(num_phys_ports, sizeof(phy_interface_t),
 993                                  GFP_KERNEL);
 994         if (!port_phy_modes)
 995                 return -ENOMEM;
 996
 997         err = felix_parse_dt(felix, port_phy_modes);
 998         if (err) {
 999                 kfree(port_phy_modes);
1000                 return err;
1001         }
1002
1003         for (i = 0; i < TARGET_MAX; i++) {
1004                 struct regmap *target;
1005
1006                 if (!felix->info->target_io_res[i].name)
1007                         continue;
1008
1009                 memcpy(&res, &felix->info->target_io_res[i], sizeof(res));
1010                 res.flags = IORESOURCE_MEM;
1011                 res.start += felix->switch_base;
1012                 res.end += felix->switch_base;
1013
1014                 target = ocelot_regmap_init(ocelot, &res);
1015                 if (IS_ERR(target)) {
1016                         dev_err(ocelot->dev,
1017                                 "Failed to map device memory space\n");
1018                         kfree(port_phy_modes);
1019                         return PTR_ERR(target);
1020                 }
1021
1022                 ocelot->targets[i] = target;
1023         }
1024
1025         err = ocelot_regfields_init(ocelot, felix->info->regfields);
1026         if (err) {
1027                 dev_err(ocelot->dev, "failed to init reg fields map\n");
1028                 kfree(port_phy_modes);
1029                 return err;
1030         }
1031
1032         for (port = 0; port < num_phys_ports; port++) {
1033                 struct ocelot_port *ocelot_port;
1034                 struct regmap *target;
1035
1036                 ocelot_port = devm_kzalloc(ocelot->dev,
1037                                            sizeof(struct ocelot_port),
1038                                            GFP_KERNEL);
1039                 if (!ocelot_port) {
1040                         dev_err(ocelot->dev,
1041                                 "failed to allocate port memory\n");
1042                         kfree(port_phy_modes);
1043                         return -ENOMEM;
1044                 }
1045
1046                 memcpy(&res, &felix->info->port_io_res[port], sizeof(res));
1047                 res.flags = IORESOURCE_MEM;
1048                 res.start += felix->switch_base;
1049                 res.end += felix->switch_base;
1050
1051                 target = ocelot_regmap_init(ocelot, &res);
1052                 if (IS_ERR(target)) {
1053                         dev_err(ocelot->dev,
1054                                 "Failed to map memory space for port %d\n",
1055                                 port);
1056                         kfree(port_phy_modes);
1057                         return PTR_ERR(target);
1058                 }
1059
1060                 ocelot_port->phy_mode = port_phy_modes[port];
1061                 ocelot_port->ocelot = ocelot;
1062                 ocelot_port->target = target;
1063                 ocelot->ports[port] = ocelot_port;
1064         }
1065
1066         kfree(port_phy_modes);
1067
1068         if (felix->info->mdio_bus_alloc) {
1069                 err = felix->info->mdio_bus_alloc(ocelot);
1070                 if (err < 0)
1071                         return err;
1072         }
1073
1074         return 0;
1075 }
1076
1077 /* Hardware initialization done here so that we can allocate structures with
1078  * devm without fear of dsa_register_switch returning -EPROBE_DEFER and causing
1079  * us to allocate structures twice (leak memory) and map PCI memory twice
1080  * (which will not work).
1081  */
1082 static int felix_setup(struct dsa_switch *ds)
1083 {
1084         struct ocelot *ocelot = ds->priv;
1085         struct felix *felix = ocelot_to_felix(ocelot);
1086         int port, err;
1087
1088         err = felix_init_structs(felix, ds->num_ports);
1089         if (err)
1090                 return err;
1091
1092         err = ocelot_init(ocelot);
1093         if (err)
1094                 goto out_mdiobus_free;
1095
1096         if (ocelot->ptp) {
1097                 err = ocelot_init_timestamp(ocelot, felix->info->ptp_caps);
1098                 if (err) {
1099                         dev_err(ocelot->dev,
1100                                 "Timestamp initialization failed\n");
1101                         ocelot->ptp = 0;
1102                 }
1103         }
1104
1105         for (port = 0; port < ds->num_ports; port++) {
1106                 if (dsa_is_unused_port(ds, port))
1107                         continue;
1108
1109                 ocelot_init_port(ocelot, port);
1110
1111                 /* Set the default QoS Classification based on PCP and DEI
1112                  * bits of vlan tag.
1113                  */
1114                 felix_port_qos_map_init(ocelot, port);
1115         }
1116
1117         err = ocelot_devlink_sb_register(ocelot);
1118         if (err)
1119                 goto out_deinit_ports;
1120
1121         for (port = 0; port < ds->num_ports; port++) {
1122                 if (!dsa_is_cpu_port(ds, port))
1123                         continue;
1124
1125                 /* The initial tag protocol is NPI which always returns 0, so
1126                  * there's no real point in checking for errors.
1127                  */
1128                 felix_set_tag_protocol(ds, port, felix->tag_proto);
1129         }
1130
1131         ds->mtu_enforcement_ingress = true;
1132         ds->assisted_learning_on_cpu_port = true;
1133
1134         return 0;
1135
1136 out_deinit_ports:
1137         for (port = 0; port < ocelot->num_phys_ports; port++) {
1138                 if (dsa_is_unused_port(ds, port))
1139                         continue;
1140
1141                 ocelot_deinit_port(ocelot, port);
1142         }
1143
1144         ocelot_deinit_timestamp(ocelot);
1145         ocelot_deinit(ocelot);
1146
1147 out_mdiobus_free:
1148         if (felix->info->mdio_bus_free)
1149                 felix->info->mdio_bus_free(ocelot);
1150
1151         return err;
1152 }
1153
1154 static void felix_teardown(struct dsa_switch *ds)
1155 {
1156         struct ocelot *ocelot = ds->priv;
1157         struct felix *felix = ocelot_to_felix(ocelot);
1158         int port;
1159
1160         for (port = 0; port < ds->num_ports; port++) {
1161                 if (!dsa_is_cpu_port(ds, port))
1162                         continue;
1163
1164                 felix_del_tag_protocol(ds, port, felix->tag_proto);
1165         }
1166
1167         ocelot_devlink_sb_unregister(ocelot);
1168         ocelot_deinit_timestamp(ocelot);
1169         ocelot_deinit(ocelot);
1170
1171         for (port = 0; port < ocelot->num_phys_ports; port++) {
1172                 if (dsa_is_unused_port(ds, port))
1173                         continue;
1174
1175                 ocelot_deinit_port(ocelot, port);
1176         }
1177
1178         if (felix->info->mdio_bus_free)
1179                 felix->info->mdio_bus_free(ocelot);
1180 }
1181
1182 static int felix_hwtstamp_get(struct dsa_switch *ds, int port,
1183                               struct ifreq *ifr)
1184 {
1185         struct ocelot *ocelot = ds->priv;
1186
1187         return ocelot_hwstamp_get(ocelot, port, ifr);
1188 }
1189
1190 static int felix_hwtstamp_set(struct dsa_switch *ds, int port,
1191                               struct ifreq *ifr)
1192 {
1193         struct ocelot *ocelot = ds->priv;
1194
1195         return ocelot_hwstamp_set(ocelot, port, ifr);
1196 }
1197
1198 static bool felix_check_xtr_pkt(struct ocelot *ocelot, unsigned int ptp_type)
1199 {
1200         struct felix *felix = ocelot_to_felix(ocelot);
1201         int err, grp = 0;
1202
1203         if (felix->tag_proto != DSA_TAG_PROTO_OCELOT_8021Q)
1204                 return false;
1205
1206         if (!felix->info->quirk_no_xtr_irq)
1207                 return false;
1208
1209         if (ptp_type == PTP_CLASS_NONE)
1210                 return false;
1211
1212         while (ocelot_read(ocelot, QS_XTR_DATA_PRESENT) & BIT(grp)) {
1213                 struct sk_buff *skb;
1214                 unsigned int type;
1215
1216                 err = ocelot_xtr_poll_frame(ocelot, grp, &skb);
1217                 if (err)
1218                         goto out;
1219
1220                 /* We trap to the CPU port module all PTP frames, but
1221                  * felix_rxtstamp() only gets called for event frames.
1222                  * So we need to avoid sending duplicate general
1223                  * message frames by running a second BPF classifier
1224                  * here and dropping those.
1225                  */
1226                 __skb_push(skb, ETH_HLEN);
1227
1228                 type = ptp_classify_raw(skb);
1229
1230                 __skb_pull(skb, ETH_HLEN);
1231
1232                 if (type == PTP_CLASS_NONE) {
1233                         kfree_skb(skb);
1234                         continue;
1235                 }
1236
1237                 netif_rx(skb);
1238         }
1239
1240 out:
1241         if (err < 0)
1242                 ocelot_drain_cpu_queue(ocelot, 0);
1243
1244         return true;
1245 }
1246
1247 static bool felix_rxtstamp(struct dsa_switch *ds, int port,
1248                            struct sk_buff *skb, unsigned int type)
1249 {
1250         u8 *extraction = skb->data - ETH_HLEN - OCELOT_TAG_LEN;
1251         struct skb_shared_hwtstamps *shhwtstamps;
1252         struct ocelot *ocelot = ds->priv;
1253         u32 tstamp_lo, tstamp_hi;
1254         struct timespec64 ts;
1255         u64 tstamp, val;
1256
1257         /* If the "no XTR IRQ" workaround is in use, tell DSA to defer this skb
1258          * for RX timestamping. Then free it, and poll for its copy through
1259          * MMIO in the CPU port module, and inject that into the stack from
1260          * ocelot_xtr_poll().
1261          */
1262         if (felix_check_xtr_pkt(ocelot, type)) {
1263                 kfree_skb(skb);
1264                 return true;
1265         }
1266
1267         ocelot_ptp_gettime64(&ocelot->ptp_info, &ts);
1268         tstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
1269
1270         ocelot_xfh_get_rew_val(extraction, &val);
1271         tstamp_lo = (u32)val;
1272
1273         tstamp_hi = tstamp >> 32;
1274         if ((tstamp & 0xffffffff) < tstamp_lo)
1275                 tstamp_hi--;
1276
1277         tstamp = ((u64)tstamp_hi << 32) | tstamp_lo;
1278
1279         shhwtstamps = skb_hwtstamps(skb);
1280         memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
1281         shhwtstamps->hwtstamp = tstamp;
1282         return false;
1283 }
1284
1285 static void felix_txtstamp(struct dsa_switch *ds, int port,
1286                            struct sk_buff *skb)
1287 {
1288         struct ocelot *ocelot = ds->priv;
1289         struct sk_buff *clone = NULL;
1290
1291         if (!ocelot->ptp)
1292                 return;
1293
1294         if (ocelot_port_txtstamp_request(ocelot, port, skb, &clone))
1295                 return;
1296
1297         if (clone)
1298                 OCELOT_SKB_CB(skb)->clone = clone;
1299 }
1300
1301 static int felix_change_mtu(struct dsa_switch *ds, int port, int new_mtu)
1302 {
1303         struct ocelot *ocelot = ds->priv;
1304
1305         ocelot_port_set_maxlen(ocelot, port, new_mtu);
1306
1307         return 0;
1308 }
1309
1310 static int felix_get_max_mtu(struct dsa_switch *ds, int port)
1311 {
1312         struct ocelot *ocelot = ds->priv;
1313
1314         return ocelot_get_max_mtu(ocelot, port);
1315 }
1316
1317 static int felix_cls_flower_add(struct dsa_switch *ds, int port,
1318                                 struct flow_cls_offload *cls, bool ingress)
1319 {
1320         struct ocelot *ocelot = ds->priv;
1321
1322         return ocelot_cls_flower_replace(ocelot, port, cls, ingress);
1323 }
1324
1325 static int felix_cls_flower_del(struct dsa_switch *ds, int port,
1326                                 struct flow_cls_offload *cls, bool ingress)
1327 {
1328         struct ocelot *ocelot = ds->priv;
1329
1330         return ocelot_cls_flower_destroy(ocelot, port, cls, ingress);
1331 }
1332
1333 static int felix_cls_flower_stats(struct dsa_switch *ds, int port,
1334                                   struct flow_cls_offload *cls, bool ingress)
1335 {
1336         struct ocelot *ocelot = ds->priv;
1337
1338         return ocelot_cls_flower_stats(ocelot, port, cls, ingress);
1339 }
1340
1341 static int felix_port_policer_add(struct dsa_switch *ds, int port,
1342                                   struct dsa_mall_policer_tc_entry *policer)
1343 {
1344         struct ocelot *ocelot = ds->priv;
1345         struct ocelot_policer pol = {
1346                 .rate = div_u64(policer->rate_bytes_per_sec, 1000) * 8,
1347                 .burst = policer->burst,
1348         };
1349
1350         return ocelot_port_policer_add(ocelot, port, &pol);
1351 }
1352
1353 static void felix_port_policer_del(struct dsa_switch *ds, int port)
1354 {
1355         struct ocelot *ocelot = ds->priv;
1356
1357         ocelot_port_policer_del(ocelot, port);
1358 }
1359
1360 static int felix_port_setup_tc(struct dsa_switch *ds, int port,
1361                                enum tc_setup_type type,
1362                                void *type_data)
1363 {
1364         struct ocelot *ocelot = ds->priv;
1365         struct felix *felix = ocelot_to_felix(ocelot);
1366
1367         if (felix->info->port_setup_tc)
1368                 return felix->info->port_setup_tc(ds, port, type, type_data);
1369         else
1370                 return -EOPNOTSUPP;
1371 }
1372
1373 static int felix_sb_pool_get(struct dsa_switch *ds, unsigned int sb_index,
1374                              u16 pool_index,
1375                              struct devlink_sb_pool_info *pool_info)
1376 {
1377         struct ocelot *ocelot = ds->priv;
1378
1379         return ocelot_sb_pool_get(ocelot, sb_index, pool_index, pool_info);
1380 }
1381
1382 static int felix_sb_pool_set(struct dsa_switch *ds, unsigned int sb_index,
1383                              u16 pool_index, u32 size,
1384                              enum devlink_sb_threshold_type threshold_type,
1385                              struct netlink_ext_ack *extack)
1386 {
1387         struct ocelot *ocelot = ds->priv;
1388
1389         return ocelot_sb_pool_set(ocelot, sb_index, pool_index, size,
1390                                   threshold_type, extack);
1391 }
1392
1393 static int felix_sb_port_pool_get(struct dsa_switch *ds, int port,
1394                                   unsigned int sb_index, u16 pool_index,
1395                                   u32 *p_threshold)
1396 {
1397         struct ocelot *ocelot = ds->priv;
1398
1399         return ocelot_sb_port_pool_get(ocelot, port, sb_index, pool_index,
1400                                        p_threshold);
1401 }
1402
1403 static int felix_sb_port_pool_set(struct dsa_switch *ds, int port,
1404                                   unsigned int sb_index, u16 pool_index,
1405                                   u32 threshold, struct netlink_ext_ack *extack)
1406 {
1407         struct ocelot *ocelot = ds->priv;
1408
1409         return ocelot_sb_port_pool_set(ocelot, port, sb_index, pool_index,
1410                                        threshold, extack);
1411 }
1412
1413 static int felix_sb_tc_pool_bind_get(struct dsa_switch *ds, int port,
1414                                      unsigned int sb_index, u16 tc_index,
1415                                      enum devlink_sb_pool_type pool_type,
1416                                      u16 *p_pool_index, u32 *p_threshold)
1417 {
1418         struct ocelot *ocelot = ds->priv;
1419
1420         return ocelot_sb_tc_pool_bind_get(ocelot, port, sb_index, tc_index,
1421                                           pool_type, p_pool_index,
1422                                           p_threshold);
1423 }
1424
1425 static int felix_sb_tc_pool_bind_set(struct dsa_switch *ds, int port,
1426                                      unsigned int sb_index, u16 tc_index,
1427                                      enum devlink_sb_pool_type pool_type,
1428                                      u16 pool_index, u32 threshold,
1429                                      struct netlink_ext_ack *extack)
1430 {
1431         struct ocelot *ocelot = ds->priv;
1432
1433         return ocelot_sb_tc_pool_bind_set(ocelot, port, sb_index, tc_index,
1434                                           pool_type, pool_index, threshold,
1435                                           extack);
1436 }
1437
1438 static int felix_sb_occ_snapshot(struct dsa_switch *ds,
1439                                  unsigned int sb_index)
1440 {
1441         struct ocelot *ocelot = ds->priv;
1442
1443         return ocelot_sb_occ_snapshot(ocelot, sb_index);
1444 }
1445
1446 static int felix_sb_occ_max_clear(struct dsa_switch *ds,
1447                                   unsigned int sb_index)
1448 {
1449         struct ocelot *ocelot = ds->priv;
1450
1451         return ocelot_sb_occ_max_clear(ocelot, sb_index);
1452 }
1453
1454 static int felix_sb_occ_port_pool_get(struct dsa_switch *ds, int port,
1455                                       unsigned int sb_index, u16 pool_index,
1456                                       u32 *p_cur, u32 *p_max)
1457 {
1458         struct ocelot *ocelot = ds->priv;
1459
1460         return ocelot_sb_occ_port_pool_get(ocelot, port, sb_index, pool_index,
1461                                            p_cur, p_max);
1462 }
1463
1464 static int felix_sb_occ_tc_port_bind_get(struct dsa_switch *ds, int port,
1465                                          unsigned int sb_index, u16 tc_index,
1466                                          enum devlink_sb_pool_type pool_type,
1467                                          u32 *p_cur, u32 *p_max)
1468 {
1469         struct ocelot *ocelot = ds->priv;
1470
1471         return ocelot_sb_occ_tc_port_bind_get(ocelot, port, sb_index, tc_index,
1472                                               pool_type, p_cur, p_max);
1473 }
1474
1475 static int felix_mrp_add(struct dsa_switch *ds, int port,
1476                          const struct switchdev_obj_mrp *mrp)
1477 {
1478         struct ocelot *ocelot = ds->priv;
1479
1480         return ocelot_mrp_add(ocelot, port, mrp);
1481 }
1482
1483 static int felix_mrp_del(struct dsa_switch *ds, int port,
1484                          const struct switchdev_obj_mrp *mrp)
1485 {
1486         struct ocelot *ocelot = ds->priv;
1487
1488         return ocelot_mrp_add(ocelot, port, mrp);
1489 }
1490
1491 static int
1492 felix_mrp_add_ring_role(struct dsa_switch *ds, int port,
1493                         const struct switchdev_obj_ring_role_mrp *mrp)
1494 {
1495         struct ocelot *ocelot = ds->priv;
1496
1497         return ocelot_mrp_add_ring_role(ocelot, port, mrp);
1498 }
1499
1500 static int
1501 felix_mrp_del_ring_role(struct dsa_switch *ds, int port,
1502                         const struct switchdev_obj_ring_role_mrp *mrp)
1503 {
1504         struct ocelot *ocelot = ds->priv;
1505
1506         return ocelot_mrp_del_ring_role(ocelot, port, mrp);
1507 }
1508
1509 const struct dsa_switch_ops felix_switch_ops = {
1510         .get_tag_protocol               = felix_get_tag_protocol,
1511         .change_tag_protocol            = felix_change_tag_protocol,
1512         .setup                          = felix_setup,
1513         .teardown                       = felix_teardown,
1514         .set_ageing_time                = felix_set_ageing_time,
1515         .get_strings                    = felix_get_strings,
1516         .get_ethtool_stats              = felix_get_ethtool_stats,
1517         .get_sset_count                 = felix_get_sset_count,
1518         .get_ts_info                    = felix_get_ts_info,
1519         .phylink_validate               = felix_phylink_validate,
1520         .phylink_mac_config             = felix_phylink_mac_config,
1521         .phylink_mac_link_down          = felix_phylink_mac_link_down,
1522         .phylink_mac_link_up            = felix_phylink_mac_link_up,
1523         .port_fdb_dump                  = felix_fdb_dump,
1524         .port_fdb_add                   = felix_fdb_add,
1525         .port_fdb_del                   = felix_fdb_del,
1526         .port_mdb_add                   = felix_mdb_add,
1527         .port_mdb_del                   = felix_mdb_del,
1528         .port_pre_bridge_flags          = felix_pre_bridge_flags,
1529         .port_bridge_flags              = felix_bridge_flags,
1530         .port_bridge_join               = felix_bridge_join,
1531         .port_bridge_leave              = felix_bridge_leave,
1532         .port_lag_join                  = felix_lag_join,
1533         .port_lag_leave                 = felix_lag_leave,
1534         .port_lag_change                = felix_lag_change,
1535         .port_stp_state_set             = felix_bridge_stp_state_set,
1536         .port_vlan_filtering            = felix_vlan_filtering,
1537         .port_vlan_add                  = felix_vlan_add,
1538         .port_vlan_del                  = felix_vlan_del,
1539         .port_hwtstamp_get              = felix_hwtstamp_get,
1540         .port_hwtstamp_set              = felix_hwtstamp_set,
1541         .port_rxtstamp                  = felix_rxtstamp,
1542         .port_txtstamp                  = felix_txtstamp,
1543         .port_change_mtu                = felix_change_mtu,
1544         .port_max_mtu                   = felix_get_max_mtu,
1545         .port_policer_add               = felix_port_policer_add,
1546         .port_policer_del               = felix_port_policer_del,
1547         .cls_flower_add                 = felix_cls_flower_add,
1548         .cls_flower_del                 = felix_cls_flower_del,
1549         .cls_flower_stats               = felix_cls_flower_stats,
1550         .port_setup_tc                  = felix_port_setup_tc,
1551         .devlink_sb_pool_get            = felix_sb_pool_get,
1552         .devlink_sb_pool_set            = felix_sb_pool_set,
1553         .devlink_sb_port_pool_get       = felix_sb_port_pool_get,
1554         .devlink_sb_port_pool_set       = felix_sb_port_pool_set,
1555         .devlink_sb_tc_pool_bind_get    = felix_sb_tc_pool_bind_get,
1556         .devlink_sb_tc_pool_bind_set    = felix_sb_tc_pool_bind_set,
1557         .devlink_sb_occ_snapshot        = felix_sb_occ_snapshot,
1558         .devlink_sb_occ_max_clear       = felix_sb_occ_max_clear,
1559         .devlink_sb_occ_port_pool_get   = felix_sb_occ_port_pool_get,
1560         .devlink_sb_occ_tc_port_bind_get= felix_sb_occ_tc_port_bind_get,
1561         .port_mrp_add                   = felix_mrp_add,
1562         .port_mrp_del                   = felix_mrp_del,
1563         .port_mrp_add_ring_role         = felix_mrp_add_ring_role,
1564         .port_mrp_del_ring_role         = felix_mrp_del_ring_role,
1565         .tag_8021q_vlan_add             = felix_tag_8021q_vlan_add,
1566         .tag_8021q_vlan_del             = felix_tag_8021q_vlan_del,
1567 };
1568
1569 struct net_device *felix_port_to_netdev(struct ocelot *ocelot, int port)
1570 {
1571         struct felix *felix = ocelot_to_felix(ocelot);
1572         struct dsa_switch *ds = felix->ds;
1573
1574         if (!dsa_is_user_port(ds, port))
1575                 return NULL;
1576
1577         return dsa_to_port(ds, port)->slave;
1578 }
1579
1580 int felix_netdev_to_port(struct net_device *dev)
1581 {
1582         struct dsa_port *dp;
1583
1584         dp = dsa_port_from_netdev(dev);
1585         if (IS_ERR(dp))
1586                 return -EINVAL;
1587
1588         return dp->index;
1589 }