drivers/net/dsa/ocelot/felix.c

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