drivers/net/dsa/sja1105/sja1105_main.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2018, Sensor-Technik Wiedemann GmbH
   3  * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com>
   4  */
   5
   6 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   7
   8 #include <linux/delay.h>
   9 #include <linux/module.h>
  10 #include <linux/printk.h>
  11 #include <linux/spi/spi.h>
  12 #include <linux/errno.h>
  13 #include <linux/gpio/consumer.h>
  14 #include <linux/phylink.h>
  15 #include <linux/of.h>
  16 #include <linux/of_net.h>
  17 #include <linux/of_mdio.h>
  18 #include <linux/of_device.h>
  19 #include <linux/netdev_features.h>
  20 #include <linux/netdevice.h>
  21 #include <linux/if_bridge.h>
  22 #include <linux/if_ether.h>
  23 #include <linux/dsa/8021q.h>
  24 #include "sja1105.h"
  25 #include "sja1105_sgmii.h"
  26 #include "sja1105_tas.h"
  27
  28 #define SJA1105_UNKNOWN_MULTICAST       0x010000000000ull
  29 #define SJA1105_DEFAULT_VLAN            (VLAN_N_VID - 1)
  30
  31 static const struct dsa_switch_ops sja1105_switch_ops;
  32
  33 static void sja1105_hw_reset(struct gpio_desc *gpio, unsigned int pulse_len,
  34                              unsigned int startup_delay)
  35 {
  36         gpiod_set_value_cansleep(gpio, 1);
  37         /* Wait for minimum reset pulse length */
  38         msleep(pulse_len);
  39         gpiod_set_value_cansleep(gpio, 0);
  40         /* Wait until chip is ready after reset */
  41         msleep(startup_delay);
  42 }
  43
  44 static void
  45 sja1105_port_allow_traffic(struct sja1105_l2_forwarding_entry *l2_fwd,
  46                            int from, int to, bool allow)
  47 {
  48         if (allow)
  49                 l2_fwd[from].reach_port |= BIT(to);
  50         else
  51                 l2_fwd[from].reach_port &= ~BIT(to);
  52 }
  53
  54 static bool sja1105_can_forward(struct sja1105_l2_forwarding_entry *l2_fwd,
  55                                 int from, int to)
  56 {
  57         return !!(l2_fwd[from].reach_port & BIT(to));
  58 }
  59
  60 /* Structure used to temporarily transport device tree
  61  * settings into sja1105_setup
  62  */
  63 struct sja1105_dt_port {
  64         phy_interface_t phy_mode;
  65         sja1105_mii_role_t role;
  66 };
  67
  68 static int sja1105_init_mac_settings(struct sja1105_private *priv)
  69 {
  70         struct sja1105_mac_config_entry default_mac = {
  71                 /* Enable all 8 priority queues on egress.
  72                  * Every queue i holds top[i] - base[i] frames.
  73                  * Sum of top[i] - base[i] is 511 (max hardware limit).
  74                  */
  75                 .top  = {0x3F, 0x7F, 0xBF, 0xFF, 0x13F, 0x17F, 0x1BF, 0x1FF},
  76                 .base = {0x0, 0x40, 0x80, 0xC0, 0x100, 0x140, 0x180, 0x1C0},
  77                 .enabled = {true, true, true, true, true, true, true, true},
  78                 /* Keep standard IFG of 12 bytes on egress. */
  79                 .ifg = 0,
  80                 /* Always put the MAC speed in automatic mode, where it can be
  81                  * adjusted at runtime by PHYLINK.
  82                  */
  83                 .speed = priv->info->port_speed[SJA1105_SPEED_AUTO],
  84                 /* No static correction for 1-step 1588 events */
  85                 .tp_delin = 0,
  86                 .tp_delout = 0,
  87                 /* Disable aging for critical TTEthernet traffic */
  88                 .maxage = 0xFF,
  89                 /* Internal VLAN (pvid) to apply to untagged ingress */
  90                 .vlanprio = 0,
  91                 .vlanid = 1,
  92                 .ing_mirr = false,
  93                 .egr_mirr = false,
  94                 /* Don't drop traffic with other EtherType than ETH_P_IP */
  95                 .drpnona664 = false,
  96                 /* Don't drop double-tagged traffic */
  97                 .drpdtag = false,
  98                 /* Don't drop untagged traffic */
  99                 .drpuntag = false,
 100                 /* Don't retag 802.1p (VID 0) traffic with the pvid */
 101                 .retag = false,
 102                 /* Disable learning and I/O on user ports by default -
 103                  * STP will enable it.
 104                  */
 105                 .dyn_learn = false,
 106                 .egress = false,
 107                 .ingress = false,
 108         };
 109         struct sja1105_mac_config_entry *mac;
 110         struct dsa_switch *ds = priv->ds;
 111         struct sja1105_table *table;
 112         int i;
 113
 114         table = &priv->static_config.tables[BLK_IDX_MAC_CONFIG];
 115
 116         /* Discard previous MAC Configuration Table */
 117         if (table->entry_count) {
 118                 kfree(table->entries);
 119                 table->entry_count = 0;
 120         }
 121
 122         table->entries = kcalloc(table->ops->max_entry_count,
 123                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 124         if (!table->entries)
 125                 return -ENOMEM;
 126
 127         table->entry_count = table->ops->max_entry_count;
 128
 129         mac = table->entries;
 130
 131         for (i = 0; i < ds->num_ports; i++) {
 132                 mac[i] = default_mac;
 133                 if (i == dsa_upstream_port(priv->ds, i)) {
 134                         /* STP doesn't get called for CPU port, so we need to
 135                          * set the I/O parameters statically.
 136                          */
 137                         mac[i].dyn_learn = true;
 138                         mac[i].ingress = true;
 139                         mac[i].egress = true;
 140                 }
 141         }
 142
 143         return 0;
 144 }
 145
 146 static int sja1105_init_mii_settings(struct sja1105_private *priv,
 147                                      struct sja1105_dt_port *ports)
 148 {
 149         struct device *dev = &priv->spidev->dev;
 150         struct sja1105_xmii_params_entry *mii;
 151         struct dsa_switch *ds = priv->ds;
 152         struct sja1105_table *table;
 153         int i;
 154
 155         table = &priv->static_config.tables[BLK_IDX_XMII_PARAMS];
 156
 157         /* Discard previous xMII Mode Parameters Table */
 158         if (table->entry_count) {
 159                 kfree(table->entries);
 160                 table->entry_count = 0;
 161         }
 162
 163         table->entries = kcalloc(table->ops->max_entry_count,
 164                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 165         if (!table->entries)
 166                 return -ENOMEM;
 167
 168         /* Override table based on PHYLINK DT bindings */
 169         table->entry_count = table->ops->max_entry_count;
 170
 171         mii = table->entries;
 172
 173         for (i = 0; i < ds->num_ports; i++) {
 174                 if (dsa_is_unused_port(priv->ds, i))
 175                         continue;
 176
 177                 switch (ports[i].phy_mode) {
 178                 case PHY_INTERFACE_MODE_MII:
 179                         if (!priv->info->supports_mii[i])
 180                                 goto unsupported;
 181
 182                         mii->xmii_mode[i] = XMII_MODE_MII;
 183                         break;
 184                 case PHY_INTERFACE_MODE_RMII:
 185                         if (!priv->info->supports_rmii[i])
 186                                 goto unsupported;
 187
 188                         mii->xmii_mode[i] = XMII_MODE_RMII;
 189                         break;
 190                 case PHY_INTERFACE_MODE_RGMII:
 191                 case PHY_INTERFACE_MODE_RGMII_ID:
 192                 case PHY_INTERFACE_MODE_RGMII_RXID:
 193                 case PHY_INTERFACE_MODE_RGMII_TXID:
 194                         if (!priv->info->supports_rgmii[i])
 195                                 goto unsupported;
 196
 197                         mii->xmii_mode[i] = XMII_MODE_RGMII;
 198                         break;
 199                 case PHY_INTERFACE_MODE_SGMII:
 200                         if (!priv->info->supports_sgmii[i])
 201                                 goto unsupported;
 202
 203                         mii->xmii_mode[i] = XMII_MODE_SGMII;
 204                         break;
 205                 case PHY_INTERFACE_MODE_2500BASEX:
 206                         if (!priv->info->supports_2500basex[i])
 207                                 goto unsupported;
 208
 209                         mii->xmii_mode[i] = XMII_MODE_SGMII;
 210                         break;
 211 unsupported:
 212                 default:
 213                         dev_err(dev, "Unsupported PHY mode %s on port %d!\n",
 214                                 phy_modes(ports[i].phy_mode), i);
 215                         return -EINVAL;
 216                 }
 217
 218                 /* Even though the SerDes port is able to drive SGMII autoneg
 219                  * like a PHY would, from the perspective of the XMII tables,
 220                  * the SGMII port should always be put in MAC mode.
 221                  */
 222                 if (ports[i].phy_mode == PHY_INTERFACE_MODE_SGMII)
 223                         mii->phy_mac[i] = XMII_MAC;
 224                 else
 225                         mii->phy_mac[i] = ports[i].role;
 226         }
 227         return 0;
 228 }
 229
 230 static int sja1105_init_static_fdb(struct sja1105_private *priv)
 231 {
 232         struct sja1105_l2_lookup_entry *l2_lookup;
 233         struct sja1105_table *table;
 234         int port;
 235
 236         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
 237
 238         /* We only populate the FDB table through dynamic L2 Address Lookup
 239          * entries, except for a special entry at the end which is a catch-all
 240          * for unknown multicast and will be used to control flooding domain.
 241          */
 242         if (table->entry_count) {
 243                 kfree(table->entries);
 244                 table->entry_count = 0;
 245         }
 246
 247         if (!priv->info->can_limit_mcast_flood)
 248                 return 0;
 249
 250         table->entries = kcalloc(1, table->ops->unpacked_entry_size,
 251                                  GFP_KERNEL);
 252         if (!table->entries)
 253                 return -ENOMEM;
 254
 255         table->entry_count = 1;
 256         l2_lookup = table->entries;
 257
 258         /* All L2 multicast addresses have an odd first octet */
 259         l2_lookup[0].macaddr = SJA1105_UNKNOWN_MULTICAST;
 260         l2_lookup[0].mask_macaddr = SJA1105_UNKNOWN_MULTICAST;
 261         l2_lookup[0].lockeds = true;
 262         l2_lookup[0].index = SJA1105_MAX_L2_LOOKUP_COUNT - 1;
 263
 264         /* Flood multicast to every port by default */
 265         for (port = 0; port < priv->ds->num_ports; port++)
 266                 if (!dsa_is_unused_port(priv->ds, port))
 267                         l2_lookup[0].destports |= BIT(port);
 268
 269         return 0;
 270 }
 271
 272 static int sja1105_init_l2_lookup_params(struct sja1105_private *priv)
 273 {
 274         struct sja1105_l2_lookup_params_entry default_l2_lookup_params = {
 275                 /* Learned FDB entries are forgotten after 300 seconds */
 276                 .maxage = SJA1105_AGEING_TIME_MS(300000),
 277                 /* All entries within a FDB bin are available for learning */
 278                 .dyn_tbsz = SJA1105ET_FDB_BIN_SIZE,
 279                 /* And the P/Q/R/S equivalent setting: */
 280                 .start_dynspc = 0,
 281                 /* 2^8 + 2^5 + 2^3 + 2^2 + 2^1 + 1 in Koopman notation */
 282                 .poly = 0x97,
 283                 /* This selects between Independent VLAN Learning (IVL) and
 284                  * Shared VLAN Learning (SVL)
 285                  */
 286                 .shared_learn = true,
 287                 /* Don't discard management traffic based on ENFPORT -
 288                  * we don't perform SMAC port enforcement anyway, so
 289                  * what we are setting here doesn't matter.
 290                  */
 291                 .no_enf_hostprt = false,
 292                 /* Don't learn SMAC for mac_fltres1 and mac_fltres0.
 293                  * Maybe correlate with no_linklocal_learn from bridge driver?
 294                  */
 295                 .no_mgmt_learn = true,
 296                 /* P/Q/R/S only */
 297                 .use_static = true,
 298                 /* Dynamically learned FDB entries can overwrite other (older)
 299                  * dynamic FDB entries
 300                  */
 301                 .owr_dyn = true,
 302                 .drpnolearn = true,
 303         };
 304         struct dsa_switch *ds = priv->ds;
 305         int port, num_used_ports = 0;
 306         struct sja1105_table *table;
 307         u64 max_fdb_entries;
 308
 309         for (port = 0; port < ds->num_ports; port++)
 310                 if (!dsa_is_unused_port(ds, port))
 311                         num_used_ports++;
 312
 313         max_fdb_entries = SJA1105_MAX_L2_LOOKUP_COUNT / num_used_ports;
 314
 315         for (port = 0; port < ds->num_ports; port++) {
 316                 if (dsa_is_unused_port(ds, port))
 317                         continue;
 318
 319                 default_l2_lookup_params.maxaddrp[port] = max_fdb_entries;
 320         }
 321
 322         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP_PARAMS];
 323
 324         if (table->entry_count) {
 325                 kfree(table->entries);
 326                 table->entry_count = 0;
 327         }
 328
 329         table->entries = kcalloc(table->ops->max_entry_count,
 330                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 331         if (!table->entries)
 332                 return -ENOMEM;
 333
 334         table->entry_count = table->ops->max_entry_count;
 335
 336         /* This table only has a single entry */
 337         ((struct sja1105_l2_lookup_params_entry *)table->entries)[0] =
 338                                 default_l2_lookup_params;
 339
 340         return 0;
 341 }
 342
 343 /* Set up a default VLAN for untagged traffic injected from the CPU
 344  * using management routes (e.g. STP, PTP) as opposed to tag_8021q.
 345  * All DT-defined ports are members of this VLAN, and there are no
 346  * restrictions on forwarding (since the CPU selects the destination).
 347  * Frames from this VLAN will always be transmitted as untagged, and
 348  * neither the bridge nor the 8021q module cannot create this VLAN ID.
 349  */
 350 static int sja1105_init_static_vlan(struct sja1105_private *priv)
 351 {
 352         struct sja1105_table *table;
 353         struct sja1105_vlan_lookup_entry pvid = {
 354                 .ving_mirr = 0,
 355                 .vegr_mirr = 0,
 356                 .vmemb_port = 0,
 357                 .vlan_bc = 0,
 358                 .tag_port = 0,
 359                 .vlanid = SJA1105_DEFAULT_VLAN,
 360         };
 361         struct dsa_switch *ds = priv->ds;
 362         int port;
 363
 364         table = &priv->static_config.tables[BLK_IDX_VLAN_LOOKUP];
 365
 366         if (table->entry_count) {
 367                 kfree(table->entries);
 368                 table->entry_count = 0;
 369         }
 370
 371         table->entries = kzalloc(table->ops->unpacked_entry_size,
 372                                  GFP_KERNEL);
 373         if (!table->entries)
 374                 return -ENOMEM;
 375
 376         table->entry_count = 1;
 377
 378         for (port = 0; port < ds->num_ports; port++) {
 379                 struct sja1105_bridge_vlan *v;
 380
 381                 if (dsa_is_unused_port(ds, port))
 382                         continue;
 383
 384                 pvid.vmemb_port |= BIT(port);
 385                 pvid.vlan_bc |= BIT(port);
 386                 pvid.tag_port &= ~BIT(port);
 387
 388                 v = kzalloc(sizeof(*v), GFP_KERNEL);
 389                 if (!v)
 390                         return -ENOMEM;
 391
 392                 v->port = port;
 393                 v->vid = SJA1105_DEFAULT_VLAN;
 394                 v->untagged = true;
 395                 if (dsa_is_cpu_port(ds, port))
 396                         v->pvid = true;
 397                 list_add(&v->list, &priv->dsa_8021q_vlans);
 398         }
 399
 400         ((struct sja1105_vlan_lookup_entry *)table->entries)[0] = pvid;
 401         return 0;
 402 }
 403
 404 static int sja1105_init_l2_forwarding(struct sja1105_private *priv)
 405 {
 406         struct sja1105_l2_forwarding_entry *l2fwd;
 407         struct dsa_switch *ds = priv->ds;
 408         struct sja1105_table *table;
 409         int i, j;
 410
 411         table = &priv->static_config.tables[BLK_IDX_L2_FORWARDING];
 412
 413         if (table->entry_count) {
 414                 kfree(table->entries);
 415                 table->entry_count = 0;
 416         }
 417
 418         table->entries = kcalloc(table->ops->max_entry_count,
 419                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 420         if (!table->entries)
 421                 return -ENOMEM;
 422
 423         table->entry_count = table->ops->max_entry_count;
 424
 425         l2fwd = table->entries;
 426
 427         /* First 5 entries define the forwarding rules */
 428         for (i = 0; i < ds->num_ports; i++) {
 429                 unsigned int upstream = dsa_upstream_port(priv->ds, i);
 430
 431                 if (dsa_is_unused_port(ds, i))
 432                         continue;
 433
 434                 for (j = 0; j < SJA1105_NUM_TC; j++)
 435                         l2fwd[i].vlan_pmap[j] = j;
 436
 437                 /* All ports start up with egress flooding enabled,
 438                  * including the CPU port.
 439                  */
 440                 priv->ucast_egress_floods |= BIT(i);
 441                 priv->bcast_egress_floods |= BIT(i);
 442
 443                 if (i == upstream)
 444                         continue;
 445
 446                 sja1105_port_allow_traffic(l2fwd, i, upstream, true);
 447                 sja1105_port_allow_traffic(l2fwd, upstream, i, true);
 448
 449                 l2fwd[i].bc_domain = BIT(upstream);
 450                 l2fwd[i].fl_domain = BIT(upstream);
 451
 452                 l2fwd[upstream].bc_domain |= BIT(i);
 453                 l2fwd[upstream].fl_domain |= BIT(i);
 454         }
 455
 456         /* Next 8 entries define VLAN PCP mapping from ingress to egress.
 457          * Create a one-to-one mapping.
 458          */
 459         for (i = 0; i < SJA1105_NUM_TC; i++) {
 460                 for (j = 0; j < ds->num_ports; j++) {
 461                         if (dsa_is_unused_port(ds, j))
 462                                 continue;
 463
 464                         l2fwd[ds->num_ports + i].vlan_pmap[j] = i;
 465                 }
 466         }
 467
 468         return 0;
 469 }
 470
 471 static int sja1105_init_l2_forwarding_params(struct sja1105_private *priv)
 472 {
 473         struct sja1105_l2_forwarding_params_entry *l2fwd_params;
 474         struct sja1105_table *table;
 475
 476         table = &priv->static_config.tables[BLK_IDX_L2_FORWARDING_PARAMS];
 477
 478         if (table->entry_count) {
 479                 kfree(table->entries);
 480                 table->entry_count = 0;
 481         }
 482
 483         table->entries = kcalloc(table->ops->max_entry_count,
 484                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 485         if (!table->entries)
 486                 return -ENOMEM;
 487
 488         table->entry_count = table->ops->max_entry_count;
 489
 490         /* This table only has a single entry */
 491         l2fwd_params = table->entries;
 492
 493         /* Disallow dynamic reconfiguration of vlan_pmap */
 494         l2fwd_params->max_dynp = 0;
 495         /* Use a single memory partition for all ingress queues */
 496         l2fwd_params->part_spc[0] = priv->info->max_frame_mem;
 497
 498         return 0;
 499 }
 500
 501 void sja1105_frame_memory_partitioning(struct sja1105_private *priv)
 502 {
 503         struct sja1105_l2_forwarding_params_entry *l2_fwd_params;
 504         struct sja1105_vl_forwarding_params_entry *vl_fwd_params;
 505         int max_mem = priv->info->max_frame_mem;
 506         struct sja1105_table *table;
 507
 508         /* VLAN retagging is implemented using a loopback port that consumes
 509          * frame buffers. That leaves less for us.
 510          */
 511         if (priv->vlan_state == SJA1105_VLAN_BEST_EFFORT)
 512                 max_mem -= SJA1105_FRAME_MEMORY_RETAGGING_OVERHEAD;
 513
 514         table = &priv->static_config.tables[BLK_IDX_L2_FORWARDING_PARAMS];
 515         l2_fwd_params = table->entries;
 516         l2_fwd_params->part_spc[0] = max_mem;
 517
 518         /* If we have any critical-traffic virtual links, we need to reserve
 519          * some frame buffer memory for them. At the moment, hardcode the value
 520          * at 100 blocks of 128 bytes of memory each. This leaves 829 blocks
 521          * remaining for best-effort traffic. TODO: figure out a more flexible
 522          * way to perform the frame buffer partitioning.
 523          */
 524         if (!priv->static_config.tables[BLK_IDX_VL_FORWARDING].entry_count)
 525                 return;
 526
 527         table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING_PARAMS];
 528         vl_fwd_params = table->entries;
 529
 530         l2_fwd_params->part_spc[0] -= SJA1105_VL_FRAME_MEMORY;
 531         vl_fwd_params->partspc[0] = SJA1105_VL_FRAME_MEMORY;
 532 }
 533
 534 static int sja1105_init_general_params(struct sja1105_private *priv)
 535 {
 536         struct sja1105_general_params_entry default_general_params = {
 537                 /* Allow dynamic changing of the mirror port */
 538                 .mirr_ptacu = true,
 539                 .switchid = priv->ds->index,
 540                 /* Priority queue for link-local management frames
 541                  * (both ingress to and egress from CPU - PTP, STP etc)
 542                  */
 543                 .hostprio = 7,
 544                 .mac_fltres1 = SJA1105_LINKLOCAL_FILTER_A,
 545                 .mac_flt1    = SJA1105_LINKLOCAL_FILTER_A_MASK,
 546                 .incl_srcpt1 = false,
 547                 .send_meta1  = false,
 548                 .mac_fltres0 = SJA1105_LINKLOCAL_FILTER_B,
 549                 .mac_flt0    = SJA1105_LINKLOCAL_FILTER_B_MASK,
 550                 .incl_srcpt0 = false,
 551                 .send_meta0  = false,
 552                 /* The destination for traffic matching mac_fltres1 and
 553                  * mac_fltres0 on all ports except host_port. Such traffic
 554                  * receieved on host_port itself would be dropped, except
 555                  * by installing a temporary 'management route'
 556                  */
 557                 .host_port = priv->ds->num_ports,
 558                 /* Default to an invalid value */
 559                 .mirr_port = priv->ds->num_ports,
 560                 /* Link-local traffic received on casc_port will be forwarded
 561                  * to host_port without embedding the source port and device ID
 562                  * info in the destination MAC address (presumably because it
 563                  * is a cascaded port and a downstream SJA switch already did
 564                  * that). Default to an invalid port (to disable the feature)
 565                  * and overwrite this if we find any DSA (cascaded) ports.
 566                  */
 567                 .casc_port = priv->ds->num_ports,
 568                 /* No TTEthernet */
 569                 .vllupformat = SJA1105_VL_FORMAT_PSFP,
 570                 .vlmarker = 0,
 571                 .vlmask = 0,
 572                 /* Only update correctionField for 1-step PTP (L2 transport) */
 573                 .ignore2stf = 0,
 574                 /* Forcefully disable VLAN filtering by telling
 575                  * the switch that VLAN has a different EtherType.
 576                  */
 577                 .tpid = ETH_P_SJA1105,
 578                 .tpid2 = ETH_P_SJA1105,
 579         };
 580         struct dsa_switch *ds = priv->ds;
 581         struct sja1105_table *table;
 582         int port;
 583
 584         for (port = 0; port < ds->num_ports; port++) {
 585                 if (dsa_is_cpu_port(ds, port)) {
 586                         default_general_params.host_port = port;
 587                         break;
 588                 }
 589         }
 590
 591         table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
 592
 593         if (table->entry_count) {
 594                 kfree(table->entries);
 595                 table->entry_count = 0;
 596         }
 597
 598         table->entries = kcalloc(table->ops->max_entry_count,
 599                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 600         if (!table->entries)
 601                 return -ENOMEM;
 602
 603         table->entry_count = table->ops->max_entry_count;
 604
 605         /* This table only has a single entry */
 606         ((struct sja1105_general_params_entry *)table->entries)[0] =
 607                                 default_general_params;
 608
 609         return 0;
 610 }
 611
 612 static int sja1105_init_avb_params(struct sja1105_private *priv)
 613 {
 614         struct sja1105_avb_params_entry *avb;
 615         struct sja1105_table *table;
 616
 617         table = &priv->static_config.tables[BLK_IDX_AVB_PARAMS];
 618
 619         /* Discard previous AVB Parameters Table */
 620         if (table->entry_count) {
 621                 kfree(table->entries);
 622                 table->entry_count = 0;
 623         }
 624
 625         table->entries = kcalloc(table->ops->max_entry_count,
 626                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 627         if (!table->entries)
 628                 return -ENOMEM;
 629
 630         table->entry_count = table->ops->max_entry_count;
 631
 632         avb = table->entries;
 633
 634         /* Configure the MAC addresses for meta frames */
 635         avb->destmeta = SJA1105_META_DMAC;
 636         avb->srcmeta  = SJA1105_META_SMAC;
 637         /* On P/Q/R/S, configure the direction of the PTP_CLK pin as input by
 638          * default. This is because there might be boards with a hardware
 639          * layout where enabling the pin as output might cause an electrical
 640          * clash. On E/T the pin is always an output, which the board designers
 641          * probably already knew, so even if there are going to be electrical
 642          * issues, there's nothing we can do.
 643          */
 644         avb->cas_master = false;
 645
 646         return 0;
 647 }
 648
 649 /* The L2 policing table is 2-stage. The table is looked up for each frame
 650  * according to the ingress port, whether it was broadcast or not, and the
 651  * classified traffic class (given by VLAN PCP). This portion of the lookup is
 652  * fixed, and gives access to the SHARINDX, an indirection register pointing
 653  * within the policing table itself, which is used to resolve the policer that
 654  * will be used for this frame.
 655  *
 656  *  Stage 1                              Stage 2
 657  * +------------+--------+              +---------------------------------+
 658  * |Port 0 TC 0 |SHARINDX|              | Policer 0: Rate, Burst, MTU     |
 659  * +------------+--------+              +---------------------------------+
 660  * |Port 0 TC 1 |SHARINDX|              | Policer 1: Rate, Burst, MTU     |
 661  * +------------+--------+              +---------------------------------+
 662  *    ...                               | Policer 2: Rate, Burst, MTU     |
 663  * +------------+--------+              +---------------------------------+
 664  * |Port 0 TC 7 |SHARINDX|              | Policer 3: Rate, Burst, MTU     |
 665  * +------------+--------+              +---------------------------------+
 666  * |Port 1 TC 0 |SHARINDX|              | Policer 4: Rate, Burst, MTU     |
 667  * +------------+--------+              +---------------------------------+
 668  *    ...                               | Policer 5: Rate, Burst, MTU     |
 669  * +------------+--------+              +---------------------------------+
 670  * |Port 1 TC 7 |SHARINDX|              | Policer 6: Rate, Burst, MTU     |
 671  * +------------+--------+              +---------------------------------+
 672  *    ...                               | Policer 7: Rate, Burst, MTU     |
 673  * +------------+--------+              +---------------------------------+
 674  * |Port 4 TC 7 |SHARINDX|                 ...
 675  * +------------+--------+
 676  * |Port 0 BCAST|SHARINDX|                 ...
 677  * +------------+--------+
 678  * |Port 1 BCAST|SHARINDX|                 ...
 679  * +------------+--------+
 680  *    ...                                  ...
 681  * +------------+--------+              +---------------------------------+
 682  * |Port 4 BCAST|SHARINDX|              | Policer 44: Rate, Burst, MTU    |
 683  * +------------+--------+              +---------------------------------+
 684  *
 685  * In this driver, we shall use policers 0-4 as statically alocated port
 686  * (matchall) policers. So we need to make the SHARINDX for all lookups
 687  * corresponding to this ingress port (8 VLAN PCP lookups and 1 broadcast
 688  * lookup) equal.
 689  * The remaining policers (40) shall be dynamically allocated for flower
 690  * policers, where the key is either vlan_prio or dst_mac ff:ff:ff:ff:ff:ff.
 691  */
 692 #define SJA1105_RATE_MBPS(speed) (((speed) * 64000) / 1000)
 693
 694 static int sja1105_init_l2_policing(struct sja1105_private *priv)
 695 {
 696         struct sja1105_l2_policing_entry *policing;
 697         struct dsa_switch *ds = priv->ds;
 698         struct sja1105_table *table;
 699         int port, tc;
 700
 701         table = &priv->static_config.tables[BLK_IDX_L2_POLICING];
 702
 703         /* Discard previous L2 Policing Table */
 704         if (table->entry_count) {
 705                 kfree(table->entries);
 706                 table->entry_count = 0;
 707         }
 708
 709         table->entries = kcalloc(table->ops->max_entry_count,
 710                                  table->ops->unpacked_entry_size, GFP_KERNEL);
 711         if (!table->entries)
 712                 return -ENOMEM;
 713
 714         table->entry_count = table->ops->max_entry_count;
 715
 716         policing = table->entries;
 717
 718         /* Setup shared indices for the matchall policers */
 719         for (port = 0; port < ds->num_ports; port++) {
 720                 int mcast = (ds->num_ports * (SJA1105_NUM_TC + 1)) + port;
 721                 int bcast = (ds->num_ports * SJA1105_NUM_TC) + port;
 722
 723                 for (tc = 0; tc < SJA1105_NUM_TC; tc++)
 724                         policing[port * SJA1105_NUM_TC + tc].sharindx = port;
 725
 726                 policing[bcast].sharindx = port;
 727                 /* Only SJA1110 has multicast policers */
 728                 if (mcast <= table->ops->max_entry_count)
 729                         policing[mcast].sharindx = port;
 730         }
 731
 732         /* Setup the matchall policer parameters */
 733         for (port = 0; port < ds->num_ports; port++) {
 734                 int mtu = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;
 735
 736                 if (dsa_is_cpu_port(priv->ds, port))
 737                         mtu += VLAN_HLEN;
 738
 739                 policing[port].smax = 65535; /* Burst size in bytes */
 740                 policing[port].rate = SJA1105_RATE_MBPS(1000);
 741                 policing[port].maxlen = mtu;
 742                 policing[port].partition = 0;
 743         }
 744
 745         return 0;
 746 }
 747
 748 static int sja1105_static_config_load(struct sja1105_private *priv,
 749                                       struct sja1105_dt_port *ports)
 750 {
 751         int rc;
 752
 753         sja1105_static_config_free(&priv->static_config);
 754         rc = sja1105_static_config_init(&priv->static_config,
 755                                         priv->info->static_ops,
 756                                         priv->info->device_id);
 757         if (rc)
 758                 return rc;
 759
 760         /* Build static configuration */
 761         rc = sja1105_init_mac_settings(priv);
 762         if (rc < 0)
 763                 return rc;
 764         rc = sja1105_init_mii_settings(priv, ports);
 765         if (rc < 0)
 766                 return rc;
 767         rc = sja1105_init_static_fdb(priv);
 768         if (rc < 0)
 769                 return rc;
 770         rc = sja1105_init_static_vlan(priv);
 771         if (rc < 0)
 772                 return rc;
 773         rc = sja1105_init_l2_lookup_params(priv);
 774         if (rc < 0)
 775                 return rc;
 776         rc = sja1105_init_l2_forwarding(priv);
 777         if (rc < 0)
 778                 return rc;
 779         rc = sja1105_init_l2_forwarding_params(priv);
 780         if (rc < 0)
 781                 return rc;
 782         rc = sja1105_init_l2_policing(priv);
 783         if (rc < 0)
 784                 return rc;
 785         rc = sja1105_init_general_params(priv);
 786         if (rc < 0)
 787                 return rc;
 788         rc = sja1105_init_avb_params(priv);
 789         if (rc < 0)
 790                 return rc;
 791
 792         /* Send initial configuration to hardware via SPI */
 793         return sja1105_static_config_upload(priv);
 794 }
 795
 796 static int sja1105_parse_rgmii_delays(struct sja1105_private *priv,
 797                                       const struct sja1105_dt_port *ports)
 798 {
 799         struct dsa_switch *ds = priv->ds;
 800         int i;
 801
 802         for (i = 0; i < ds->num_ports; i++) {
 803                 if (ports[i].role == XMII_MAC)
 804                         continue;
 805
 806                 if (ports[i].phy_mode == PHY_INTERFACE_MODE_RGMII_RXID ||
 807                     ports[i].phy_mode == PHY_INTERFACE_MODE_RGMII_ID)
 808                         priv->rgmii_rx_delay[i] = true;
 809
 810                 if (ports[i].phy_mode == PHY_INTERFACE_MODE_RGMII_TXID ||
 811                     ports[i].phy_mode == PHY_INTERFACE_MODE_RGMII_ID)
 812                         priv->rgmii_tx_delay[i] = true;
 813
 814                 if ((priv->rgmii_rx_delay[i] || priv->rgmii_tx_delay[i]) &&
 815                      !priv->info->setup_rgmii_delay)
 816                         return -EINVAL;
 817         }
 818         return 0;
 819 }
 820
 821 static int sja1105_parse_ports_node(struct sja1105_private *priv,
 822                                     struct sja1105_dt_port *ports,
 823                                     struct device_node *ports_node)
 824 {
 825         struct device *dev = &priv->spidev->dev;
 826         struct device_node *child;
 827
 828         for_each_available_child_of_node(ports_node, child) {
 829                 struct device_node *phy_node;
 830                 phy_interface_t phy_mode;
 831                 u32 index;
 832                 int err;
 833
 834                 /* Get switch port number from DT */
 835                 if (of_property_read_u32(child, "reg", &index) < 0) {
 836                         dev_err(dev, "Port number not defined in device tree "
 837                                 "(property \"reg\")\n");
 838                         of_node_put(child);
 839                         return -ENODEV;
 840                 }
 841
 842                 /* Get PHY mode from DT */
 843                 err = of_get_phy_mode(child, &phy_mode);
 844                 if (err) {
 845                         dev_err(dev, "Failed to read phy-mode or "
 846                                 "phy-interface-type property for port %d\n",
 847                                 index);
 848                         of_node_put(child);
 849                         return -ENODEV;
 850                 }
 851                 ports[index].phy_mode = phy_mode;
 852
 853                 phy_node = of_parse_phandle(child, "phy-handle", 0);
 854                 if (!phy_node) {
 855                         if (!of_phy_is_fixed_link(child)) {
 856                                 dev_err(dev, "phy-handle or fixed-link "
 857                                         "properties missing!\n");
 858                                 of_node_put(child);
 859                                 return -ENODEV;
 860                         }
 861                         /* phy-handle is missing, but fixed-link isn't.
 862                          * So it's a fixed link. Default to PHY role.
 863                          */
 864                         ports[index].role = XMII_PHY;
 865                 } else {
 866                         /* phy-handle present => put port in MAC role */
 867                         ports[index].role = XMII_MAC;
 868                         of_node_put(phy_node);
 869                 }
 870
 871                 /* The MAC/PHY role can be overridden with explicit bindings */
 872                 if (of_property_read_bool(child, "sja1105,role-mac"))
 873                         ports[index].role = XMII_MAC;
 874                 else if (of_property_read_bool(child, "sja1105,role-phy"))
 875                         ports[index].role = XMII_PHY;
 876
 877                 priv->phy_mode[index] = phy_mode;
 878         }
 879
 880         return 0;
 881 }
 882
 883 static int sja1105_parse_dt(struct sja1105_private *priv,
 884                             struct sja1105_dt_port *ports)
 885 {
 886         struct device *dev = &priv->spidev->dev;
 887         struct device_node *switch_node = dev->of_node;
 888         struct device_node *ports_node;
 889         int rc;
 890
 891         ports_node = of_get_child_by_name(switch_node, "ports");
 892         if (!ports_node)
 893                 ports_node = of_get_child_by_name(switch_node, "ethernet-ports");
 894         if (!ports_node) {
 895                 dev_err(dev, "Incorrect bindings: absent \"ports\" node\n");
 896                 return -ENODEV;
 897         }
 898
 899         rc = sja1105_parse_ports_node(priv, ports, ports_node);
 900         of_node_put(ports_node);
 901
 902         return rc;
 903 }
 904
 905 static int sja1105_sgmii_read(struct sja1105_private *priv, int port, int mmd,
 906                               int pcs_reg)
 907 {
 908         u64 addr = (mmd << 16) | pcs_reg;
 909         u32 val;
 910         int rc;
 911
 912         if (port != SJA1105_SGMII_PORT)
 913                 return -ENODEV;
 914
 915         rc = sja1105_xfer_u32(priv, SPI_READ, addr, &val, NULL);
 916         if (rc < 0)
 917                 return rc;
 918
 919         return val;
 920 }
 921
 922 static int sja1105_sgmii_write(struct sja1105_private *priv, int port, int mmd,
 923                                int pcs_reg, u16 pcs_val)
 924 {
 925         u64 addr = (mmd << 16) | pcs_reg;
 926         u32 val = pcs_val;
 927         int rc;
 928
 929         if (port != SJA1105_SGMII_PORT)
 930                 return -ENODEV;
 931
 932         rc = sja1105_xfer_u32(priv, SPI_WRITE, addr, &val, NULL);
 933         if (rc < 0)
 934                 return rc;
 935
 936         return val;
 937 }
 938
 939 static void sja1105_sgmii_pcs_config(struct sja1105_private *priv, int port,
 940                                      bool an_enabled, bool an_master)
 941 {
 942         u16 ac = SJA1105_AC_AUTONEG_MODE_SGMII;
 943
 944         /* DIGITAL_CONTROL_1: Enable vendor-specific MMD1, allow the PHY to
 945          * stop the clock during LPI mode, make the MAC reconfigure
 946          * autonomously after PCS autoneg is done, flush the internal FIFOs.
 947          */
 948         sja1105_sgmii_write(priv, port, MDIO_MMD_VEND2, SJA1105_DC1,
 949                             SJA1105_DC1_EN_VSMMD1 |
 950                             SJA1105_DC1_CLOCK_STOP_EN |
 951                             SJA1105_DC1_MAC_AUTO_SW |
 952                             SJA1105_DC1_INIT);
 953         /* DIGITAL_CONTROL_2: No polarity inversion for TX and RX lanes */
 954         sja1105_sgmii_write(priv, port, MDIO_MMD_VEND2, SJA1105_DC2,
 955                             SJA1105_DC2_TX_POL_INV_DISABLE);
 956         /* AUTONEG_CONTROL: Use SGMII autoneg */
 957         if (an_master)
 958                 ac |= SJA1105_AC_PHY_MODE | SJA1105_AC_SGMII_LINK;
 959         sja1105_sgmii_write(priv, port, MDIO_MMD_VEND2, SJA1105_AC, ac);
 960         /* BASIC_CONTROL: enable in-band AN now, if requested. Otherwise,
 961          * sja1105_sgmii_pcs_force_speed must be called later for the link
 962          * to become operational.
 963          */
 964         if (an_enabled)
 965                 sja1105_sgmii_write(priv, port, MDIO_MMD_VEND2, MDIO_CTRL1,
 966                                     BMCR_ANENABLE | BMCR_ANRESTART);
 967 }
 968
 969 static void sja1105_sgmii_pcs_force_speed(struct sja1105_private *priv,
 970                                           int port, int speed)
 971 {
 972         int pcs_speed;
 973
 974         switch (speed) {
 975         case SPEED_1000:
 976                 pcs_speed = BMCR_SPEED1000;
 977                 break;
 978         case SPEED_100:
 979                 pcs_speed = BMCR_SPEED100;
 980                 break;
 981         case SPEED_10:
 982                 pcs_speed = BMCR_SPEED10;
 983                 break;
 984         default:
 985                 dev_err(priv->ds->dev, "Invalid speed %d\n", speed);
 986                 return;
 987         }
 988         sja1105_sgmii_write(priv, port, MDIO_MMD_VEND2, MDIO_CTRL1,
 989                             pcs_speed | BMCR_FULLDPLX);
 990 }
 991
 992 /* Convert link speed from SJA1105 to ethtool encoding */
 993 static int sja1105_port_speed_to_ethtool(struct sja1105_private *priv,
 994                                          u64 speed)
 995 {
 996         if (speed == priv->info->port_speed[SJA1105_SPEED_10MBPS])
 997                 return SPEED_10;
 998         if (speed == priv->info->port_speed[SJA1105_SPEED_100MBPS])
 999                 return SPEED_100;
1000         if (speed == priv->info->port_speed[SJA1105_SPEED_1000MBPS])
1001                 return SPEED_1000;
1002         if (speed == priv->info->port_speed[SJA1105_SPEED_2500MBPS])
1003                 return SPEED_2500;
1004         return SPEED_UNKNOWN;
1005 }
1006
1007 /* Set link speed in the MAC configuration for a specific port. */
1008 static int sja1105_adjust_port_config(struct sja1105_private *priv, int port,
1009                                       int speed_mbps)
1010 {
1011         struct sja1105_mac_config_entry *mac;
1012         struct device *dev = priv->ds->dev;
1013         u64 speed;
1014         int rc;
1015
1016         /* On P/Q/R/S, one can read from the device via the MAC reconfiguration
1017          * tables. On E/T, MAC reconfig tables are not readable, only writable.
1018          * We have to *know* what the MAC looks like.  For the sake of keeping
1019          * the code common, we'll use the static configuration tables as a
1020          * reasonable approximation for both E/T and P/Q/R/S.
1021          */
1022         mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
1023
1024         switch (speed_mbps) {
1025         case SPEED_UNKNOWN:
1026                 /* PHYLINK called sja1105_mac_config() to inform us about
1027                  * the state->interface, but AN has not completed and the
1028                  * speed is not yet valid. UM10944.pdf says that setting
1029                  * SJA1105_SPEED_AUTO at runtime disables the port, so that is
1030                  * ok for power consumption in case AN will never complete -
1031                  * otherwise PHYLINK should come back with a new update.
1032                  */
1033                 speed = priv->info->port_speed[SJA1105_SPEED_AUTO];
1034                 break;
1035         case SPEED_10:
1036                 speed = priv->info->port_speed[SJA1105_SPEED_10MBPS];
1037                 break;
1038         case SPEED_100:
1039                 speed = priv->info->port_speed[SJA1105_SPEED_100MBPS];
1040                 break;
1041         case SPEED_1000:
1042                 speed = priv->info->port_speed[SJA1105_SPEED_1000MBPS];
1043                 break;
1044         default:
1045                 dev_err(dev, "Invalid speed %iMbps\n", speed_mbps);
1046                 return -EINVAL;
1047         }
1048
1049         /* Overwrite SJA1105_SPEED_AUTO from the static MAC configuration
1050          * table, since this will be used for the clocking setup, and we no
1051          * longer need to store it in the static config (already told hardware
1052          * we want auto during upload phase).
1053          * Actually for the SGMII port, the MAC is fixed at 1 Gbps and
1054          * we need to configure the PCS only (if even that).
1055          */
1056         if (priv->phy_mode[port] == PHY_INTERFACE_MODE_SGMII)
1057                 mac[port].speed = priv->info->port_speed[SJA1105_SPEED_1000MBPS];
1058         else
1059                 mac[port].speed = speed;
1060
1061         /* Write to the dynamic reconfiguration tables */
1062         rc = sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
1063                                           &mac[port], true);
1064         if (rc < 0) {
1065                 dev_err(dev, "Failed to write MAC config: %d\n", rc);
1066                 return rc;
1067         }
1068
1069         /* Reconfigure the PLLs for the RGMII interfaces (required 125 MHz at
1070          * gigabit, 25 MHz at 100 Mbps and 2.5 MHz at 10 Mbps). For MII and
1071          * RMII no change of the clock setup is required. Actually, changing
1072          * the clock setup does interrupt the clock signal for a certain time
1073          * which causes trouble for all PHYs relying on this signal.
1074          */
1075         if (!phy_interface_mode_is_rgmii(priv->phy_mode[port]))
1076                 return 0;
1077
1078         return sja1105_clocking_setup_port(priv, port);
1079 }
1080
1081 /* The SJA1105 MAC programming model is through the static config (the xMII
1082  * Mode table cannot be dynamically reconfigured), and we have to program
1083  * that early (earlier than PHYLINK calls us, anyway).
1084  * So just error out in case the connected PHY attempts to change the initial
1085  * system interface MII protocol from what is defined in the DT, at least for
1086  * now.
1087  */
1088 static bool sja1105_phy_mode_mismatch(struct sja1105_private *priv, int port,
1089                                       phy_interface_t interface)
1090 {
1091         return priv->phy_mode[port] != interface;
1092 }
1093
1094 static void sja1105_mac_config(struct dsa_switch *ds, int port,
1095                                unsigned int mode,
1096                                const struct phylink_link_state *state)
1097 {
1098         struct sja1105_private *priv = ds->priv;
1099         bool is_sgmii;
1100
1101         is_sgmii = (state->interface == PHY_INTERFACE_MODE_SGMII);
1102
1103         if (sja1105_phy_mode_mismatch(priv, port, state->interface)) {
1104                 dev_err(ds->dev, "Changing PHY mode to %s not supported!\n",
1105                         phy_modes(state->interface));
1106                 return;
1107         }
1108
1109         if (phylink_autoneg_inband(mode) && !is_sgmii) {
1110                 dev_err(ds->dev, "In-band AN not supported!\n");
1111                 return;
1112         }
1113
1114         if (is_sgmii)
1115                 sja1105_sgmii_pcs_config(priv, port,
1116                                          phylink_autoneg_inband(mode),
1117                                          false);
1118 }
1119
1120 static void sja1105_mac_link_down(struct dsa_switch *ds, int port,
1121                                   unsigned int mode,
1122                                   phy_interface_t interface)
1123 {
1124         sja1105_inhibit_tx(ds->priv, BIT(port), true);
1125 }
1126
1127 static void sja1105_mac_link_up(struct dsa_switch *ds, int port,
1128                                 unsigned int mode,
1129                                 phy_interface_t interface,
1130                                 struct phy_device *phydev,
1131                                 int speed, int duplex,
1132                                 bool tx_pause, bool rx_pause)
1133 {
1134         struct sja1105_private *priv = ds->priv;
1135
1136         sja1105_adjust_port_config(priv, port, speed);
1137
1138         if (priv->phy_mode[port] == PHY_INTERFACE_MODE_SGMII &&
1139             !phylink_autoneg_inband(mode))
1140                 sja1105_sgmii_pcs_force_speed(priv, port, speed);
1141
1142         sja1105_inhibit_tx(priv, BIT(port), false);
1143 }
1144
1145 static void sja1105_phylink_validate(struct dsa_switch *ds, int port,
1146                                      unsigned long *supported,
1147                                      struct phylink_link_state *state)
1148 {
1149         /* Construct a new mask which exhaustively contains all link features
1150          * supported by the MAC, and then apply that (logical AND) to what will
1151          * be sent to the PHY for "marketing".
1152          */
1153         __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
1154         struct sja1105_private *priv = ds->priv;
1155         struct sja1105_xmii_params_entry *mii;
1156
1157         mii = priv->static_config.tables[BLK_IDX_XMII_PARAMS].entries;
1158
1159         /* include/linux/phylink.h says:
1160          *     When @state->interface is %PHY_INTERFACE_MODE_NA, phylink
1161          *     expects the MAC driver to return all supported link modes.
1162          */
1163         if (state->interface != PHY_INTERFACE_MODE_NA &&
1164             sja1105_phy_mode_mismatch(priv, port, state->interface)) {
1165                 bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
1166                 return;
1167         }
1168
1169         /* The MAC does not support pause frames, and also doesn't
1170          * support half-duplex traffic modes.
1171          */
1172         phylink_set(mask, Autoneg);
1173         phylink_set(mask, MII);
1174         phylink_set(mask, 10baseT_Full);
1175         phylink_set(mask, 100baseT_Full);
1176         phylink_set(mask, 100baseT1_Full);
1177         if (mii->xmii_mode[port] == XMII_MODE_RGMII ||
1178             mii->xmii_mode[port] == XMII_MODE_SGMII)
1179                 phylink_set(mask, 1000baseT_Full);
1180
1181         bitmap_and(supported, supported, mask, __ETHTOOL_LINK_MODE_MASK_NBITS);
1182         bitmap_and(state->advertising, state->advertising, mask,
1183                    __ETHTOOL_LINK_MODE_MASK_NBITS);
1184 }
1185
1186 static int sja1105_mac_pcs_get_state(struct dsa_switch *ds, int port,
1187                                      struct phylink_link_state *state)
1188 {
1189         struct sja1105_private *priv = ds->priv;
1190         int ais;
1191
1192         /* Read the vendor-specific AUTONEG_INTR_STATUS register */
1193         ais = sja1105_sgmii_read(priv, port, MDIO_MMD_VEND2, SJA1105_AIS);
1194         if (ais < 0)
1195                 return ais;
1196
1197         switch (SJA1105_AIS_SPEED(ais)) {
1198         case 0:
1199                 state->speed = SPEED_10;
1200                 break;
1201         case 1:
1202                 state->speed = SPEED_100;
1203                 break;
1204         case 2:
1205                 state->speed = SPEED_1000;
1206                 break;
1207         default:
1208                 dev_err(ds->dev, "Invalid SGMII PCS speed %lu\n",
1209                         SJA1105_AIS_SPEED(ais));
1210         }
1211         state->duplex = SJA1105_AIS_DUPLEX_MODE(ais);
1212         state->an_complete = SJA1105_AIS_COMPLETE(ais);
1213         state->link = SJA1105_AIS_LINK_STATUS(ais);
1214
1215         return 0;
1216 }
1217
1218 static int
1219 sja1105_find_static_fdb_entry(struct sja1105_private *priv, int port,
1220                               const struct sja1105_l2_lookup_entry *requested)
1221 {
1222         struct sja1105_l2_lookup_entry *l2_lookup;
1223         struct sja1105_table *table;
1224         int i;
1225
1226         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
1227         l2_lookup = table->entries;
1228
1229         for (i = 0; i < table->entry_count; i++)
1230                 if (l2_lookup[i].macaddr == requested->macaddr &&
1231                     l2_lookup[i].vlanid == requested->vlanid &&
1232                     l2_lookup[i].destports & BIT(port))
1233                         return i;
1234
1235         return -1;
1236 }
1237
1238 /* We want FDB entries added statically through the bridge command to persist
1239  * across switch resets, which are a common thing during normal SJA1105
1240  * operation. So we have to back them up in the static configuration tables
1241  * and hence apply them on next static config upload... yay!
1242  */
1243 static int
1244 sja1105_static_fdb_change(struct sja1105_private *priv, int port,
1245                           const struct sja1105_l2_lookup_entry *requested,
1246                           bool keep)
1247 {
1248         struct sja1105_l2_lookup_entry *l2_lookup;
1249         struct sja1105_table *table;
1250         int rc, match;
1251
1252         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
1253
1254         match = sja1105_find_static_fdb_entry(priv, port, requested);
1255         if (match < 0) {
1256                 /* Can't delete a missing entry. */
1257                 if (!keep)
1258                         return 0;
1259
1260                 /* No match => new entry */
1261                 rc = sja1105_table_resize(table, table->entry_count + 1);
1262                 if (rc)
1263                         return rc;
1264
1265                 match = table->entry_count - 1;
1266         }
1267
1268         /* Assign pointer after the resize (it may be new memory) */
1269         l2_lookup = table->entries;
1270
1271         /* We have a match.
1272          * If the job was to add this FDB entry, it's already done (mostly
1273          * anyway, since the port forwarding mask may have changed, case in
1274          * which we update it).
1275          * Otherwise we have to delete it.
1276          */
1277         if (keep) {
1278                 l2_lookup[match] = *requested;
1279                 return 0;
1280         }
1281
1282         /* To remove, the strategy is to overwrite the element with
1283          * the last one, and then reduce the array size by 1
1284          */
1285         l2_lookup[match] = l2_lookup[table->entry_count - 1];
1286         return sja1105_table_resize(table, table->entry_count - 1);
1287 }
1288
1289 /* First-generation switches have a 4-way set associative TCAM that
1290  * holds the FDB entries. An FDB index spans from 0 to 1023 and is comprised of
1291  * a "bin" (grouping of 4 entries) and a "way" (an entry within a bin).
1292  * For the placement of a newly learnt FDB entry, the switch selects the bin
1293  * based on a hash function, and the way within that bin incrementally.
1294  */
1295 static int sja1105et_fdb_index(int bin, int way)
1296 {
1297         return bin * SJA1105ET_FDB_BIN_SIZE + way;
1298 }
1299
1300 static int sja1105et_is_fdb_entry_in_bin(struct sja1105_private *priv, int bin,
1301                                          const u8 *addr, u16 vid,
1302                                          struct sja1105_l2_lookup_entry *match,
1303                                          int *last_unused)
1304 {
1305         int way;
1306
1307         for (way = 0; way < SJA1105ET_FDB_BIN_SIZE; way++) {
1308                 struct sja1105_l2_lookup_entry l2_lookup = {0};
1309                 int index = sja1105et_fdb_index(bin, way);
1310
1311                 /* Skip unused entries, optionally marking them
1312                  * into the return value
1313                  */
1314                 if (sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
1315                                                 index, &l2_lookup)) {
1316                         if (last_unused)
1317                                 *last_unused = way;
1318                         continue;
1319                 }
1320
1321                 if (l2_lookup.macaddr == ether_addr_to_u64(addr) &&
1322                     l2_lookup.vlanid == vid) {
1323                         if (match)
1324                                 *match = l2_lookup;
1325                         return way;
1326                 }
1327         }
1328         /* Return an invalid entry index if not found */
1329         return -1;
1330 }
1331
1332 int sja1105et_fdb_add(struct dsa_switch *ds, int port,
1333                       const unsigned char *addr, u16 vid)
1334 {
1335         struct sja1105_l2_lookup_entry l2_lookup = {0};
1336         struct sja1105_private *priv = ds->priv;
1337         struct device *dev = ds->dev;
1338         int last_unused = -1;
1339         int bin, way, rc;
1340
1341         bin = sja1105et_fdb_hash(priv, addr, vid);
1342
1343         way = sja1105et_is_fdb_entry_in_bin(priv, bin, addr, vid,
1344                                             &l2_lookup, &last_unused);
1345         if (way >= 0) {
1346                 /* We have an FDB entry. Is our port in the destination
1347                  * mask? If yes, we need to do nothing. If not, we need
1348                  * to rewrite the entry by adding this port to it.
1349                  */
1350                 if (l2_lookup.destports & BIT(port))
1351                         return 0;
1352                 l2_lookup.destports |= BIT(port);
1353         } else {
1354                 int index = sja1105et_fdb_index(bin, way);
1355
1356                 /* We don't have an FDB entry. We construct a new one and
1357                  * try to find a place for it within the FDB table.
1358                  */
1359                 l2_lookup.macaddr = ether_addr_to_u64(addr);
1360                 l2_lookup.destports = BIT(port);
1361                 l2_lookup.vlanid = vid;
1362
1363                 if (last_unused >= 0) {
1364                         way = last_unused;
1365                 } else {
1366                         /* Bin is full, need to evict somebody.
1367                          * Choose victim at random. If you get these messages
1368                          * often, you may need to consider changing the
1369                          * distribution function:
1370                          * static_config[BLK_IDX_L2_LOOKUP_PARAMS].entries->poly
1371                          */
1372                         get_random_bytes(&way, sizeof(u8));
1373                         way %= SJA1105ET_FDB_BIN_SIZE;
1374                         dev_warn(dev, "Warning, FDB bin %d full while adding entry for %pM. Evicting entry %u.\n",
1375                                  bin, addr, way);
1376                         /* Evict entry */
1377                         sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
1378                                                      index, NULL, false);
1379                 }
1380         }
1381         l2_lookup.index = sja1105et_fdb_index(bin, way);
1382
1383         rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
1384                                           l2_lookup.index, &l2_lookup,
1385                                           true);
1386         if (rc < 0)
1387                 return rc;
1388
1389         return sja1105_static_fdb_change(priv, port, &l2_lookup, true);
1390 }
1391
1392 int sja1105et_fdb_del(struct dsa_switch *ds, int port,
1393                       const unsigned char *addr, u16 vid)
1394 {
1395         struct sja1105_l2_lookup_entry l2_lookup = {0};
1396         struct sja1105_private *priv = ds->priv;
1397         int index, bin, way, rc;
1398         bool keep;
1399
1400         bin = sja1105et_fdb_hash(priv, addr, vid);
1401         way = sja1105et_is_fdb_entry_in_bin(priv, bin, addr, vid,
1402                                             &l2_lookup, NULL);
1403         if (way < 0)
1404                 return 0;
1405         index = sja1105et_fdb_index(bin, way);
1406
1407         /* We have an FDB entry. Is our port in the destination mask? If yes,
1408          * we need to remove it. If the resulting port mask becomes empty, we
1409          * need to completely evict the FDB entry.
1410          * Otherwise we just write it back.
1411          */
1412         l2_lookup.destports &= ~BIT(port);
1413
1414         if (l2_lookup.destports)
1415                 keep = true;
1416         else
1417                 keep = false;
1418
1419         rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
1420                                           index, &l2_lookup, keep);
1421         if (rc < 0)
1422                 return rc;
1423
1424         return sja1105_static_fdb_change(priv, port, &l2_lookup, keep);
1425 }
1426
1427 int sja1105pqrs_fdb_add(struct dsa_switch *ds, int port,
1428                         const unsigned char *addr, u16 vid)
1429 {
1430         struct sja1105_l2_lookup_entry l2_lookup = {0};
1431         struct sja1105_private *priv = ds->priv;
1432         int rc, i;
1433
1434         /* Search for an existing entry in the FDB table */
1435         l2_lookup.macaddr = ether_addr_to_u64(addr);
1436         l2_lookup.vlanid = vid;
1437         l2_lookup.iotag = SJA1105_S_TAG;
1438         l2_lookup.mask_macaddr = GENMASK_ULL(ETH_ALEN * 8 - 1, 0);
1439         if (priv->vlan_state != SJA1105_VLAN_UNAWARE) {
1440                 l2_lookup.mask_vlanid = VLAN_VID_MASK;
1441                 l2_lookup.mask_iotag = BIT(0);
1442         } else {
1443                 l2_lookup.mask_vlanid = 0;
1444                 l2_lookup.mask_iotag = 0;
1445         }
1446         l2_lookup.destports = BIT(port);
1447
1448         rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
1449                                          SJA1105_SEARCH, &l2_lookup);
1450         if (rc == 0) {
1451                 /* Found and this port is already in the entry's
1452                  * port mask => job done
1453                  */
1454                 if (l2_lookup.destports & BIT(port))
1455                         return 0;
1456                 /* l2_lookup.index is populated by the switch in case it
1457                  * found something.
1458                  */
1459                 l2_lookup.destports |= BIT(port);
1460                 goto skip_finding_an_index;
1461         }
1462
1463         /* Not found, so try to find an unused spot in the FDB.
1464          * This is slightly inefficient because the strategy is knock-knock at
1465          * every possible position from 0 to 1023.
1466          */
1467         for (i = 0; i < SJA1105_MAX_L2_LOOKUP_COUNT; i++) {
1468                 rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
1469                                                  i, NULL);
1470                 if (rc < 0)
1471                         break;
1472         }
1473         if (i == SJA1105_MAX_L2_LOOKUP_COUNT) {
1474                 dev_err(ds->dev, "FDB is full, cannot add entry.\n");
1475                 return -EINVAL;
1476         }
1477         l2_lookup.lockeds = true;
1478         l2_lookup.index = i;
1479
1480 skip_finding_an_index:
1481         rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
1482                                           l2_lookup.index, &l2_lookup,
1483                                           true);
1484         if (rc < 0)
1485                 return rc;
1486
1487         return sja1105_static_fdb_change(priv, port, &l2_lookup, true);
1488 }
1489
1490 int sja1105pqrs_fdb_del(struct dsa_switch *ds, int port,
1491                         const unsigned char *addr, u16 vid)
1492 {
1493         struct sja1105_l2_lookup_entry l2_lookup = {0};
1494         struct sja1105_private *priv = ds->priv;
1495         bool keep;
1496         int rc;
1497
1498         l2_lookup.macaddr = ether_addr_to_u64(addr);
1499         l2_lookup.vlanid = vid;
1500         l2_lookup.iotag = SJA1105_S_TAG;
1501         l2_lookup.mask_macaddr = GENMASK_ULL(ETH_ALEN * 8 - 1, 0);
1502         if (priv->vlan_state != SJA1105_VLAN_UNAWARE) {
1503                 l2_lookup.mask_vlanid = VLAN_VID_MASK;
1504                 l2_lookup.mask_iotag = BIT(0);
1505         } else {
1506                 l2_lookup.mask_vlanid = 0;
1507                 l2_lookup.mask_iotag = 0;
1508         }
1509         l2_lookup.destports = BIT(port);
1510
1511         rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
1512                                          SJA1105_SEARCH, &l2_lookup);
1513         if (rc < 0)
1514                 return 0;
1515
1516         l2_lookup.destports &= ~BIT(port);
1517
1518         /* Decide whether we remove just this port from the FDB entry,
1519          * or if we remove it completely.
1520          */
1521         if (l2_lookup.destports)
1522                 keep = true;
1523         else
1524                 keep = false;
1525
1526         rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
1527                                           l2_lookup.index, &l2_lookup, keep);
1528         if (rc < 0)
1529                 return rc;
1530
1531         return sja1105_static_fdb_change(priv, port, &l2_lookup, keep);
1532 }
1533
1534 static int sja1105_fdb_add(struct dsa_switch *ds, int port,
1535                            const unsigned char *addr, u16 vid)
1536 {
1537         struct sja1105_private *priv = ds->priv;
1538
1539         /* dsa_8021q is in effect when the bridge's vlan_filtering isn't,
1540          * so the switch still does some VLAN processing internally.
1541          * But Shared VLAN Learning (SVL) is also active, and it will take
1542          * care of autonomous forwarding between the unique pvid's of each
1543          * port.  Here we just make sure that users can't add duplicate FDB
1544          * entries when in this mode - the actual VID doesn't matter except
1545          * for what gets printed in 'bridge fdb show'.  In the case of zero,
1546          * no VID gets printed at all.
1547          */
1548         if (priv->vlan_state != SJA1105_VLAN_FILTERING_FULL)
1549                 vid = 0;
1550
1551         return priv->info->fdb_add_cmd(ds, port, addr, vid);
1552 }
1553
1554 static int sja1105_fdb_del(struct dsa_switch *ds, int port,
1555                            const unsigned char *addr, u16 vid)
1556 {
1557         struct sja1105_private *priv = ds->priv;
1558
1559         if (priv->vlan_state != SJA1105_VLAN_FILTERING_FULL)
1560                 vid = 0;
1561
1562         return priv->info->fdb_del_cmd(ds, port, addr, vid);
1563 }
1564
1565 static int sja1105_fdb_dump(struct dsa_switch *ds, int port,
1566                             dsa_fdb_dump_cb_t *cb, void *data)
1567 {
1568         struct sja1105_private *priv = ds->priv;
1569         struct device *dev = ds->dev;
1570         int i;
1571
1572         for (i = 0; i < SJA1105_MAX_L2_LOOKUP_COUNT; i++) {
1573                 struct sja1105_l2_lookup_entry l2_lookup = {0};
1574                 u8 macaddr[ETH_ALEN];
1575                 int rc;
1576
1577                 rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
1578                                                  i, &l2_lookup);
1579                 /* No fdb entry at i, not an issue */
1580                 if (rc == -ENOENT)
1581                         continue;
1582                 if (rc) {
1583                         dev_err(dev, "Failed to dump FDB: %d\n", rc);
1584                         return rc;
1585                 }
1586
1587                 /* FDB dump callback is per port. This means we have to
1588                  * disregard a valid entry if it's not for this port, even if
1589                  * only to revisit it later. This is inefficient because the
1590                  * 1024-sized FDB table needs to be traversed 4 times through
1591                  * SPI during a 'bridge fdb show' command.
1592                  */
1593                 if (!(l2_lookup.destports & BIT(port)))
1594                         continue;
1595
1596                 /* We need to hide the FDB entry for unknown multicast */
1597                 if (l2_lookup.macaddr == SJA1105_UNKNOWN_MULTICAST &&
1598                     l2_lookup.mask_macaddr == SJA1105_UNKNOWN_MULTICAST)
1599                         continue;
1600
1601                 u64_to_ether_addr(l2_lookup.macaddr, macaddr);
1602
1603                 /* We need to hide the dsa_8021q VLANs from the user. */
1604                 if (priv->vlan_state == SJA1105_VLAN_UNAWARE)
1605                         l2_lookup.vlanid = 0;
1606                 cb(macaddr, l2_lookup.vlanid, l2_lookup.lockeds, data);
1607         }
1608         return 0;
1609 }
1610
1611 static int sja1105_mdb_add(struct dsa_switch *ds, int port,
1612                            const struct switchdev_obj_port_mdb *mdb)
1613 {
1614         return sja1105_fdb_add(ds, port, mdb->addr, mdb->vid);
1615 }
1616
1617 static int sja1105_mdb_del(struct dsa_switch *ds, int port,
1618                            const struct switchdev_obj_port_mdb *mdb)
1619 {
1620         return sja1105_fdb_del(ds, port, mdb->addr, mdb->vid);
1621 }
1622
1623 /* Common function for unicast and broadcast flood configuration.
1624  * Flooding is configured between each {ingress, egress} port pair, and since
1625  * the bridge's semantics are those of "egress flooding", it means we must
1626  * enable flooding towards this port from all ingress ports that are in the
1627  * same forwarding domain.
1628  */
1629 static int sja1105_manage_flood_domains(struct sja1105_private *priv)
1630 {
1631         struct sja1105_l2_forwarding_entry *l2_fwd;
1632         struct dsa_switch *ds = priv->ds;
1633         int from, to, rc;
1634
1635         l2_fwd = priv->static_config.tables[BLK_IDX_L2_FORWARDING].entries;
1636
1637         for (from = 0; from < ds->num_ports; from++) {
1638                 u64 fl_domain = 0, bc_domain = 0;
1639
1640                 for (to = 0; to < priv->ds->num_ports; to++) {
1641                         if (!sja1105_can_forward(l2_fwd, from, to))
1642                                 continue;
1643
1644                         if (priv->ucast_egress_floods & BIT(to))
1645                                 fl_domain |= BIT(to);
1646                         if (priv->bcast_egress_floods & BIT(to))
1647                                 bc_domain |= BIT(to);
1648                 }
1649
1650                 /* Nothing changed, nothing to do */
1651                 if (l2_fwd[from].fl_domain == fl_domain &&
1652                     l2_fwd[from].bc_domain == bc_domain)
1653                         continue;
1654
1655                 l2_fwd[from].fl_domain = fl_domain;
1656                 l2_fwd[from].bc_domain = bc_domain;
1657
1658                 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_FORWARDING,
1659                                                   from, &l2_fwd[from], true);
1660                 if (rc < 0)
1661                         return rc;
1662         }
1663
1664         return 0;
1665 }
1666
1667 static int sja1105_bridge_member(struct dsa_switch *ds, int port,
1668                                  struct net_device *br, bool member)
1669 {
1670         struct sja1105_l2_forwarding_entry *l2_fwd;
1671         struct sja1105_private *priv = ds->priv;
1672         int i, rc;
1673
1674         l2_fwd = priv->static_config.tables[BLK_IDX_L2_FORWARDING].entries;
1675
1676         for (i = 0; i < ds->num_ports; i++) {
1677                 /* Add this port to the forwarding matrix of the
1678                  * other ports in the same bridge, and viceversa.
1679                  */
1680                 if (!dsa_is_user_port(ds, i))
1681                         continue;
1682                 /* For the ports already under the bridge, only one thing needs
1683                  * to be done, and that is to add this port to their
1684                  * reachability domain. So we can perform the SPI write for
1685                  * them immediately. However, for this port itself (the one
1686                  * that is new to the bridge), we need to add all other ports
1687                  * to its reachability domain. So we do that incrementally in
1688                  * this loop, and perform the SPI write only at the end, once
1689                  * the domain contains all other bridge ports.
1690                  */
1691                 if (i == port)
1692                         continue;
1693                 if (dsa_to_port(ds, i)->bridge_dev != br)
1694                         continue;
1695                 sja1105_port_allow_traffic(l2_fwd, i, port, member);
1696                 sja1105_port_allow_traffic(l2_fwd, port, i, member);
1697
1698                 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_FORWARDING,
1699                                                   i, &l2_fwd[i], true);
1700                 if (rc < 0)
1701                         return rc;
1702         }
1703
1704         rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_FORWARDING,
1705                                           port, &l2_fwd[port], true);
1706         if (rc)
1707                 return rc;
1708
1709         return sja1105_manage_flood_domains(priv);
1710 }
1711
1712 static void sja1105_bridge_stp_state_set(struct dsa_switch *ds, int port,
1713                                          u8 state)
1714 {
1715         struct sja1105_private *priv = ds->priv;
1716         struct sja1105_mac_config_entry *mac;
1717
1718         mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
1719
1720         switch (state) {
1721         case BR_STATE_DISABLED:
1722         case BR_STATE_BLOCKING:
1723                 /* From UM10944 description of DRPDTAG (why put this there?):
1724                  * "Management traffic flows to the port regardless of the state
1725                  * of the INGRESS flag". So BPDUs are still be allowed to pass.
1726                  * At the moment no difference between DISABLED and BLOCKING.
1727                  */
1728                 mac[port].ingress   = false;
1729                 mac[port].egress    = false;
1730                 mac[port].dyn_learn = false;
1731                 break;
1732         case BR_STATE_LISTENING:
1733                 mac[port].ingress   = true;
1734                 mac[port].egress    = false;
1735                 mac[port].dyn_learn = false;
1736                 break;
1737         case BR_STATE_LEARNING:
1738                 mac[port].ingress   = true;
1739                 mac[port].egress    = false;
1740                 mac[port].dyn_learn = !!(priv->learn_ena & BIT(port));
1741                 break;
1742         case BR_STATE_FORWARDING:
1743                 mac[port].ingress   = true;
1744                 mac[port].egress    = true;
1745                 mac[port].dyn_learn = !!(priv->learn_ena & BIT(port));
1746                 break;
1747         default:
1748                 dev_err(ds->dev, "invalid STP state: %d\n", state);
1749                 return;
1750         }
1751
1752         sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
1753                                      &mac[port], true);
1754 }
1755
1756 static int sja1105_bridge_join(struct dsa_switch *ds, int port,
1757                                struct net_device *br)
1758 {
1759         return sja1105_bridge_member(ds, port, br, true);
1760 }
1761
1762 static void sja1105_bridge_leave(struct dsa_switch *ds, int port,
1763                                  struct net_device *br)
1764 {
1765         sja1105_bridge_member(ds, port, br, false);
1766 }
1767
1768 #define BYTES_PER_KBIT (1000LL / 8)
1769
1770 static int sja1105_find_unused_cbs_shaper(struct sja1105_private *priv)
1771 {
1772         int i;
1773
1774         for (i = 0; i < priv->info->num_cbs_shapers; i++)
1775                 if (!priv->cbs[i].idle_slope && !priv->cbs[i].send_slope)
1776                         return i;
1777
1778         return -1;
1779 }
1780
1781 static int sja1105_delete_cbs_shaper(struct sja1105_private *priv, int port,
1782                                      int prio)
1783 {
1784         int i;
1785
1786         for (i = 0; i < priv->info->num_cbs_shapers; i++) {
1787                 struct sja1105_cbs_entry *cbs = &priv->cbs[i];
1788
1789                 if (cbs->port == port && cbs->prio == prio) {
1790                         memset(cbs, 0, sizeof(*cbs));
1791                         return sja1105_dynamic_config_write(priv, BLK_IDX_CBS,
1792                                                             i, cbs, true);
1793                 }
1794         }
1795
1796         return 0;
1797 }
1798
1799 static int sja1105_setup_tc_cbs(struct dsa_switch *ds, int port,
1800                                 struct tc_cbs_qopt_offload *offload)
1801 {
1802         struct sja1105_private *priv = ds->priv;
1803         struct sja1105_cbs_entry *cbs;
1804         int index;
1805
1806         if (!offload->enable)
1807                 return sja1105_delete_cbs_shaper(priv, port, offload->queue);
1808
1809         index = sja1105_find_unused_cbs_shaper(priv);
1810         if (index < 0)
1811                 return -ENOSPC;
1812
1813         cbs = &priv->cbs[index];
1814         cbs->port = port;
1815         cbs->prio = offload->queue;
1816         /* locredit and sendslope are negative by definition. In hardware,
1817          * positive values must be provided, and the negative sign is implicit.
1818          */
1819         cbs->credit_hi = offload->hicredit;
1820         cbs->credit_lo = abs(offload->locredit);
1821         /* User space is in kbits/sec, hardware in bytes/sec */
1822         cbs->idle_slope = offload->idleslope * BYTES_PER_KBIT;
1823         cbs->send_slope = abs(offload->sendslope * BYTES_PER_KBIT);
1824         /* Convert the negative values from 64-bit 2's complement
1825          * to 32-bit 2's complement (for the case of 0x80000000 whose
1826          * negative is still negative).
1827          */
1828         cbs->credit_lo &= GENMASK_ULL(31, 0);
1829         cbs->send_slope &= GENMASK_ULL(31, 0);
1830
1831         return sja1105_dynamic_config_write(priv, BLK_IDX_CBS, index, cbs,
1832                                             true);
1833 }
1834
1835 static int sja1105_reload_cbs(struct sja1105_private *priv)
1836 {
1837         int rc = 0, i;
1838
1839         for (i = 0; i < priv->info->num_cbs_shapers; i++) {
1840                 struct sja1105_cbs_entry *cbs = &priv->cbs[i];
1841
1842                 if (!cbs->idle_slope && !cbs->send_slope)
1843                         continue;
1844
1845                 rc = sja1105_dynamic_config_write(priv, BLK_IDX_CBS, i, cbs,
1846                                                   true);
1847                 if (rc)
1848                         break;
1849         }
1850
1851         return rc;
1852 }
1853
1854 static const char * const sja1105_reset_reasons[] = {
1855         [SJA1105_VLAN_FILTERING] = "VLAN filtering",
1856         [SJA1105_RX_HWTSTAMPING] = "RX timestamping",
1857         [SJA1105_AGEING_TIME] = "Ageing time",
1858         [SJA1105_SCHEDULING] = "Time-aware scheduling",
1859         [SJA1105_BEST_EFFORT_POLICING] = "Best-effort policing",
1860         [SJA1105_VIRTUAL_LINKS] = "Virtual links",
1861 };
1862
1863 /* For situations where we need to change a setting at runtime that is only
1864  * available through the static configuration, resetting the switch in order
1865  * to upload the new static config is unavoidable. Back up the settings we
1866  * modify at runtime (currently only MAC) and restore them after uploading,
1867  * such that this operation is relatively seamless.
1868  */
1869 int sja1105_static_config_reload(struct sja1105_private *priv,
1870                                  enum sja1105_reset_reason reason)
1871 {
1872         struct ptp_system_timestamp ptp_sts_before;
1873         struct ptp_system_timestamp ptp_sts_after;
1874         int speed_mbps[SJA1105_MAX_NUM_PORTS];
1875         u16 bmcr[SJA1105_MAX_NUM_PORTS] = {0};
1876         struct sja1105_mac_config_entry *mac;
1877         struct dsa_switch *ds = priv->ds;
1878         s64 t1, t2, t3, t4;
1879         s64 t12, t34;
1880         int rc, i;
1881         s64 now;
1882
1883         mutex_lock(&priv->mgmt_lock);
1884
1885         mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
1886
1887         /* Back up the dynamic link speed changed by sja1105_adjust_port_config
1888          * in order to temporarily restore it to SJA1105_SPEED_AUTO - which the
1889          * switch wants to see in the static config in order to allow us to
1890          * change it through the dynamic interface later.
1891          */
1892         for (i = 0; i < ds->num_ports; i++) {
1893                 speed_mbps[i] = sja1105_port_speed_to_ethtool(priv,
1894                                                               mac[i].speed);
1895                 mac[i].speed = priv->info->port_speed[SJA1105_SPEED_AUTO];
1896
1897                 if (priv->phy_mode[i] == PHY_INTERFACE_MODE_SGMII)
1898                         bmcr[i] = sja1105_sgmii_read(priv, i,
1899                                                      MDIO_MMD_VEND2,
1900                                                      MDIO_CTRL1);
1901         }
1902
1903         /* No PTP operations can run right now */
1904         mutex_lock(&priv->ptp_data.lock);
1905
1906         rc = __sja1105_ptp_gettimex(ds, &now, &ptp_sts_before);
1907         if (rc < 0)
1908                 goto out_unlock_ptp;
1909
1910         /* Reset switch and send updated static configuration */
1911         rc = sja1105_static_config_upload(priv);
1912         if (rc < 0)
1913                 goto out_unlock_ptp;
1914
1915         rc = __sja1105_ptp_settime(ds, 0, &ptp_sts_after);
1916         if (rc < 0)
1917                 goto out_unlock_ptp;
1918
1919         t1 = timespec64_to_ns(&ptp_sts_before.pre_ts);
1920         t2 = timespec64_to_ns(&ptp_sts_before.post_ts);
1921         t3 = timespec64_to_ns(&ptp_sts_after.pre_ts);
1922         t4 = timespec64_to_ns(&ptp_sts_after.post_ts);
1923         /* Mid point, corresponds to pre-reset PTPCLKVAL */
1924         t12 = t1 + (t2 - t1) / 2;
1925         /* Mid point, corresponds to post-reset PTPCLKVAL, aka 0 */
1926         t34 = t3 + (t4 - t3) / 2;
1927         /* Advance PTPCLKVAL by the time it took since its readout */
1928         now += (t34 - t12);
1929
1930         __sja1105_ptp_adjtime(ds, now);
1931
1932 out_unlock_ptp:
1933         mutex_unlock(&priv->ptp_data.lock);
1934
1935         dev_info(priv->ds->dev,
1936                  "Reset switch and programmed static config. Reason: %s\n",
1937                  sja1105_reset_reasons[reason]);
1938
1939         /* Configure the CGU (PLLs) for MII and RMII PHYs.
1940          * For these interfaces there is no dynamic configuration
1941          * needed, since PLLs have same settings at all speeds.
1942          */
1943         rc = priv->info->clocking_setup(priv);
1944         if (rc < 0)
1945                 goto out;
1946
1947         for (i = 0; i < ds->num_ports; i++) {
1948                 bool an_enabled;
1949
1950                 rc = sja1105_adjust_port_config(priv, i, speed_mbps[i]);
1951                 if (rc < 0)
1952                         goto out;
1953
1954                 if (priv->phy_mode[i] != PHY_INTERFACE_MODE_SGMII)
1955                         continue;
1956
1957                 an_enabled = !!(bmcr[i] & BMCR_ANENABLE);
1958
1959                 sja1105_sgmii_pcs_config(priv, i, an_enabled, false);
1960
1961                 if (!an_enabled) {
1962                         int speed = SPEED_UNKNOWN;
1963
1964                         if (bmcr[i] & BMCR_SPEED1000)
1965                                 speed = SPEED_1000;
1966                         else if (bmcr[i] & BMCR_SPEED100)
1967                                 speed = SPEED_100;
1968                         else
1969                                 speed = SPEED_10;
1970
1971                         sja1105_sgmii_pcs_force_speed(priv, i, speed);
1972                 }
1973         }
1974
1975         rc = sja1105_reload_cbs(priv);
1976         if (rc < 0)
1977                 goto out;
1978 out:
1979         mutex_unlock(&priv->mgmt_lock);
1980
1981         return rc;
1982 }
1983
1984 static int sja1105_pvid_apply(struct sja1105_private *priv, int port, u16 pvid)
1985 {
1986         struct sja1105_mac_config_entry *mac;
1987
1988         mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
1989
1990         mac[port].vlanid = pvid;
1991
1992         return sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
1993                                            &mac[port], true);
1994 }
1995
1996 static int sja1105_crosschip_bridge_join(struct dsa_switch *ds,
1997                                          int tree_index, int sw_index,
1998                                          int other_port, struct net_device *br)
1999 {
2000         struct dsa_switch *other_ds = dsa_switch_find(tree_index, sw_index);
2001         struct sja1105_private *other_priv = other_ds->priv;
2002         struct sja1105_private *priv = ds->priv;
2003         int port, rc;
2004
2005         if (other_ds->ops != &sja1105_switch_ops)
2006                 return 0;
2007
2008         for (port = 0; port < ds->num_ports; port++) {
2009                 if (!dsa_is_user_port(ds, port))
2010                         continue;
2011                 if (dsa_to_port(ds, port)->bridge_dev != br)
2012                         continue;
2013
2014                 rc = dsa_8021q_crosschip_bridge_join(priv->dsa_8021q_ctx,
2015                                                      port,
2016                                                      other_priv->dsa_8021q_ctx,
2017                                                      other_port);
2018                 if (rc)
2019                         return rc;
2020
2021                 rc = dsa_8021q_crosschip_bridge_join(other_priv->dsa_8021q_ctx,
2022                                                      other_port,
2023                                                      priv->dsa_8021q_ctx,
2024                                                      port);
2025                 if (rc)
2026                         return rc;
2027         }
2028
2029         return 0;
2030 }
2031
2032 static void sja1105_crosschip_bridge_leave(struct dsa_switch *ds,
2033                                            int tree_index, int sw_index,
2034                                            int other_port,
2035                                            struct net_device *br)
2036 {
2037         struct dsa_switch *other_ds = dsa_switch_find(tree_index, sw_index);
2038         struct sja1105_private *other_priv = other_ds->priv;
2039         struct sja1105_private *priv = ds->priv;
2040         int port;
2041
2042         if (other_ds->ops != &sja1105_switch_ops)
2043                 return;
2044
2045         for (port = 0; port < ds->num_ports; port++) {
2046                 if (!dsa_is_user_port(ds, port))
2047                         continue;
2048                 if (dsa_to_port(ds, port)->bridge_dev != br)
2049                         continue;
2050
2051                 dsa_8021q_crosschip_bridge_leave(priv->dsa_8021q_ctx, port,
2052                                                  other_priv->dsa_8021q_ctx,
2053                                                  other_port);
2054
2055                 dsa_8021q_crosschip_bridge_leave(other_priv->dsa_8021q_ctx,
2056                                                  other_port,
2057                                                  priv->dsa_8021q_ctx, port);
2058         }
2059 }
2060
2061 static int sja1105_setup_8021q_tagging(struct dsa_switch *ds, bool enabled)
2062 {
2063         struct sja1105_private *priv = ds->priv;
2064         int rc;
2065
2066         rc = dsa_8021q_setup(priv->dsa_8021q_ctx, enabled);
2067         if (rc)
2068                 return rc;
2069
2070         dev_info(ds->dev, "%s switch tagging\n",
2071                  enabled ? "Enabled" : "Disabled");
2072         return 0;
2073 }
2074
2075 static enum dsa_tag_protocol
2076 sja1105_get_tag_protocol(struct dsa_switch *ds, int port,
2077                          enum dsa_tag_protocol mp)
2078 {
2079         return DSA_TAG_PROTO_SJA1105;
2080 }
2081
2082 static int sja1105_find_free_subvlan(u16 *subvlan_map, bool pvid)
2083 {
2084         int subvlan;
2085
2086         if (pvid)
2087                 return 0;
2088
2089         for (subvlan = 1; subvlan < DSA_8021Q_N_SUBVLAN; subvlan++)
2090                 if (subvlan_map[subvlan] == VLAN_N_VID)
2091                         return subvlan;
2092
2093         return -1;
2094 }
2095
2096 static int sja1105_find_subvlan(u16 *subvlan_map, u16 vid)
2097 {
2098         int subvlan;
2099
2100         for (subvlan = 0; subvlan < DSA_8021Q_N_SUBVLAN; subvlan++)
2101                 if (subvlan_map[subvlan] == vid)
2102                         return subvlan;
2103
2104         return -1;
2105 }
2106
2107 static int sja1105_find_committed_subvlan(struct sja1105_private *priv,
2108                                           int port, u16 vid)
2109 {
2110         struct sja1105_port *sp = &priv->ports[port];
2111
2112         return sja1105_find_subvlan(sp->subvlan_map, vid);
2113 }
2114
2115 static void sja1105_init_subvlan_map(u16 *subvlan_map)
2116 {
2117         int subvlan;
2118
2119         for (subvlan = 0; subvlan < DSA_8021Q_N_SUBVLAN; subvlan++)
2120                 subvlan_map[subvlan] = VLAN_N_VID;
2121 }
2122
2123 static void sja1105_commit_subvlan_map(struct sja1105_private *priv, int port,
2124                                        u16 *subvlan_map)
2125 {
2126         struct sja1105_port *sp = &priv->ports[port];
2127         int subvlan;
2128
2129         for (subvlan = 0; subvlan < DSA_8021Q_N_SUBVLAN; subvlan++)
2130                 sp->subvlan_map[subvlan] = subvlan_map[subvlan];
2131 }
2132
2133 static int sja1105_is_vlan_configured(struct sja1105_private *priv, u16 vid)
2134 {
2135         struct sja1105_vlan_lookup_entry *vlan;
2136         int count, i;
2137
2138         vlan = priv->static_config.tables[BLK_IDX_VLAN_LOOKUP].entries;
2139         count = priv->static_config.tables[BLK_IDX_VLAN_LOOKUP].entry_count;
2140
2141         for (i = 0; i < count; i++)
2142                 if (vlan[i].vlanid == vid)
2143                         return i;
2144
2145         /* Return an invalid entry index if not found */
2146         return -1;
2147 }
2148
2149 static int
2150 sja1105_find_retagging_entry(struct sja1105_retagging_entry *retagging,
2151                              int count, int from_port, u16 from_vid,
2152                              u16 to_vid)
2153 {
2154         int i;
2155
2156         for (i = 0; i < count; i++)
2157                 if (retagging[i].ing_port == BIT(from_port) &&
2158                     retagging[i].vlan_ing == from_vid &&
2159                     retagging[i].vlan_egr == to_vid)
2160                         return i;
2161
2162         /* Return an invalid entry index if not found */
2163         return -1;
2164 }
2165
2166 static int sja1105_commit_vlans(struct sja1105_private *priv,
2167                                 struct sja1105_vlan_lookup_entry *new_vlan,
2168                                 struct sja1105_retagging_entry *new_retagging,
2169                                 int num_retagging)
2170 {
2171         struct sja1105_retagging_entry *retagging;
2172         struct sja1105_vlan_lookup_entry *vlan;
2173         struct sja1105_table *table;
2174         int num_vlans = 0;
2175         int rc, i, k = 0;
2176
2177         /* VLAN table */
2178         table = &priv->static_config.tables[BLK_IDX_VLAN_LOOKUP];
2179         vlan = table->entries;
2180
2181         for (i = 0; i < VLAN_N_VID; i++) {
2182                 int match = sja1105_is_vlan_configured(priv, i);
2183
2184                 if (new_vlan[i].vlanid != VLAN_N_VID)
2185                         num_vlans++;
2186
2187                 if (new_vlan[i].vlanid == VLAN_N_VID && match >= 0) {
2188                         /* Was there before, no longer is. Delete */
2189                         dev_dbg(priv->ds->dev, "Deleting VLAN %d\n", i);
2190                         rc = sja1105_dynamic_config_write(priv,
2191                                                           BLK_IDX_VLAN_LOOKUP,
2192                                                           i, &vlan[match], false);
2193                         if (rc < 0)
2194                                 return rc;
2195                 } else if (new_vlan[i].vlanid != VLAN_N_VID) {
2196                         /* Nothing changed, don't do anything */
2197                         if (match >= 0 &&
2198                             vlan[match].vlanid == new_vlan[i].vlanid &&
2199                             vlan[match].tag_port == new_vlan[i].tag_port &&
2200                             vlan[match].vlan_bc == new_vlan[i].vlan_bc &&
2201                             vlan[match].vmemb_port == new_vlan[i].vmemb_port)
2202                                 continue;
2203                         /* Update entry */
2204                         dev_dbg(priv->ds->dev, "Updating VLAN %d\n", i);
2205                         rc = sja1105_dynamic_config_write(priv,
2206                                                           BLK_IDX_VLAN_LOOKUP,
2207                                                           i, &new_vlan[i],
2208                                                           true);
2209                         if (rc < 0)
2210                                 return rc;
2211                 }
2212         }
2213
2214         if (table->entry_count)
2215                 kfree(table->entries);
2216
2217         table->entries = kcalloc(num_vlans, table->ops->unpacked_entry_size,
2218                                  GFP_KERNEL);
2219         if (!table->entries)
2220                 return -ENOMEM;
2221
2222         table->entry_count = num_vlans;
2223         vlan = table->entries;
2224
2225         for (i = 0; i < VLAN_N_VID; i++) {
2226                 if (new_vlan[i].vlanid == VLAN_N_VID)
2227                         continue;
2228                 vlan[k++] = new_vlan[i];
2229         }
2230
2231         /* VLAN Retagging Table */
2232         table = &priv->static_config.tables[BLK_IDX_RETAGGING];
2233         retagging = table->entries;
2234
2235         for (i = 0; i < table->entry_count; i++) {
2236                 rc = sja1105_dynamic_config_write(priv, BLK_IDX_RETAGGING,
2237                                                   i, &retagging[i], false);
2238                 if (rc)
2239                         return rc;
2240         }
2241
2242         if (table->entry_count)
2243                 kfree(table->entries);
2244
2245         table->entries = kcalloc(num_retagging, table->ops->unpacked_entry_size,
2246                                  GFP_KERNEL);
2247         if (!table->entries)
2248                 return -ENOMEM;
2249
2250         table->entry_count = num_retagging;
2251         retagging = table->entries;
2252
2253         for (i = 0; i < num_retagging; i++) {
2254                 retagging[i] = new_retagging[i];
2255
2256                 /* Update entry */
2257                 rc = sja1105_dynamic_config_write(priv, BLK_IDX_RETAGGING,
2258                                                   i, &retagging[i], true);
2259                 if (rc < 0)
2260                         return rc;
2261         }
2262
2263         return 0;
2264 }
2265
2266 struct sja1105_crosschip_vlan {
2267         struct list_head list;
2268         u16 vid;
2269         bool untagged;
2270         int port;
2271         int other_port;
2272         struct dsa_8021q_context *other_ctx;
2273 };
2274
2275 struct sja1105_crosschip_switch {
2276         struct list_head list;
2277         struct dsa_8021q_context *other_ctx;
2278 };
2279
2280 static int sja1105_commit_pvid(struct sja1105_private *priv)
2281 {
2282         struct sja1105_bridge_vlan *v;
2283         struct list_head *vlan_list;
2284         int rc = 0;
2285
2286         if (priv->vlan_state == SJA1105_VLAN_FILTERING_FULL)
2287                 vlan_list = &priv->bridge_vlans;
2288         else
2289                 vlan_list = &priv->dsa_8021q_vlans;
2290
2291         list_for_each_entry(v, vlan_list, list) {
2292                 if (v->pvid) {
2293                         rc = sja1105_pvid_apply(priv, v->port, v->vid);
2294                         if (rc)
2295                                 break;
2296                 }
2297         }
2298
2299         return rc;
2300 }
2301
2302 static int
2303 sja1105_build_bridge_vlans(struct sja1105_private *priv,
2304                            struct sja1105_vlan_lookup_entry *new_vlan)
2305 {
2306         struct sja1105_bridge_vlan *v;
2307
2308         if (priv->vlan_state == SJA1105_VLAN_UNAWARE)
2309                 return 0;
2310
2311         list_for_each_entry(v, &priv->bridge_vlans, list) {
2312                 int match = v->vid;
2313
2314                 new_vlan[match].vlanid = v->vid;
2315                 new_vlan[match].vmemb_port |= BIT(v->port);
2316                 new_vlan[match].vlan_bc |= BIT(v->port);
2317                 if (!v->untagged)
2318                         new_vlan[match].tag_port |= BIT(v->port);
2319         }
2320
2321         return 0;
2322 }
2323
2324 static int
2325 sja1105_build_dsa_8021q_vlans(struct sja1105_private *priv,
2326                               struct sja1105_vlan_lookup_entry *new_vlan)
2327 {
2328         struct sja1105_bridge_vlan *v;
2329
2330         if (priv->vlan_state == SJA1105_VLAN_FILTERING_FULL)
2331                 return 0;
2332
2333         list_for_each_entry(v, &priv->dsa_8021q_vlans, list) {
2334                 int match = v->vid;
2335
2336                 new_vlan[match].vlanid = v->vid;
2337                 new_vlan[match].vmemb_port |= BIT(v->port);
2338                 new_vlan[match].vlan_bc |= BIT(v->port);
2339                 if (!v->untagged)
2340                         new_vlan[match].tag_port |= BIT(v->port);
2341         }
2342
2343         return 0;
2344 }
2345
2346 static int sja1105_build_subvlans(struct sja1105_private *priv,
2347                                   u16 subvlan_map[][DSA_8021Q_N_SUBVLAN],
2348                                   struct sja1105_vlan_lookup_entry *new_vlan,
2349                                   struct sja1105_retagging_entry *new_retagging,
2350                                   int *num_retagging)
2351 {
2352         struct sja1105_bridge_vlan *v;
2353         int k = *num_retagging;
2354
2355         if (priv->vlan_state != SJA1105_VLAN_BEST_EFFORT)
2356                 return 0;
2357
2358         list_for_each_entry(v, &priv->bridge_vlans, list) {
2359                 int upstream = dsa_upstream_port(priv->ds, v->port);
2360                 int match, subvlan;
2361                 u16 rx_vid;
2362
2363                 /* Only sub-VLANs on user ports need to be applied.
2364                  * Bridge VLANs also include VLANs added automatically
2365                  * by DSA on the CPU port.
2366                  */
2367                 if (!dsa_is_user_port(priv->ds, v->port))
2368                         continue;
2369
2370                 subvlan = sja1105_find_subvlan(subvlan_map[v->port],
2371                                                v->vid);
2372                 if (subvlan < 0) {
2373                         subvlan = sja1105_find_free_subvlan(subvlan_map[v->port],
2374                                                             v->pvid);
2375                         if (subvlan < 0) {
2376                                 dev_err(priv->ds->dev, "No more free subvlans\n");
2377                                 return -ENOSPC;
2378                         }
2379                 }
2380
2381                 rx_vid = dsa_8021q_rx_vid_subvlan(priv->ds, v->port, subvlan);
2382
2383                 /* @v->vid on @v->port needs to be retagged to @rx_vid
2384                  * on @upstream. Assume @v->vid on @v->port and on
2385                  * @upstream was already configured by the previous
2386                  * iteration over bridge_vlans.
2387                  */
2388                 match = rx_vid;
2389                 new_vlan[match].vlanid = rx_vid;
2390                 new_vlan[match].vmemb_port |= BIT(v->port);
2391                 new_vlan[match].vmemb_port |= BIT(upstream);
2392                 new_vlan[match].vlan_bc |= BIT(v->port);
2393                 new_vlan[match].vlan_bc |= BIT(upstream);
2394                 /* The "untagged" flag is set the same as for the
2395                  * original VLAN
2396                  */
2397                 if (!v->untagged)
2398                         new_vlan[match].tag_port |= BIT(v->port);
2399                 /* But it's always tagged towards the CPU */
2400                 new_vlan[match].tag_port |= BIT(upstream);
2401
2402                 /* The Retagging Table generates packet *clones* with
2403                  * the new VLAN. This is a very odd hardware quirk
2404                  * which we need to suppress by dropping the original
2405                  * packet.
2406                  * Deny egress of the original VLAN towards the CPU
2407                  * port. This will force the switch to drop it, and
2408                  * we'll see only the retagged packets.
2409                  */
2410                 match = v->vid;
2411                 new_vlan[match].vlan_bc &= ~BIT(upstream);
2412
2413                 /* And the retagging itself */
2414                 new_retagging[k].vlan_ing = v->vid;
2415                 new_retagging[k].vlan_egr = rx_vid;
2416                 new_retagging[k].ing_port = BIT(v->port);
2417                 new_retagging[k].egr_port = BIT(upstream);
2418                 if (k++ == SJA1105_MAX_RETAGGING_COUNT) {
2419                         dev_err(priv->ds->dev, "No more retagging rules\n");
2420                         return -ENOSPC;
2421                 }
2422
2423                 subvlan_map[v->port][subvlan] = v->vid;
2424         }
2425
2426         *num_retagging = k;
2427
2428         return 0;
2429 }
2430
2431 /* Sadly, in crosschip scenarios where the CPU port is also the link to another
2432  * switch, we should retag backwards (the dsa_8021q vid to the original vid) on
2433  * the CPU port of neighbour switches.
2434  */
2435 static int
2436 sja1105_build_crosschip_subvlans(struct sja1105_private *priv,
2437                                  struct sja1105_vlan_lookup_entry *new_vlan,
2438                                  struct sja1105_retagging_entry *new_retagging,
2439                                  int *num_retagging)
2440 {
2441         struct sja1105_crosschip_vlan *tmp, *pos;
2442         struct dsa_8021q_crosschip_link *c;
2443         struct sja1105_bridge_vlan *v, *w;
2444         struct list_head crosschip_vlans;
2445         int k = *num_retagging;
2446         int rc = 0;
2447
2448         if (priv->vlan_state != SJA1105_VLAN_BEST_EFFORT)
2449                 return 0;
2450
2451         INIT_LIST_HEAD(&crosschip_vlans);
2452
2453         list_for_each_entry(c, &priv->dsa_8021q_ctx->crosschip_links, list) {
2454                 struct sja1105_private *other_priv = c->other_ctx->ds->priv;
2455
2456                 if (other_priv->vlan_state == SJA1105_VLAN_FILTERING_FULL)
2457                         continue;
2458
2459                 /* Crosschip links are also added to the CPU ports.
2460                  * Ignore those.
2461                  */
2462                 if (!dsa_is_user_port(priv->ds, c->port))
2463                         continue;
2464                 if (!dsa_is_user_port(c->other_ctx->ds, c->other_port))
2465                         continue;
2466
2467                 /* Search for VLANs on the remote port */
2468                 list_for_each_entry(v, &other_priv->bridge_vlans, list) {
2469                         bool already_added = false;
2470                         bool we_have_it = false;
2471
2472                         if (v->port != c->other_port)
2473                                 continue;
2474
2475                         /* If @v is a pvid on @other_ds, it does not need
2476                          * re-retagging, because its SVL field is 0 and we
2477                          * already allow that, via the dsa_8021q crosschip
2478                          * links.
2479                          */
2480                         if (v->pvid)
2481                                 continue;
2482
2483                         /* Search for the VLAN on our local port */
2484                         list_for_each_entry(w, &priv->bridge_vlans, list) {
2485                                 if (w->port == c->port && w->vid == v->vid) {
2486                                         we_have_it = true;
2487                                         break;
2488                                 }
2489                         }
2490
2491                         if (!we_have_it)
2492                                 continue;
2493
2494                         list_for_each_entry(tmp, &crosschip_vlans, list) {
2495                                 if (tmp->vid == v->vid &&
2496                                     tmp->untagged == v->untagged &&
2497                                     tmp->port == c->port &&
2498                                     tmp->other_port == v->port &&
2499                                     tmp->other_ctx == c->other_ctx) {
2500                                         already_added = true;
2501                                         break;
2502                                 }
2503                         }
2504
2505                         if (already_added)
2506                                 continue;
2507
2508                         tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
2509                         if (!tmp) {
2510                                 dev_err(priv->ds->dev, "Failed to allocate memory\n");
2511                                 rc = -ENOMEM;
2512                                 goto out;
2513                         }
2514                         tmp->vid = v->vid;
2515                         tmp->port = c->port;
2516                         tmp->other_port = v->port;
2517                         tmp->other_ctx = c->other_ctx;
2518                         tmp->untagged = v->untagged;
2519                         list_add(&tmp->list, &crosschip_vlans);
2520                 }
2521         }
2522
2523         list_for_each_entry(tmp, &crosschip_vlans, list) {
2524                 struct sja1105_private *other_priv = tmp->other_ctx->ds->priv;
2525                 int upstream = dsa_upstream_port(priv->ds, tmp->port);
2526                 int match, subvlan;
2527                 u16 rx_vid;
2528
2529                 subvlan = sja1105_find_committed_subvlan(other_priv,
2530                                                          tmp->other_port,
2531                                                          tmp->vid);
2532                 /* If this happens, it's a bug. The neighbour switch does not
2533                  * have a subvlan for tmp->vid on tmp->other_port, but it
2534                  * should, since we already checked for its vlan_state.
2535                  */
2536                 if (WARN_ON(subvlan < 0)) {
2537                         rc = -EINVAL;
2538                         goto out;
2539                 }
2540
2541                 rx_vid = dsa_8021q_rx_vid_subvlan(tmp->other_ctx->ds,
2542                                                   tmp->other_port,
2543                                                   subvlan);
2544
2545                 /* The @rx_vid retagged from @tmp->vid on
2546                  * {@tmp->other_ds, @tmp->other_port} needs to be
2547                  * re-retagged to @tmp->vid on the way back to us.
2548                  *
2549                  * Assume the original @tmp->vid is already configured
2550                  * on this local switch, otherwise we wouldn't be
2551                  * retagging its subvlan on the other switch in the
2552                  * first place. We just need to add a reverse retagging
2553                  * rule for @rx_vid and install @rx_vid on our ports.
2554                  */
2555                 match = rx_vid;
2556                 new_vlan[match].vlanid = rx_vid;
2557                 new_vlan[match].vmemb_port |= BIT(tmp->port);
2558                 new_vlan[match].vmemb_port |= BIT(upstream);
2559                 /* The "untagged" flag is set the same as for the
2560                  * original VLAN. And towards the CPU, it doesn't
2561                  * really matter, because @rx_vid will only receive
2562                  * traffic on that port. For consistency with other dsa_8021q
2563                  * VLANs, we'll keep the CPU port tagged.
2564                  */
2565                 if (!tmp->untagged)
2566                         new_vlan[match].tag_port |= BIT(tmp->port);
2567                 new_vlan[match].tag_port |= BIT(upstream);
2568                 /* Deny egress of @rx_vid towards our front-panel port.
2569                  * This will force the switch to drop it, and we'll see
2570                  * only the re-retagged packets (having the original,
2571                  * pre-initial-retagging, VLAN @tmp->vid).
2572                  */
2573                 new_vlan[match].vlan_bc &= ~BIT(tmp->port);
2574
2575                 /* On reverse retagging, the same ingress VLAN goes to multiple
2576                  * ports. So we have an opportunity to create composite rules
2577                  * to not waste the limited space in the retagging table.
2578                  */
2579                 k = sja1105_find_retagging_entry(new_retagging, *num_retagging,
2580                                                  upstream, rx_vid, tmp->vid);
2581                 if (k < 0) {
2582                         if (*num_retagging == SJA1105_MAX_RETAGGING_COUNT) {
2583                                 dev_err(priv->ds->dev, "No more retagging rules\n");
2584                                 rc = -ENOSPC;
2585                                 goto out;
2586                         }
2587                         k = (*num_retagging)++;
2588                 }
2589                 /* And the retagging itself */
2590                 new_retagging[k].vlan_ing = rx_vid;
2591                 new_retagging[k].vlan_egr = tmp->vid;
2592                 new_retagging[k].ing_port = BIT(upstream);
2593                 new_retagging[k].egr_port |= BIT(tmp->port);
2594         }
2595
2596 out:
2597         list_for_each_entry_safe(tmp, pos, &crosschip_vlans, list) {
2598                 list_del(&tmp->list);
2599                 kfree(tmp);
2600         }
2601
2602         return rc;
2603 }
2604
2605 static int sja1105_build_vlan_table(struct sja1105_private *priv, bool notify);
2606
2607 static int sja1105_notify_crosschip_switches(struct sja1105_private *priv)
2608 {
2609         struct sja1105_crosschip_switch *s, *pos;
2610         struct list_head crosschip_switches;
2611         struct dsa_8021q_crosschip_link *c;
2612         int rc = 0;
2613
2614         INIT_LIST_HEAD(&crosschip_switches);
2615
2616         list_for_each_entry(c, &priv->dsa_8021q_ctx->crosschip_links, list) {
2617                 bool already_added = false;
2618
2619                 list_for_each_entry(s, &crosschip_switches, list) {
2620                         if (s->other_ctx == c->other_ctx) {
2621                                 already_added = true;
2622                                 break;
2623                         }
2624                 }
2625
2626                 if (already_added)
2627                         continue;
2628
2629                 s = kzalloc(sizeof(*s), GFP_KERNEL);
2630                 if (!s) {
2631                         dev_err(priv->ds->dev, "Failed to allocate memory\n");
2632                         rc = -ENOMEM;
2633                         goto out;
2634                 }
2635                 s->other_ctx = c->other_ctx;
2636                 list_add(&s->list, &crosschip_switches);
2637         }
2638
2639         list_for_each_entry(s, &crosschip_switches, list) {
2640                 struct sja1105_private *other_priv = s->other_ctx->ds->priv;
2641
2642                 rc = sja1105_build_vlan_table(other_priv, false);
2643                 if (rc)
2644                         goto out;
2645         }
2646
2647 out:
2648         list_for_each_entry_safe(s, pos, &crosschip_switches, list) {
2649                 list_del(&s->list);
2650                 kfree(s);
2651         }
2652
2653         return rc;
2654 }
2655
2656 static int sja1105_build_vlan_table(struct sja1105_private *priv, bool notify)
2657 {
2658         u16 subvlan_map[SJA1105_MAX_NUM_PORTS][DSA_8021Q_N_SUBVLAN];
2659         struct sja1105_retagging_entry *new_retagging;
2660         struct sja1105_vlan_lookup_entry *new_vlan;
2661         struct sja1105_table *table;
2662         int i, num_retagging = 0;
2663         int rc;
2664
2665         table = &priv->static_config.tables[BLK_IDX_VLAN_LOOKUP];
2666         new_vlan = kcalloc(VLAN_N_VID,
2667                            table->ops->unpacked_entry_size, GFP_KERNEL);
2668         if (!new_vlan)
2669                 return -ENOMEM;
2670
2671         table = &priv->static_config.tables[BLK_IDX_VLAN_LOOKUP];
2672         new_retagging = kcalloc(SJA1105_MAX_RETAGGING_COUNT,
2673                                 table->ops->unpacked_entry_size, GFP_KERNEL);
2674         if (!new_retagging) {
2675                 kfree(new_vlan);
2676                 return -ENOMEM;
2677         }
2678
2679         for (i = 0; i < VLAN_N_VID; i++)
2680                 new_vlan[i].vlanid = VLAN_N_VID;
2681
2682         for (i = 0; i < SJA1105_MAX_RETAGGING_COUNT; i++)
2683                 new_retagging[i].vlan_ing = VLAN_N_VID;
2684
2685         for (i = 0; i < priv->ds->num_ports; i++)
2686                 sja1105_init_subvlan_map(subvlan_map[i]);
2687
2688         /* Bridge VLANs */
2689         rc = sja1105_build_bridge_vlans(priv, new_vlan);
2690         if (rc)
2691                 goto out;
2692
2693         /* VLANs necessary for dsa_8021q operation, given to us by tag_8021q.c:
2694          * - RX VLANs
2695          * - TX VLANs
2696          * - Crosschip links
2697          */
2698         rc = sja1105_build_dsa_8021q_vlans(priv, new_vlan);
2699         if (rc)
2700                 goto out;
2701
2702         /* Private VLANs necessary for dsa_8021q operation, which we need to
2703          * determine on our own:
2704          * - Sub-VLANs
2705          * - Sub-VLANs of crosschip switches
2706          */
2707         rc = sja1105_build_subvlans(priv, subvlan_map, new_vlan, new_retagging,
2708                                     &num_retagging);
2709         if (rc)
2710                 goto out;
2711
2712         rc = sja1105_build_crosschip_subvlans(priv, new_vlan, new_retagging,
2713                                               &num_retagging);
2714         if (rc)
2715                 goto out;
2716
2717         rc = sja1105_commit_vlans(priv, new_vlan, new_retagging, num_retagging);
2718         if (rc)
2719                 goto out;
2720
2721         rc = sja1105_commit_pvid(priv);
2722         if (rc)
2723                 goto out;
2724
2725         for (i = 0; i < priv->ds->num_ports; i++)
2726                 sja1105_commit_subvlan_map(priv, i, subvlan_map[i]);
2727
2728         if (notify) {
2729                 rc = sja1105_notify_crosschip_switches(priv);
2730                 if (rc)
2731                         goto out;
2732         }
2733
2734 out:
2735         kfree(new_vlan);
2736         kfree(new_retagging);
2737
2738         return rc;
2739 }
2740
2741 /* The TPID setting belongs to the General Parameters table,
2742  * which can only be partially reconfigured at runtime (and not the TPID).
2743  * So a switch reset is required.
2744  */
2745 int sja1105_vlan_filtering(struct dsa_switch *ds, int port, bool enabled,
2746                            struct netlink_ext_ack *extack)
2747 {
2748         struct sja1105_l2_lookup_params_entry *l2_lookup_params;
2749         struct sja1105_general_params_entry *general_params;
2750         struct sja1105_private *priv = ds->priv;
2751         enum sja1105_vlan_state state;
2752         struct sja1105_table *table;
2753         struct sja1105_rule *rule;
2754         bool want_tagging;
2755         u16 tpid, tpid2;
2756         int rc;
2757
2758         list_for_each_entry(rule, &priv->flow_block.rules, list) {
2759                 if (rule->type == SJA1105_RULE_VL) {
2760                         NL_SET_ERR_MSG_MOD(extack,
2761                                            "Cannot change VLAN filtering with active VL rules");
2762                         return -EBUSY;
2763                 }
2764         }
2765
2766         if (enabled) {
2767                 /* Enable VLAN filtering. */
2768                 tpid  = ETH_P_8021Q;
2769                 tpid2 = ETH_P_8021AD;
2770         } else {
2771                 /* Disable VLAN filtering. */
2772                 tpid  = ETH_P_SJA1105;
2773                 tpid2 = ETH_P_SJA1105;
2774         }
2775
2776         for (port = 0; port < ds->num_ports; port++) {
2777                 struct sja1105_port *sp = &priv->ports[port];
2778
2779                 if (enabled)
2780                         sp->xmit_tpid = priv->info->qinq_tpid;
2781                 else
2782                         sp->xmit_tpid = ETH_P_SJA1105;
2783         }
2784
2785         if (!enabled)
2786                 state = SJA1105_VLAN_UNAWARE;
2787         else if (priv->best_effort_vlan_filtering)
2788                 state = SJA1105_VLAN_BEST_EFFORT;
2789         else
2790                 state = SJA1105_VLAN_FILTERING_FULL;
2791
2792         if (priv->vlan_state == state)
2793                 return 0;
2794
2795         priv->vlan_state = state;
2796         want_tagging = (state == SJA1105_VLAN_UNAWARE ||
2797                         state == SJA1105_VLAN_BEST_EFFORT);
2798
2799         table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
2800         general_params = table->entries;
2801         /* EtherType used to identify inner tagged (C-tag) VLAN traffic */
2802         general_params->tpid = tpid;
2803         /* EtherType used to identify outer tagged (S-tag) VLAN traffic */
2804         general_params->tpid2 = tpid2;
2805         /* When VLAN filtering is on, we need to at least be able to
2806          * decode management traffic through the "backup plan".
2807          */
2808         general_params->incl_srcpt1 = enabled;
2809         general_params->incl_srcpt0 = enabled;
2810
2811         want_tagging = priv->best_effort_vlan_filtering || !enabled;
2812
2813         /* VLAN filtering => independent VLAN learning.
2814          * No VLAN filtering (or best effort) => shared VLAN learning.
2815          *
2816          * In shared VLAN learning mode, untagged traffic still gets
2817          * pvid-tagged, and the FDB table gets populated with entries
2818          * containing the "real" (pvid or from VLAN tag) VLAN ID.
2819          * However the switch performs a masked L2 lookup in the FDB,
2820          * effectively only looking up a frame's DMAC (and not VID) for the
2821          * forwarding decision.
2822          *
2823          * This is extremely convenient for us, because in modes with
2824          * vlan_filtering=0, dsa_8021q actually installs unique pvid's into
2825          * each front panel port. This is good for identification but breaks
2826          * learning badly - the VID of the learnt FDB entry is unique, aka
2827          * no frames coming from any other port are going to have it. So
2828          * for forwarding purposes, this is as though learning was broken
2829          * (all frames get flooded).
2830          */
2831         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP_PARAMS];
2832         l2_lookup_params = table->entries;
2833         l2_lookup_params->shared_learn = want_tagging;
2834
2835         sja1105_frame_memory_partitioning(priv);
2836
2837         rc = sja1105_build_vlan_table(priv, false);
2838         if (rc)
2839                 return rc;
2840
2841         rc = sja1105_static_config_reload(priv, SJA1105_VLAN_FILTERING);
2842         if (rc)
2843                 NL_SET_ERR_MSG_MOD(extack, "Failed to change VLAN Ethertype");
2844
2845         /* Switch port identification based on 802.1Q is only passable
2846          * if we are not under a vlan_filtering bridge. So make sure
2847          * the two configurations are mutually exclusive (of course, the
2848          * user may know better, i.e. best_effort_vlan_filtering).
2849          */
2850         return sja1105_setup_8021q_tagging(ds, want_tagging);
2851 }
2852
2853 /* Returns number of VLANs added (0 or 1) on success,
2854  * or a negative error code.
2855  */
2856 static int sja1105_vlan_add_one(struct dsa_switch *ds, int port, u16 vid,
2857                                 u16 flags, struct list_head *vlan_list)
2858 {
2859         bool untagged = flags & BRIDGE_VLAN_INFO_UNTAGGED;
2860         bool pvid = flags & BRIDGE_VLAN_INFO_PVID;
2861         struct sja1105_bridge_vlan *v;
2862
2863         list_for_each_entry(v, vlan_list, list) {
2864                 if (v->port == port && v->vid == vid) {
2865                         /* Already added */
2866                         if (v->untagged == untagged && v->pvid == pvid)
2867                                 /* Nothing changed */
2868                                 return 0;
2869
2870                         /* It's the same VLAN, but some of the flags changed
2871                          * and the user did not bother to delete it first.
2872                          * Update it and trigger sja1105_build_vlan_table.
2873                          */
2874                         v->untagged = untagged;
2875                         v->pvid = pvid;
2876                         return 1;
2877                 }
2878         }
2879
2880         v = kzalloc(sizeof(*v), GFP_KERNEL);
2881         if (!v) {
2882                 dev_err(ds->dev, "Out of memory while storing VLAN\n");
2883                 return -ENOMEM;
2884         }
2885
2886         v->port = port;
2887         v->vid = vid;
2888         v->untagged = untagged;
2889         v->pvid = pvid;
2890         list_add(&v->list, vlan_list);
2891
2892         return 1;
2893 }
2894
2895 /* Returns number of VLANs deleted (0 or 1) */
2896 static int sja1105_vlan_del_one(struct dsa_switch *ds, int port, u16 vid,
2897                                 struct list_head *vlan_list)
2898 {
2899         struct sja1105_bridge_vlan *v, *n;
2900
2901         list_for_each_entry_safe(v, n, vlan_list, list) {
2902                 if (v->port == port && v->vid == vid) {
2903                         list_del(&v->list);
2904                         kfree(v);
2905                         return 1;
2906                 }
2907         }
2908
2909         return 0;
2910 }
2911
2912 static int sja1105_vlan_add(struct dsa_switch *ds, int port,
2913                             const struct switchdev_obj_port_vlan *vlan,
2914                             struct netlink_ext_ack *extack)
2915 {
2916         struct sja1105_private *priv = ds->priv;
2917         bool vlan_table_changed = false;
2918         int rc;
2919
2920         /* If the user wants best-effort VLAN filtering (aka vlan_filtering
2921          * bridge plus tagging), be sure to at least deny alterations to the
2922          * configuration done by dsa_8021q.
2923          */
2924         if (priv->vlan_state != SJA1105_VLAN_FILTERING_FULL &&
2925             vid_is_dsa_8021q(vlan->vid)) {
2926                 NL_SET_ERR_MSG_MOD(extack,
2927                                    "Range 1024-3071 reserved for dsa_8021q operation");
2928                 return -EBUSY;
2929         }
2930
2931         rc = sja1105_vlan_add_one(ds, port, vlan->vid, vlan->flags,
2932                                   &priv->bridge_vlans);
2933         if (rc < 0)
2934                 return rc;
2935         if (rc > 0)
2936                 vlan_table_changed = true;
2937
2938         if (!vlan_table_changed)
2939                 return 0;
2940
2941         return sja1105_build_vlan_table(priv, true);
2942 }
2943
2944 static int sja1105_vlan_del(struct dsa_switch *ds, int port,
2945                             const struct switchdev_obj_port_vlan *vlan)
2946 {
2947         struct sja1105_private *priv = ds->priv;
2948         bool vlan_table_changed = false;
2949         int rc;
2950
2951         rc = sja1105_vlan_del_one(ds, port, vlan->vid, &priv->bridge_vlans);
2952         if (rc > 0)
2953                 vlan_table_changed = true;
2954
2955         if (!vlan_table_changed)
2956                 return 0;
2957
2958         return sja1105_build_vlan_table(priv, true);
2959 }
2960
2961 static int sja1105_dsa_8021q_vlan_add(struct dsa_switch *ds, int port, u16 vid,
2962                                       u16 flags)
2963 {
2964         struct sja1105_private *priv = ds->priv;
2965         int rc;
2966
2967         rc = sja1105_vlan_add_one(ds, port, vid, flags, &priv->dsa_8021q_vlans);
2968         if (rc <= 0)
2969                 return rc;
2970
2971         return sja1105_build_vlan_table(priv, true);
2972 }
2973
2974 static int sja1105_dsa_8021q_vlan_del(struct dsa_switch *ds, int port, u16 vid)
2975 {
2976         struct sja1105_private *priv = ds->priv;
2977         int rc;
2978
2979         rc = sja1105_vlan_del_one(ds, port, vid, &priv->dsa_8021q_vlans);
2980         if (!rc)
2981                 return 0;
2982
2983         return sja1105_build_vlan_table(priv, true);
2984 }
2985
2986 static const struct dsa_8021q_ops sja1105_dsa_8021q_ops = {
2987         .vlan_add       = sja1105_dsa_8021q_vlan_add,
2988         .vlan_del       = sja1105_dsa_8021q_vlan_del,
2989 };
2990
2991 /* The programming model for the SJA1105 switch is "all-at-once" via static
2992  * configuration tables. Some of these can be dynamically modified at runtime,
2993  * but not the xMII mode parameters table.
2994  * Furthermode, some PHYs may not have crystals for generating their clocks
2995  * (e.g. RMII). Instead, their 50MHz clock is supplied via the SJA1105 port's
2996  * ref_clk pin. So port clocking needs to be initialized early, before
2997  * connecting to PHYs is attempted, otherwise they won't respond through MDIO.
2998  * Setting correct PHY link speed does not matter now.
2999  * But dsa_slave_phy_setup is called later than sja1105_setup, so the PHY
3000  * bindings are not yet parsed by DSA core. We need to parse early so that we
3001  * can populate the xMII mode parameters table.
3002  */
3003 static int sja1105_setup(struct dsa_switch *ds)
3004 {
3005         struct sja1105_dt_port ports[SJA1105_MAX_NUM_PORTS];
3006         struct sja1105_private *priv = ds->priv;
3007         int rc;
3008
3009         rc = sja1105_parse_dt(priv, ports);
3010         if (rc < 0) {
3011                 dev_err(ds->dev, "Failed to parse DT: %d\n", rc);
3012                 return rc;
3013         }
3014
3015         /* Error out early if internal delays are required through DT
3016          * and we can't apply them.
3017          */
3018         rc = sja1105_parse_rgmii_delays(priv, ports);
3019         if (rc < 0) {
3020                 dev_err(ds->dev, "RGMII delay not supported\n");
3021                 return rc;
3022         }
3023
3024         rc = sja1105_ptp_clock_register(ds);
3025         if (rc < 0) {
3026                 dev_err(ds->dev, "Failed to register PTP clock: %d\n", rc);
3027                 return rc;
3028         }
3029         /* Create and send configuration down to device */
3030         rc = sja1105_static_config_load(priv, ports);
3031         if (rc < 0) {
3032                 dev_err(ds->dev, "Failed to load static config: %d\n", rc);
3033                 goto out_ptp_clock_unregister;
3034         }
3035         /* Configure the CGU (PHY link modes and speeds) */
3036         rc = priv->info->clocking_setup(priv);
3037         if (rc < 0) {
3038                 dev_err(ds->dev, "Failed to configure MII clocking: %d\n", rc);
3039                 goto out_static_config_free;
3040         }
3041         /* On SJA1105, VLAN filtering per se is always enabled in hardware.
3042          * The only thing we can do to disable it is lie about what the 802.1Q
3043          * EtherType is.
3044          * So it will still try to apply VLAN filtering, but all ingress
3045          * traffic (except frames received with EtherType of ETH_P_SJA1105)
3046          * will be internally tagged with a distorted VLAN header where the
3047          * TPID is ETH_P_SJA1105, and the VLAN ID is the port pvid.
3048          */
3049         ds->vlan_filtering_is_global = true;
3050
3051         /* Advertise the 8 egress queues */
3052         ds->num_tx_queues = SJA1105_NUM_TC;
3053
3054         ds->mtu_enforcement_ingress = true;
3055
3056         priv->best_effort_vlan_filtering = true;
3057
3058         rc = sja1105_devlink_setup(ds);
3059         if (rc < 0)
3060                 goto out_static_config_free;
3061
3062         /* The DSA/switchdev model brings up switch ports in standalone mode by
3063          * default, and that means vlan_filtering is 0 since they're not under
3064          * a bridge, so it's safe to set up switch tagging at this time.
3065          */
3066         rtnl_lock();
3067         rc = sja1105_setup_8021q_tagging(ds, true);
3068         rtnl_unlock();
3069         if (rc)
3070                 goto out_devlink_teardown;
3071
3072         return 0;
3073
3074 out_devlink_teardown:
3075         sja1105_devlink_teardown(ds);
3076 out_ptp_clock_unregister:
3077         sja1105_ptp_clock_unregister(ds);
3078 out_static_config_free:
3079         sja1105_static_config_free(&priv->static_config);
3080
3081         return rc;
3082 }
3083
3084 static void sja1105_teardown(struct dsa_switch *ds)
3085 {
3086         struct sja1105_private *priv = ds->priv;
3087         struct sja1105_bridge_vlan *v, *n;
3088         int port;
3089
3090         for (port = 0; port < ds->num_ports; port++) {
3091                 struct sja1105_port *sp = &priv->ports[port];
3092
3093                 if (!dsa_is_user_port(ds, port))
3094                         continue;
3095
3096                 if (sp->xmit_worker)
3097                         kthread_destroy_worker(sp->xmit_worker);
3098         }
3099
3100         sja1105_devlink_teardown(ds);
3101         sja1105_flower_teardown(ds);
3102         sja1105_tas_teardown(ds);
3103         sja1105_ptp_clock_unregister(ds);
3104         sja1105_static_config_free(&priv->static_config);
3105
3106         list_for_each_entry_safe(v, n, &priv->dsa_8021q_vlans, list) {
3107                 list_del(&v->list);
3108                 kfree(v);
3109         }
3110
3111         list_for_each_entry_safe(v, n, &priv->bridge_vlans, list) {
3112                 list_del(&v->list);
3113                 kfree(v);
3114         }
3115 }
3116
3117 static void sja1105_port_disable(struct dsa_switch *ds, int port)
3118 {
3119         struct sja1105_private *priv = ds->priv;
3120         struct sja1105_port *sp = &priv->ports[port];
3121
3122         if (!dsa_is_user_port(ds, port))
3123                 return;
3124
3125         kthread_cancel_work_sync(&sp->xmit_work);
3126         skb_queue_purge(&sp->xmit_queue);
3127 }
3128
3129 static int sja1105_mgmt_xmit(struct dsa_switch *ds, int port, int slot,
3130                              struct sk_buff *skb, bool takets)
3131 {
3132         struct sja1105_mgmt_entry mgmt_route = {0};
3133         struct sja1105_private *priv = ds->priv;
3134         struct ethhdr *hdr;
3135         int timeout = 10;
3136         int rc;
3137
3138         hdr = eth_hdr(skb);
3139
3140         mgmt_route.macaddr = ether_addr_to_u64(hdr->h_dest);
3141         mgmt_route.destports = BIT(port);
3142         mgmt_route.enfport = 1;
3143         mgmt_route.tsreg = 0;
3144         mgmt_route.takets = takets;
3145
3146         rc = sja1105_dynamic_config_write(priv, BLK_IDX_MGMT_ROUTE,
3147                                           slot, &mgmt_route, true);
3148         if (rc < 0) {
3149                 kfree_skb(skb);
3150                 return rc;
3151         }
3152
3153         /* Transfer skb to the host port. */
3154         dsa_enqueue_skb(skb, dsa_to_port(ds, port)->slave);
3155
3156         /* Wait until the switch has processed the frame */
3157         do {
3158                 rc = sja1105_dynamic_config_read(priv, BLK_IDX_MGMT_ROUTE,
3159                                                  slot, &mgmt_route);
3160                 if (rc < 0) {
3161                         dev_err_ratelimited(priv->ds->dev,
3162                                             "failed to poll for mgmt route\n");
3163                         continue;
3164                 }
3165
3166                 /* UM10944: The ENFPORT flag of the respective entry is
3167                  * cleared when a match is found. The host can use this
3168                  * flag as an acknowledgment.
3169                  */
3170                 cpu_relax();
3171         } while (mgmt_route.enfport && --timeout);
3172
3173         if (!timeout) {
3174                 /* Clean up the management route so that a follow-up
3175                  * frame may not match on it by mistake.
3176                  * This is only hardware supported on P/Q/R/S - on E/T it is
3177                  * a no-op and we are silently discarding the -EOPNOTSUPP.
3178                  */
3179                 sja1105_dynamic_config_write(priv, BLK_IDX_MGMT_ROUTE,
3180                                              slot, &mgmt_route, false);
3181                 dev_err_ratelimited(priv->ds->dev, "xmit timed out\n");
3182         }
3183
3184         return NETDEV_TX_OK;
3185 }
3186
3187 #define work_to_port(work) \
3188                 container_of((work), struct sja1105_port, xmit_work)
3189 #define tagger_to_sja1105(t) \
3190                 container_of((t), struct sja1105_private, tagger_data)
3191
3192 /* Deferred work is unfortunately necessary because setting up the management
3193  * route cannot be done from atomit context (SPI transfer takes a sleepable
3194  * lock on the bus)
3195  */
3196 static void sja1105_port_deferred_xmit(struct kthread_work *work)
3197 {
3198         struct sja1105_port *sp = work_to_port(work);
3199         struct sja1105_tagger_data *tagger_data = sp->data;
3200         struct sja1105_private *priv = tagger_to_sja1105(tagger_data);
3201         int port = sp - priv->ports;
3202         struct sk_buff *skb;
3203
3204         while ((skb = skb_dequeue(&sp->xmit_queue)) != NULL) {
3205                 struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
3206
3207                 mutex_lock(&priv->mgmt_lock);
3208
3209                 sja1105_mgmt_xmit(priv->ds, port, 0, skb, !!clone);
3210
3211                 /* The clone, if there, was made by dsa_skb_tx_timestamp */
3212                 if (clone)
3213                         sja1105_ptp_txtstamp_skb(priv->ds, port, clone);
3214
3215                 mutex_unlock(&priv->mgmt_lock);
3216         }
3217 }
3218
3219 /* The MAXAGE setting belongs to the L2 Forwarding Parameters table,
3220  * which cannot be reconfigured at runtime. So a switch reset is required.
3221  */
3222 static int sja1105_set_ageing_time(struct dsa_switch *ds,
3223                                    unsigned int ageing_time)
3224 {
3225         struct sja1105_l2_lookup_params_entry *l2_lookup_params;
3226         struct sja1105_private *priv = ds->priv;
3227         struct sja1105_table *table;
3228         unsigned int maxage;
3229
3230         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP_PARAMS];
3231         l2_lookup_params = table->entries;
3232
3233         maxage = SJA1105_AGEING_TIME_MS(ageing_time);
3234
3235         if (l2_lookup_params->maxage == maxage)
3236                 return 0;
3237
3238         l2_lookup_params->maxage = maxage;
3239
3240         return sja1105_static_config_reload(priv, SJA1105_AGEING_TIME);
3241 }
3242
3243 static int sja1105_change_mtu(struct dsa_switch *ds, int port, int new_mtu)
3244 {
3245         struct sja1105_l2_policing_entry *policing;
3246         struct sja1105_private *priv = ds->priv;
3247
3248         new_mtu += VLAN_ETH_HLEN + ETH_FCS_LEN;
3249
3250         if (dsa_is_cpu_port(ds, port))
3251                 new_mtu += VLAN_HLEN;
3252
3253         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
3254
3255         if (policing[port].maxlen == new_mtu)
3256                 return 0;
3257
3258         policing[port].maxlen = new_mtu;
3259
3260         return sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
3261 }
3262
3263 static int sja1105_get_max_mtu(struct dsa_switch *ds, int port)
3264 {
3265         return 2043 - VLAN_ETH_HLEN - ETH_FCS_LEN;
3266 }
3267
3268 static int sja1105_port_setup_tc(struct dsa_switch *ds, int port,
3269                                  enum tc_setup_type type,
3270                                  void *type_data)
3271 {
3272         switch (type) {
3273         case TC_SETUP_QDISC_TAPRIO:
3274                 return sja1105_setup_tc_taprio(ds, port, type_data);
3275         case TC_SETUP_QDISC_CBS:
3276                 return sja1105_setup_tc_cbs(ds, port, type_data);
3277         default:
3278                 return -EOPNOTSUPP;
3279         }
3280 }
3281
3282 /* We have a single mirror (@to) port, but can configure ingress and egress
3283  * mirroring on all other (@from) ports.
3284  * We need to allow mirroring rules only as long as the @to port is always the
3285  * same, and we need to unset the @to port from mirr_port only when there is no
3286  * mirroring rule that references it.
3287  */
3288 static int sja1105_mirror_apply(struct sja1105_private *priv, int from, int to,
3289                                 bool ingress, bool enabled)
3290 {
3291         struct sja1105_general_params_entry *general_params;
3292         struct sja1105_mac_config_entry *mac;
3293         struct dsa_switch *ds = priv->ds;
3294         struct sja1105_table *table;
3295         bool already_enabled;
3296         u64 new_mirr_port;
3297         int rc;
3298
3299         table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
3300         general_params = table->entries;
3301
3302         mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
3303
3304         already_enabled = (general_params->mirr_port != ds->num_ports);
3305         if (already_enabled && enabled && general_params->mirr_port != to) {
3306                 dev_err(priv->ds->dev,
3307                         "Delete mirroring rules towards port %llu first\n",
3308                         general_params->mirr_port);
3309                 return -EBUSY;
3310         }
3311
3312         new_mirr_port = to;
3313         if (!enabled) {
3314                 bool keep = false;
3315                 int port;
3316
3317                 /* Anybody still referencing mirr_port? */
3318                 for (port = 0; port < ds->num_ports; port++) {
3319                         if (mac[port].ing_mirr || mac[port].egr_mirr) {
3320                                 keep = true;
3321                                 break;
3322                         }
3323                 }
3324                 /* Unset already_enabled for next time */
3325                 if (!keep)
3326                         new_mirr_port = ds->num_ports;
3327         }
3328         if (new_mirr_port != general_params->mirr_port) {
3329                 general_params->mirr_port = new_mirr_port;
3330
3331                 rc = sja1105_dynamic_config_write(priv, BLK_IDX_GENERAL_PARAMS,
3332                                                   0, general_params, true);
3333                 if (rc < 0)
3334                         return rc;
3335         }
3336
3337         if (ingress)
3338                 mac[from].ing_mirr = enabled;
3339         else
3340                 mac[from].egr_mirr = enabled;
3341
3342         return sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, from,
3343                                             &mac[from], true);
3344 }
3345
3346 static int sja1105_mirror_add(struct dsa_switch *ds, int port,
3347                               struct dsa_mall_mirror_tc_entry *mirror,
3348                               bool ingress)
3349 {
3350         return sja1105_mirror_apply(ds->priv, port, mirror->to_local_port,
3351                                     ingress, true);
3352 }
3353
3354 static void sja1105_mirror_del(struct dsa_switch *ds, int port,
3355                                struct dsa_mall_mirror_tc_entry *mirror)
3356 {
3357         sja1105_mirror_apply(ds->priv, port, mirror->to_local_port,
3358                              mirror->ingress, false);
3359 }
3360
3361 static int sja1105_port_policer_add(struct dsa_switch *ds, int port,
3362                                     struct dsa_mall_policer_tc_entry *policer)
3363 {
3364         struct sja1105_l2_policing_entry *policing;
3365         struct sja1105_private *priv = ds->priv;
3366
3367         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
3368
3369         /* In hardware, every 8 microseconds the credit level is incremented by
3370          * the value of RATE bytes divided by 64, up to a maximum of SMAX
3371          * bytes.
3372          */
3373         policing[port].rate = div_u64(512 * policer->rate_bytes_per_sec,
3374                                       1000000);
3375         policing[port].smax = policer->burst;
3376
3377         return sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
3378 }
3379
3380 static void sja1105_port_policer_del(struct dsa_switch *ds, int port)
3381 {
3382         struct sja1105_l2_policing_entry *policing;
3383         struct sja1105_private *priv = ds->priv;
3384
3385         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
3386
3387         policing[port].rate = SJA1105_RATE_MBPS(1000);
3388         policing[port].smax = 65535;
3389
3390         sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
3391 }
3392
3393 static int sja1105_port_set_learning(struct sja1105_private *priv, int port,
3394                                      bool enabled)
3395 {
3396         struct sja1105_mac_config_entry *mac;
3397         int rc;
3398
3399         mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
3400
3401         mac[port].dyn_learn = enabled;
3402
3403         rc = sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
3404                                           &mac[port], true);
3405         if (rc)
3406                 return rc;
3407
3408         if (enabled)
3409                 priv->learn_ena |= BIT(port);
3410         else
3411                 priv->learn_ena &= ~BIT(port);
3412
3413         return 0;
3414 }
3415
3416 static int sja1105_port_ucast_bcast_flood(struct sja1105_private *priv, int to,
3417                                           struct switchdev_brport_flags flags)
3418 {
3419         if (flags.mask & BR_FLOOD) {
3420                 if (flags.val & BR_FLOOD)
3421                         priv->ucast_egress_floods |= BIT(to);
3422                 else
3423                         priv->ucast_egress_floods &= ~BIT(to);
3424         }
3425
3426         if (flags.mask & BR_BCAST_FLOOD) {
3427                 if (flags.val & BR_BCAST_FLOOD)
3428                         priv->bcast_egress_floods |= BIT(to);
3429                 else
3430                         priv->bcast_egress_floods &= ~BIT(to);
3431         }
3432
3433         return sja1105_manage_flood_domains(priv);
3434 }
3435
3436 static int sja1105_port_mcast_flood(struct sja1105_private *priv, int to,
3437                                     struct switchdev_brport_flags flags,
3438                                     struct netlink_ext_ack *extack)
3439 {
3440         struct sja1105_l2_lookup_entry *l2_lookup;
3441         struct sja1105_table *table;
3442         int match;
3443
3444         table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
3445         l2_lookup = table->entries;
3446
3447         for (match = 0; match < table->entry_count; match++)
3448                 if (l2_lookup[match].macaddr == SJA1105_UNKNOWN_MULTICAST &&
3449                     l2_lookup[match].mask_macaddr == SJA1105_UNKNOWN_MULTICAST)
3450                         break;
3451
3452         if (match == table->entry_count) {
3453                 NL_SET_ERR_MSG_MOD(extack,
3454                                    "Could not find FDB entry for unknown multicast");
3455                 return -ENOSPC;
3456         }
3457
3458         if (flags.val & BR_MCAST_FLOOD)
3459                 l2_lookup[match].destports |= BIT(to);
3460         else
3461                 l2_lookup[match].destports &= ~BIT(to);
3462
3463         return sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
3464                                             l2_lookup[match].index,
3465                                             &l2_lookup[match],
3466                                             true);
3467 }
3468
3469 static int sja1105_port_pre_bridge_flags(struct dsa_switch *ds, int port,
3470                                          struct switchdev_brport_flags flags,
3471                                          struct netlink_ext_ack *extack)
3472 {
3473         struct sja1105_private *priv = ds->priv;
3474
3475         if (flags.mask & ~(BR_LEARNING | BR_FLOOD | BR_MCAST_FLOOD |
3476                            BR_BCAST_FLOOD))
3477                 return -EINVAL;
3478
3479         if (flags.mask & (BR_FLOOD | BR_MCAST_FLOOD) &&
3480             !priv->info->can_limit_mcast_flood) {
3481                 bool multicast = !!(flags.val & BR_MCAST_FLOOD);
3482                 bool unicast = !!(flags.val & BR_FLOOD);
3483
3484                 if (unicast != multicast) {
3485                         NL_SET_ERR_MSG_MOD(extack,
3486                                            "This chip cannot configure multicast flooding independently of unicast");
3487                         return -EINVAL;
3488                 }
3489         }
3490
3491         return 0;
3492 }
3493
3494 static int sja1105_port_bridge_flags(struct dsa_switch *ds, int port,
3495                                      struct switchdev_brport_flags flags,
3496                                      struct netlink_ext_ack *extack)
3497 {
3498         struct sja1105_private *priv = ds->priv;
3499         int rc;
3500
3501         if (flags.mask & BR_LEARNING) {
3502                 bool learn_ena = !!(flags.val & BR_LEARNING);
3503
3504                 rc = sja1105_port_set_learning(priv, port, learn_ena);
3505                 if (rc)
3506                         return rc;
3507         }
3508
3509         if (flags.mask & (BR_FLOOD | BR_BCAST_FLOOD)) {
3510                 rc = sja1105_port_ucast_bcast_flood(priv, port, flags);
3511                 if (rc)
3512                         return rc;
3513         }
3514
3515         /* For chips that can't offload BR_MCAST_FLOOD independently, there
3516          * is nothing to do here, we ensured the configuration is in sync by
3517          * offloading BR_FLOOD.
3518          */
3519         if (flags.mask & BR_MCAST_FLOOD && priv->info->can_limit_mcast_flood) {
3520                 rc = sja1105_port_mcast_flood(priv, port, flags,
3521                                               extack);
3522                 if (rc)
3523                         return rc;
3524         }
3525
3526         return 0;
3527 }
3528
3529 static const struct dsa_switch_ops sja1105_switch_ops = {
3530         .get_tag_protocol       = sja1105_get_tag_protocol,
3531         .setup                  = sja1105_setup,
3532         .teardown               = sja1105_teardown,
3533         .set_ageing_time        = sja1105_set_ageing_time,
3534         .port_change_mtu        = sja1105_change_mtu,
3535         .port_max_mtu           = sja1105_get_max_mtu,
3536         .phylink_validate       = sja1105_phylink_validate,
3537         .phylink_mac_link_state = sja1105_mac_pcs_get_state,
3538         .phylink_mac_config     = sja1105_mac_config,
3539         .phylink_mac_link_up    = sja1105_mac_link_up,
3540         .phylink_mac_link_down  = sja1105_mac_link_down,
3541         .get_strings            = sja1105_get_strings,
3542         .get_ethtool_stats      = sja1105_get_ethtool_stats,
3543         .get_sset_count         = sja1105_get_sset_count,
3544         .get_ts_info            = sja1105_get_ts_info,
3545         .port_disable           = sja1105_port_disable,
3546         .port_fdb_dump          = sja1105_fdb_dump,
3547         .port_fdb_add           = sja1105_fdb_add,
3548         .port_fdb_del           = sja1105_fdb_del,
3549         .port_bridge_join       = sja1105_bridge_join,
3550         .port_bridge_leave      = sja1105_bridge_leave,
3551         .port_pre_bridge_flags  = sja1105_port_pre_bridge_flags,
3552         .port_bridge_flags      = sja1105_port_bridge_flags,
3553         .port_stp_state_set     = sja1105_bridge_stp_state_set,
3554         .port_vlan_filtering    = sja1105_vlan_filtering,
3555         .port_vlan_add          = sja1105_vlan_add,
3556         .port_vlan_del          = sja1105_vlan_del,
3557         .port_mdb_add           = sja1105_mdb_add,
3558         .port_mdb_del           = sja1105_mdb_del,
3559         .port_hwtstamp_get      = sja1105_hwtstamp_get,
3560         .port_hwtstamp_set      = sja1105_hwtstamp_set,
3561         .port_rxtstamp          = sja1105_port_rxtstamp,
3562         .port_txtstamp          = sja1105_port_txtstamp,
3563         .port_setup_tc          = sja1105_port_setup_tc,
3564         .port_mirror_add        = sja1105_mirror_add,
3565         .port_mirror_del        = sja1105_mirror_del,
3566         .port_policer_add       = sja1105_port_policer_add,
3567         .port_policer_del       = sja1105_port_policer_del,
3568         .cls_flower_add         = sja1105_cls_flower_add,
3569         .cls_flower_del         = sja1105_cls_flower_del,
3570         .cls_flower_stats       = sja1105_cls_flower_stats,
3571         .crosschip_bridge_join  = sja1105_crosschip_bridge_join,
3572         .crosschip_bridge_leave = sja1105_crosschip_bridge_leave,
3573         .devlink_param_get      = sja1105_devlink_param_get,
3574         .devlink_param_set      = sja1105_devlink_param_set,
3575         .devlink_info_get       = sja1105_devlink_info_get,
3576 };
3577
3578 static const struct of_device_id sja1105_dt_ids[];
3579
3580 static int sja1105_check_device_id(struct sja1105_private *priv)
3581 {
3582         const struct sja1105_regs *regs = priv->info->regs;
3583         u8 prod_id[SJA1105_SIZE_DEVICE_ID] = {0};
3584         struct device *dev = &priv->spidev->dev;
3585         const struct of_device_id *match;
3586         u32 device_id;
3587         u64 part_no;
3588         int rc;
3589
3590         rc = sja1105_xfer_u32(priv, SPI_READ, regs->device_id, &device_id,
3591                               NULL);
3592         if (rc < 0)
3593                 return rc;
3594
3595         rc = sja1105_xfer_buf(priv, SPI_READ, regs->prod_id, prod_id,
3596                               SJA1105_SIZE_DEVICE_ID);
3597         if (rc < 0)
3598                 return rc;
3599
3600         sja1105_unpack(prod_id, &part_no, 19, 4, SJA1105_SIZE_DEVICE_ID);
3601
3602         for (match = sja1105_dt_ids; match->compatible[0]; match++) {
3603                 const struct sja1105_info *info = match->data;
3604
3605                 /* Is what's been probed in our match table at all? */
3606                 if (info->device_id != device_id || info->part_no != part_no)
3607                         continue;
3608
3609                 /* But is it what's in the device tree? */
3610                 if (priv->info->device_id != device_id ||
3611                     priv->info->part_no != part_no) {
3612                         dev_warn(dev, "Device tree specifies chip %s but found %s, please fix it!\n",
3613                                  priv->info->name, info->name);
3614                         /* It isn't. No problem, pick that up. */
3615                         priv->info = info;
3616                 }
3617
3618                 return 0;
3619         }
3620
3621         dev_err(dev, "Unexpected {device ID, part number}: 0x%x 0x%llx\n",
3622                 device_id, part_no);
3623
3624         return -ENODEV;
3625 }
3626
3627 static int sja1105_probe(struct spi_device *spi)
3628 {
3629         struct sja1105_tagger_data *tagger_data;
3630         struct device *dev = &spi->dev;
3631         struct sja1105_private *priv;
3632         size_t max_xfer, max_msg;
3633         struct dsa_switch *ds;
3634         int rc, port;
3635
3636         if (!dev->of_node) {
3637                 dev_err(dev, "No DTS bindings for SJA1105 driver\n");
3638                 return -EINVAL;
3639         }
3640
3641         priv = devm_kzalloc(dev, sizeof(struct sja1105_private), GFP_KERNEL);
3642         if (!priv)
3643                 return -ENOMEM;
3644
3645         /* Configure the optional reset pin and bring up switch */
3646         priv->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
3647         if (IS_ERR(priv->reset_gpio))
3648                 dev_dbg(dev, "reset-gpios not defined, ignoring\n");
3649         else
3650                 sja1105_hw_reset(priv->reset_gpio, 1, 1);
3651
3652         /* Populate our driver private structure (priv) based on
3653          * the device tree node that was probed (spi)
3654          */
3655         priv->spidev = spi;
3656         spi_set_drvdata(spi, priv);
3657
3658         /* Configure the SPI bus */
3659         spi->bits_per_word = 8;
3660         rc = spi_setup(spi);
3661         if (rc < 0) {
3662                 dev_err(dev, "Could not init SPI\n");
3663                 return rc;
3664         }
3665
3666         /* In sja1105_xfer, we send spi_messages composed of two spi_transfers:
3667          * a small one for the message header and another one for the current
3668          * chunk of the packed buffer.
3669          * Check that the restrictions imposed by the SPI controller are
3670          * respected: the chunk buffer is smaller than the max transfer size,
3671          * and the total length of the chunk plus its message header is smaller
3672          * than the max message size.
3673          * We do that during probe time since the maximum transfer size is a
3674          * runtime invariant.
3675          */
3676         max_xfer = spi_max_transfer_size(spi);
3677         max_msg = spi_max_message_size(spi);
3678
3679         /* We need to send at least one 64-bit word of SPI payload per message
3680          * in order to be able to make useful progress.
3681          */
3682         if (max_msg < SJA1105_SIZE_SPI_MSG_HEADER + 8) {
3683                 dev_err(dev, "SPI master cannot send large enough buffers, aborting\n");
3684                 return -EINVAL;
3685         }
3686
3687         priv->max_xfer_len = SJA1105_SIZE_SPI_MSG_MAXLEN;
3688         if (priv->max_xfer_len > max_xfer)
3689                 priv->max_xfer_len = max_xfer;
3690         if (priv->max_xfer_len > max_msg - SJA1105_SIZE_SPI_MSG_HEADER)
3691                 priv->max_xfer_len = max_msg - SJA1105_SIZE_SPI_MSG_HEADER;
3692
3693         priv->info = of_device_get_match_data(dev);
3694
3695         /* Detect hardware device */
3696         rc = sja1105_check_device_id(priv);
3697         if (rc < 0) {
3698                 dev_err(dev, "Device ID check failed: %d\n", rc);
3699                 return rc;
3700         }
3701
3702         dev_info(dev, "Probed switch chip: %s\n", priv->info->name);
3703
3704         ds = devm_kzalloc(dev, sizeof(*ds), GFP_KERNEL);
3705         if (!ds)
3706                 return -ENOMEM;
3707
3708         ds->dev = dev;
3709         ds->num_ports = SJA1105_MAX_NUM_PORTS;
3710         ds->ops = &sja1105_switch_ops;
3711         ds->priv = priv;
3712         priv->ds = ds;
3713
3714         tagger_data = &priv->tagger_data;
3715
3716         mutex_init(&priv->ptp_data.lock);
3717         mutex_init(&priv->mgmt_lock);
3718
3719         priv->dsa_8021q_ctx = devm_kzalloc(dev, sizeof(*priv->dsa_8021q_ctx),
3720                                            GFP_KERNEL);
3721         if (!priv->dsa_8021q_ctx)
3722                 return -ENOMEM;
3723
3724         priv->dsa_8021q_ctx->ops = &sja1105_dsa_8021q_ops;
3725         priv->dsa_8021q_ctx->proto = htons(ETH_P_8021Q);
3726         priv->dsa_8021q_ctx->ds = ds;
3727
3728         INIT_LIST_HEAD(&priv->dsa_8021q_ctx->crosschip_links);
3729         INIT_LIST_HEAD(&priv->bridge_vlans);
3730         INIT_LIST_HEAD(&priv->dsa_8021q_vlans);
3731
3732         sja1105_tas_setup(ds);
3733         sja1105_flower_setup(ds);
3734
3735         rc = dsa_register_switch(priv->ds);
3736         if (rc)
3737                 return rc;
3738
3739         if (IS_ENABLED(CONFIG_NET_SCH_CBS)) {
3740                 priv->cbs = devm_kcalloc(dev, priv->info->num_cbs_shapers,
3741                                          sizeof(struct sja1105_cbs_entry),
3742                                          GFP_KERNEL);
3743                 if (!priv->cbs) {
3744                         rc = -ENOMEM;
3745                         goto out_unregister_switch;
3746                 }
3747         }
3748
3749         /* Connections between dsa_port and sja1105_port */
3750         for (port = 0; port < ds->num_ports; port++) {
3751                 struct sja1105_port *sp = &priv->ports[port];
3752                 struct dsa_port *dp = dsa_to_port(ds, port);
3753                 struct net_device *slave;
3754                 int subvlan;
3755
3756                 if (!dsa_is_user_port(ds, port))
3757                         continue;
3758
3759                 dp->priv = sp;
3760                 sp->dp = dp;
3761                 sp->data = tagger_data;
3762                 slave = dp->slave;
3763                 kthread_init_work(&sp->xmit_work, sja1105_port_deferred_xmit);
3764                 sp->xmit_worker = kthread_create_worker(0, "%s_xmit",
3765                                                         slave->name);
3766                 if (IS_ERR(sp->xmit_worker)) {
3767                         rc = PTR_ERR(sp->xmit_worker);
3768                         dev_err(ds->dev,
3769                                 "failed to create deferred xmit thread: %d\n",
3770                                 rc);
3771                         goto out_destroy_workers;
3772                 }
3773                 skb_queue_head_init(&sp->xmit_queue);
3774                 sp->xmit_tpid = ETH_P_SJA1105;
3775
3776                 for (subvlan = 0; subvlan < DSA_8021Q_N_SUBVLAN; subvlan++)
3777                         sp->subvlan_map[subvlan] = VLAN_N_VID;
3778         }
3779
3780         return 0;
3781
3782 out_destroy_workers:
3783         while (port-- > 0) {
3784                 struct sja1105_port *sp = &priv->ports[port];
3785
3786                 if (!dsa_is_user_port(ds, port))
3787                         continue;
3788
3789                 kthread_destroy_worker(sp->xmit_worker);
3790         }
3791
3792 out_unregister_switch:
3793         dsa_unregister_switch(ds);
3794
3795         return rc;
3796 }
3797
3798 static int sja1105_remove(struct spi_device *spi)
3799 {
3800         struct sja1105_private *priv = spi_get_drvdata(spi);
3801
3802         dsa_unregister_switch(priv->ds);
3803         return 0;
3804 }
3805
3806 static const struct of_device_id sja1105_dt_ids[] = {
3807         { .compatible = "nxp,sja1105e", .data = &sja1105e_info },
3808         { .compatible = "nxp,sja1105t", .data = &sja1105t_info },
3809         { .compatible = "nxp,sja1105p", .data = &sja1105p_info },
3810         { .compatible = "nxp,sja1105q", .data = &sja1105q_info },
3811         { .compatible = "nxp,sja1105r", .data = &sja1105r_info },
3812         { .compatible = "nxp,sja1105s", .data = &sja1105s_info },
3813         { /* sentinel */ },
3814 };
3815 MODULE_DEVICE_TABLE(of, sja1105_dt_ids);
3816
3817 static struct spi_driver sja1105_driver = {
3818         .driver = {
3819                 .name  = "sja1105",
3820                 .owner = THIS_MODULE,
3821                 .of_match_table = of_match_ptr(sja1105_dt_ids),
3822         },
3823         .probe  = sja1105_probe,
3824         .remove = sja1105_remove,
3825 };
3826
3827 module_spi_driver(sja1105_driver);
3828
3829 MODULE_AUTHOR("Vladimir Oltean <olteanv@gmail.com>");
3830 MODULE_AUTHOR("Georg Waibel <georg.waibel@sensor-technik.de>");
3831 MODULE_DESCRIPTION("SJA1105 Driver");
3832 MODULE_LICENSE("GPL v2");