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