Quelle  sja1105_main.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0
/* Copyright (c) 2018, Sensor-Technik Wiedemann GmbH
 * Copyright (c) 2018-2019, Vladimir Oltean <olteanv@gmail.com>
 */

#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt

#include <linux/delay.h>
#include <linux/module.h>
#include <linux/printk.h>
#include <linux/spi/spi.h>
#include <linux/errno.h>
#include <linux/gpio/consumer.h>
#include <linux/phylink.h>
#include <linux/of.h>
#include <linux/of_net.h>
#include <linux/of_mdio.h>
#include <linux/netdev_features.h>
#include <linux/netdevice.h>
#include <linux/if_bridge.h>
#include <linux/if_ether.h>
#include <linux/dsa/8021q.h>
#include <linux/units.h>

#include "sja1105.h"
#include "sja1105_tas.h"

#define SJA1105_UNKNOWN_MULTICAST 0x010000000000ull

/* Configure the optional reset pin and bring up switch */
static int sja1105_hw_reset(struct device *dev, unsigned int pulse_len,
       unsigned int startup_delay)
{
 struct gpio_desc *gpio;

 gpio = gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
 if (IS_ERR(gpio))
  return PTR_ERR(gpio);

 if (!gpio)
  return 0;

 gpiod_set_value_cansleep(gpio, 1);
 /* Wait for minimum reset pulse length */
 msleep(pulse_len);
 gpiod_set_value_cansleep(gpio, 0);
 /* Wait until chip is ready after reset */
 msleep(startup_delay);

 gpiod_put(gpio);

 return 0;
}

static void
sja1105_port_allow_traffic(struct sja1105_l2_forwarding_entry *l2_fwd,
      int from, int to, bool allow)
{
 if (allow)
  l2_fwd[from].reach_port |= BIT(to);
 else
  l2_fwd[from].reach_port &= ~BIT(to);
}

static bool sja1105_can_forward(struct sja1105_l2_forwarding_entry *l2_fwd,
    int from, int to)
{
 return !!(l2_fwd[from].reach_port & BIT(to));
}

static int sja1105_is_vlan_configured(struct sja1105_private *priv, u16 vid)
{
 struct sja1105_vlan_lookup_entry *vlan;
 int count, i;

 vlan = priv->static_config.tables[BLK_IDX_VLAN_LOOKUP].entries;
 count = priv->static_config.tables[BLK_IDX_VLAN_LOOKUP].entry_count;

 for (i = 0; i < count; i++)
  if (vlan[i].vlanid == vid)
   return i;

 /* Return an invalid entry index if not found */
 return -1;
}

static int sja1105_drop_untagged(struct dsa_switch *ds, int port, bool drop)
{
 struct sja1105_private *priv = ds->priv;
 struct sja1105_mac_config_entry *mac;

 mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;

 if (mac[port].drpuntag == drop)
  return 0;

 mac[port].drpuntag = drop;

 return sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
         &mac[port], true);
}

static int sja1105_pvid_apply(struct sja1105_private *priv, int port, u16 pvid)
{
 struct sja1105_mac_config_entry *mac;

 mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;

 if (mac[port].vlanid == pvid)
  return 0;

 mac[port].vlanid = pvid;

 return sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
         &mac[port], true);
}

static int sja1105_commit_pvid(struct dsa_switch *ds, int port)
{
 struct dsa_port *dp = dsa_to_port(ds, port);
 struct net_device *br = dsa_port_bridge_dev_get(dp);
 struct sja1105_private *priv = ds->priv;
 struct sja1105_vlan_lookup_entry *vlan;
 bool drop_untagged = false;
 int match, rc;
 u16 pvid;

 if (br && br_vlan_enabled(br))
  pvid = priv->bridge_pvid[port];
 else
  pvid = priv->tag_8021q_pvid[port];

 rc = sja1105_pvid_apply(priv, port, pvid);
 if (rc)
  return rc;

 /* Only force dropping of untagged packets when the port is under a
 * VLAN-aware bridge. When the tag_8021q pvid is used, we are
 * deliberately removing the RX VLAN from the port's VMEMB_PORT list,
 * to prevent DSA tag spoofing from the link partner. Untagged packets
 * are the only ones that should be received with tag_8021q, so
 * definitely don't drop them.
 */
 if (pvid == priv->bridge_pvid[port]) {
  vlan = priv->static_config.tables[BLK_IDX_VLAN_LOOKUP].entries;

  match = sja1105_is_vlan_configured(priv, pvid);

  if (match < 0 || !(vlan[match].vmemb_port & BIT(port)))
   drop_untagged = true;
 }

 if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port))
  drop_untagged = true;

 return sja1105_drop_untagged(ds, port, drop_untagged);
}

static int sja1105_init_mac_settings(struct sja1105_private *priv)
{
 struct sja1105_mac_config_entry default_mac = {
  /* Enable all 8 priority queues on egress.
 * Every queue i holds top[i] - base[i] frames.
 * Sum of top[i] - base[i] is 511 (max hardware limit).
 */
  .top  = {0x3F, 0x7F, 0xBF, 0xFF, 0x13F, 0x17F, 0x1BF, 0x1FF},
  .base = {0x0, 0x40, 0x80, 0xC0, 0x100, 0x140, 0x180, 0x1C0},
  .enabled = {true, true, true, true, true, true, true, true},
  /* Keep standard IFG of 12 bytes on egress. */
  .ifg = 0,
  /* Always put the MAC speed in automatic mode, where it can be
 * adjusted at runtime by PHYLINK.
 */
  .speed = priv->info->port_speed[SJA1105_SPEED_AUTO],
  /* No static correction for 1-step 1588 events */
  .tp_delin = 0,
  .tp_delout = 0,
  /* Disable aging for critical TTEthernet traffic */
  .maxage = 0xFF,
  /* Internal VLAN (pvid) to apply to untagged ingress */
  .vlanprio = 0,
  .vlanid = 1,
  .ing_mirr = false,
  .egr_mirr = false,
  /* Don't drop traffic with other EtherType than ETH_P_IP */
  .drpnona664 = false,
  /* Don't drop double-tagged traffic */
  .drpdtag = false,
  /* Don't drop untagged traffic */
  .drpuntag = false,
  /* Don't retag 802.1p (VID 0) traffic with the pvid */
  .retag = false,
  /* Disable learning and I/O on user ports by default -
 * STP will enable it.
 */
  .dyn_learn = false,
  .egress = false,
  .ingress = false,
 };
 struct sja1105_mac_config_entry *mac;
 struct dsa_switch *ds = priv->ds;
 struct sja1105_table *table;
 struct dsa_port *dp;

 table = &priv->static_config.tables[BLK_IDX_MAC_CONFIG];

 /* Discard previous MAC Configuration Table */
 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 mac = table->entries;

 list_for_each_entry(dp, &ds->dst->ports, list) {
  if (dp->ds != ds)
   continue;

  mac[dp->index] = default_mac;

  /* Let sja1105_bridge_stp_state_set() keep address learning
 * enabled for the DSA ports. CPU ports use software-assisted
 * learning to ensure that only FDB entries belonging to the
 * bridge are learned, and that they are learned towards all
 * CPU ports in a cross-chip topology if multiple CPU ports
 * exist.
 */
  if (dsa_port_is_dsa(dp))
   dp->learning = true;

  /* Disallow untagged packets from being received on the
 * CPU and DSA ports.
 */
  if (dsa_port_is_cpu(dp) || dsa_port_is_dsa(dp))
   mac[dp->index].drpuntag = true;
 }

 return 0;
}

static int sja1105_init_mii_settings(struct sja1105_private *priv)
{
 struct device *dev = &priv->spidev->dev;
 struct sja1105_xmii_params_entry *mii;
 struct dsa_switch *ds = priv->ds;
 struct sja1105_table *table;
 int i;

 table = &priv->static_config.tables[BLK_IDX_XMII_PARAMS];

 /* Discard previous xMII Mode Parameters Table */
 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 /* Override table based on PHYLINK DT bindings */
 table->entry_count = table->ops->max_entry_count;

 mii = table->entries;

 for (i = 0; i < ds->num_ports; i++) {
  sja1105_mii_role_t role = XMII_MAC;

  if (dsa_is_unused_port(priv->ds, i))
   continue;

  switch (priv->phy_mode[i]) {
  case PHY_INTERFACE_MODE_INTERNAL:
   if (priv->info->internal_phy[i] == SJA1105_NO_PHY)
    goto unsupported;

   mii->xmii_mode[i] = XMII_MODE_MII;
   if (priv->info->internal_phy[i] == SJA1105_PHY_BASE_TX)
    mii->special[i] = true;

   break;
  case PHY_INTERFACE_MODE_REVMII:
   role = XMII_PHY;
   fallthrough;
  case PHY_INTERFACE_MODE_MII:
   if (!priv->info->supports_mii[i])
    goto unsupported;

   mii->xmii_mode[i] = XMII_MODE_MII;
   break;
  case PHY_INTERFACE_MODE_REVRMII:
   role = XMII_PHY;
   fallthrough;
  case PHY_INTERFACE_MODE_RMII:
   if (!priv->info->supports_rmii[i])
    goto unsupported;

   mii->xmii_mode[i] = XMII_MODE_RMII;
   break;
  case PHY_INTERFACE_MODE_RGMII:
  case PHY_INTERFACE_MODE_RGMII_ID:
  case PHY_INTERFACE_MODE_RGMII_RXID:
  case PHY_INTERFACE_MODE_RGMII_TXID:
   if (!priv->info->supports_rgmii[i])
    goto unsupported;

   mii->xmii_mode[i] = XMII_MODE_RGMII;
   break;
  case PHY_INTERFACE_MODE_SGMII:
   if (!priv->info->supports_sgmii[i])
    goto unsupported;

   mii->xmii_mode[i] = XMII_MODE_SGMII;
   mii->special[i] = true;
   break;
  case PHY_INTERFACE_MODE_2500BASEX:
   if (!priv->info->supports_2500basex[i])
    goto unsupported;

   mii->xmii_mode[i] = XMII_MODE_SGMII;
   mii->special[i] = true;
   break;
unsupported:
  default:
   dev_err(dev, "Unsupported PHY mode %s on port %d!\n",
    phy_modes(priv->phy_mode[i]), i);
   return -EINVAL;
  }

  mii->phy_mac[i] = role;
 }
 return 0;
}

static int sja1105_init_static_fdb(struct sja1105_private *priv)
{
 struct sja1105_l2_lookup_entry *l2_lookup;
 struct sja1105_table *table;
 int port;

 table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];

 /* We only populate the FDB table through dynamic L2 Address Lookup
 * entries, except for a special entry at the end which is a catch-all
 * for unknown multicast and will be used to control flooding domain.
 */
 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 if (!priv->info->can_limit_mcast_flood)
  return 0;

 table->entries = kcalloc(1, table->ops->unpacked_entry_size,
     GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = 1;
 l2_lookup = table->entries;

 /* All L2 multicast addresses have an odd first octet */
 l2_lookup[0].macaddr = SJA1105_UNKNOWN_MULTICAST;
 l2_lookup[0].mask_macaddr = SJA1105_UNKNOWN_MULTICAST;
 l2_lookup[0].lockeds = true;
 l2_lookup[0].index = SJA1105_MAX_L2_LOOKUP_COUNT - 1;

 /* Flood multicast to every port by default */
 for (port = 0; port < priv->ds->num_ports; port++)
  if (!dsa_is_unused_port(priv->ds, port))
   l2_lookup[0].destports |= BIT(port);

 return 0;
}

static int sja1105_init_l2_lookup_params(struct sja1105_private *priv)
{
 struct sja1105_l2_lookup_params_entry default_l2_lookup_params = {
  /* Learned FDB entries are forgotten after 300 seconds */
  .maxage = SJA1105_AGEING_TIME_MS(300000),
  /* All entries within a FDB bin are available for learning */
  .dyn_tbsz = SJA1105ET_FDB_BIN_SIZE,
  /* And the P/Q/R/S equivalent setting: */
  .start_dynspc = 0,
  /* 2^8 + 2^5 + 2^3 + 2^2 + 2^1 + 1 in Koopman notation */
  .poly = 0x97,
  /* Always use Independent VLAN Learning (IVL) */
  .shared_learn = false,
  /* Don't discard management traffic based on ENFPORT -
 * we don't perform SMAC port enforcement anyway, so
 * what we are setting here doesn't matter.
 */
  .no_enf_hostprt = false,
  /* Don't learn SMAC for mac_fltres1 and mac_fltres0.
 * Maybe correlate with no_linklocal_learn from bridge driver?
 */
  .no_mgmt_learn = true,
  /* P/Q/R/S only */
  .use_static = true,
  /* Dynamically learned FDB entries can overwrite other (older)
 * dynamic FDB entries
 */
  .owr_dyn = true,
  .drpnolearn = true,
 };
 struct dsa_switch *ds = priv->ds;
 int port, num_used_ports = 0;
 struct sja1105_table *table;
 u64 max_fdb_entries;

 for (port = 0; port < ds->num_ports; port++)
  if (!dsa_is_unused_port(ds, port))
   num_used_ports++;

 max_fdb_entries = SJA1105_MAX_L2_LOOKUP_COUNT / num_used_ports;

 for (port = 0; port < ds->num_ports; port++) {
  if (dsa_is_unused_port(ds, port))
   continue;

  default_l2_lookup_params.maxaddrp[port] = max_fdb_entries;
 }

 table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP_PARAMS];

 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 /* This table only has a single entry */
 ((struct sja1105_l2_lookup_params_entry *)table->entries)[0] =
    default_l2_lookup_params;

 return 0;
}

/* Set up a default VLAN for untagged traffic injected from the CPU
 * using management routes (e.g. STP, PTP) as opposed to tag_8021q.
 * All DT-defined ports are members of this VLAN, and there are no
 * restrictions on forwarding (since the CPU selects the destination).
 * Frames from this VLAN will always be transmitted as untagged, and
 * neither the bridge nor the 8021q module cannot create this VLAN ID.
 */
static int sja1105_init_static_vlan(struct sja1105_private *priv)
{
 struct sja1105_table *table;
 struct sja1105_vlan_lookup_entry pvid = {
  .type_entry = SJA1110_VLAN_D_TAG,
  .ving_mirr = 0,
  .vegr_mirr = 0,
  .vmemb_port = 0,
  .vlan_bc = 0,
  .tag_port = 0,
  .vlanid = SJA1105_DEFAULT_VLAN,
 };
 struct dsa_switch *ds = priv->ds;
 int port;

 table = &priv->static_config.tables[BLK_IDX_VLAN_LOOKUP];

 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kzalloc(table->ops->unpacked_entry_size,
     GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = 1;

 for (port = 0; port < ds->num_ports; port++) {
  if (dsa_is_unused_port(ds, port))
   continue;

  pvid.vmemb_port |= BIT(port);
  pvid.vlan_bc |= BIT(port);
  pvid.tag_port &= ~BIT(port);

  if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
   priv->tag_8021q_pvid[port] = SJA1105_DEFAULT_VLAN;
   priv->bridge_pvid[port] = SJA1105_DEFAULT_VLAN;
  }
 }

 ((struct sja1105_vlan_lookup_entry *)table->entries)[0] = pvid;
 return 0;
}

static int sja1105_init_l2_forwarding(struct sja1105_private *priv)
{
 struct sja1105_l2_forwarding_entry *l2fwd;
 struct dsa_switch *ds = priv->ds;
 struct dsa_switch_tree *dst;
 struct sja1105_table *table;
 struct dsa_link *dl;
 int port, tc;
 int from, to;

 table = &priv->static_config.tables[BLK_IDX_L2_FORWARDING];

 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 l2fwd = table->entries;

 /* First 5 entries in the L2 Forwarding Table define the forwarding
 * rules and the VLAN PCP to ingress queue mapping.
 * Set up the ingress queue mapping first.
 */
 for (port = 0; port < ds->num_ports; port++) {
  if (dsa_is_unused_port(ds, port))
   continue;

  for (tc = 0; tc < SJA1105_NUM_TC; tc++)
   l2fwd[port].vlan_pmap[tc] = tc;
 }

 /* Then manage the forwarding domain for user ports. These can forward
 * only to the always-on domain (CPU port and DSA links)
 */
 for (from = 0; from < ds->num_ports; from++) {
  if (!dsa_is_user_port(ds, from))
   continue;

  for (to = 0; to < ds->num_ports; to++) {
   if (!dsa_is_cpu_port(ds, to) &&
       !dsa_is_dsa_port(ds, to))
    continue;

   l2fwd[from].bc_domain |= BIT(to);
   l2fwd[from].fl_domain |= BIT(to);

   sja1105_port_allow_traffic(l2fwd, from, to, true);
  }
 }

 /* Then manage the forwarding domain for DSA links and CPU ports (the
 * always-on domain). These can send packets to any enabled port except
 * themselves.
 */
 for (from = 0; from < ds->num_ports; from++) {
  if (!dsa_is_cpu_port(ds, from) && !dsa_is_dsa_port(ds, from))
   continue;

  for (to = 0; to < ds->num_ports; to++) {
   if (dsa_is_unused_port(ds, to))
    continue;

   if (from == to)
    continue;

   l2fwd[from].bc_domain |= BIT(to);
   l2fwd[from].fl_domain |= BIT(to);

   sja1105_port_allow_traffic(l2fwd, from, to, true);
  }
 }

 /* In odd topologies ("H" connections where there is a DSA link to
 * another switch which also has its own CPU port), TX packets can loop
 * back into the system (they are flooded from CPU port 1 to the DSA
 * link, and from there to CPU port 2). Prevent this from happening by
 * cutting RX from DSA links towards our CPU port, if the remote switch
 * has its own CPU port and therefore doesn't need ours for network
 * stack termination.
 */
 dst = ds->dst;

 list_for_each_entry(dl, &dst->rtable, list) {
  if (dl->dp->ds != ds || dl->link_dp->cpu_dp == dl->dp->cpu_dp)
   continue;

  from = dl->dp->index;
  to = dsa_upstream_port(ds, from);

  dev_warn(ds->dev,
    "H topology detected, cutting RX from DSA link %d to CPU port %d to prevent TX packet loops\n",
    from, to);

  sja1105_port_allow_traffic(l2fwd, from, to, false);

  l2fwd[from].bc_domain &= ~BIT(to);
  l2fwd[from].fl_domain &= ~BIT(to);
 }

 /* Finally, manage the egress flooding domain. All ports start up with
 * flooding enabled, including the CPU port and DSA links.
 */
 for (port = 0; port < ds->num_ports; port++) {
  if (dsa_is_unused_port(ds, port))
   continue;

  priv->ucast_egress_floods |= BIT(port);
  priv->bcast_egress_floods |= BIT(port);
 }

 /* Next 8 entries define VLAN PCP mapping from ingress to egress.
 * Create a one-to-one mapping.
 */
 for (tc = 0; tc < SJA1105_NUM_TC; tc++) {
  for (port = 0; port < ds->num_ports; port++) {
   if (dsa_is_unused_port(ds, port))
    continue;

   l2fwd[ds->num_ports + tc].vlan_pmap[port] = tc;
  }

  l2fwd[ds->num_ports + tc].type_egrpcp2outputq = true;
 }

 return 0;
}

static int sja1110_init_pcp_remapping(struct sja1105_private *priv)
{
 struct sja1110_pcp_remapping_entry *pcp_remap;
 struct dsa_switch *ds = priv->ds;
 struct sja1105_table *table;
 int port, tc;

 table = &priv->static_config.tables[BLK_IDX_PCP_REMAPPING];

 /* Nothing to do for SJA1105 */
 if (!table->ops->max_entry_count)
  return 0;

 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 pcp_remap = table->entries;

 /* Repeat the configuration done for vlan_pmap */
 for (port = 0; port < ds->num_ports; port++) {
  if (dsa_is_unused_port(ds, port))
   continue;

  for (tc = 0; tc < SJA1105_NUM_TC; tc++)
   pcp_remap[port].egrpcp[tc] = tc;
 }

 return 0;
}

static int sja1105_init_l2_forwarding_params(struct sja1105_private *priv)
{
 struct sja1105_l2_forwarding_params_entry *l2fwd_params;
 struct sja1105_table *table;

 table = &priv->static_config.tables[BLK_IDX_L2_FORWARDING_PARAMS];

 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 /* This table only has a single entry */
 l2fwd_params = table->entries;

 /* Disallow dynamic reconfiguration of vlan_pmap */
 l2fwd_params->max_dynp = 0;
 /* Use a single memory partition for all ingress queues */
 l2fwd_params->part_spc[0] = priv->info->max_frame_mem;

 return 0;
}

void sja1105_frame_memory_partitioning(struct sja1105_private *priv)
{
 struct sja1105_l2_forwarding_params_entry *l2_fwd_params;
 struct sja1105_vl_forwarding_params_entry *vl_fwd_params;
 struct sja1105_table *table;

 table = &priv->static_config.tables[BLK_IDX_L2_FORWARDING_PARAMS];
 l2_fwd_params = table->entries;
 l2_fwd_params->part_spc[0] = SJA1105_MAX_FRAME_MEMORY;

 /* If we have any critical-traffic virtual links, we need to reserve
 * some frame buffer memory for them. At the moment, hardcode the value
 * at 100 blocks of 128 bytes of memory each. This leaves 829 blocks
 * remaining for best-effort traffic. TODO: figure out a more flexible
 * way to perform the frame buffer partitioning.
 */
 if (!priv->static_config.tables[BLK_IDX_VL_FORWARDING].entry_count)
  return;

 table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING_PARAMS];
 vl_fwd_params = table->entries;

 l2_fwd_params->part_spc[0] -= SJA1105_VL_FRAME_MEMORY;
 vl_fwd_params->partspc[0] = SJA1105_VL_FRAME_MEMORY;
}

/* SJA1110 TDMACONFIGIDX values:
 *
 *      | 100 Mbps ports |  1Gbps ports  | 2.5Gbps ports | Disabled ports
 * -----+----------------+---------------+---------------+---------------
 *   0  |   0, [5:10]    |     [1:2]     |     [3:4]     |     retag
 *   1  |0, [5:10], retag|     [1:2]     |     [3:4]     |       -
 *   2  |   0, [5:10]    |  [1:3], retag |       4       |       -
 *   3  |   0, [5:10]    |[1:2], 4, retag|       3       |       -
 *   4  |  0, 2, [5:10]  |    1, retag   |     [3:4]     |       -
 *   5  |  0, 1, [5:10]  |    2, retag   |     [3:4]     |       -
 *  14  |   0, [5:10]    | [1:4], retag  |       -       |       -
 *  15  |     [5:10]     | [0:4], retag  |       -       |       -
 */
static void sja1110_select_tdmaconfigidx(struct sja1105_private *priv)
{
 struct sja1105_general_params_entry *general_params;
 struct sja1105_table *table;
 bool port_1_is_base_tx;
 bool port_3_is_2500;
 bool port_4_is_2500;
 u64 tdmaconfigidx;

 if (priv->info->device_id != SJA1110_DEVICE_ID)
  return;

 table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
 general_params = table->entries;

 /* All the settings below are "as opposed to SGMII", which is the
 * other pinmuxing option.
 */
 port_1_is_base_tx = priv->phy_mode[1] == PHY_INTERFACE_MODE_INTERNAL;
 port_3_is_2500 = priv->phy_mode[3] == PHY_INTERFACE_MODE_2500BASEX;
 port_4_is_2500 = priv->phy_mode[4] == PHY_INTERFACE_MODE_2500BASEX;

 if (port_1_is_base_tx)
  /* Retagging port will operate at 1 Gbps */
  tdmaconfigidx = 5;
 else if (port_3_is_2500 && port_4_is_2500)
  /* Retagging port will operate at 100 Mbps */
  tdmaconfigidx = 1;
 else if (port_3_is_2500)
  /* Retagging port will operate at 1 Gbps */
  tdmaconfigidx = 3;
 else if (port_4_is_2500)
  /* Retagging port will operate at 1 Gbps */
  tdmaconfigidx = 2;
 else
  /* Retagging port will operate at 1 Gbps */
  tdmaconfigidx = 14;

 general_params->tdmaconfigidx = tdmaconfigidx;
}

static int sja1105_init_topology(struct sja1105_private *priv,
     struct sja1105_general_params_entry *general_params)
{
 struct dsa_switch *ds = priv->ds;
 int port;

 /* The host port is the destination for traffic matching mac_fltres1
 * and mac_fltres0 on all ports except itself. Default to an invalid
 * value.
 */
 general_params->host_port = ds->num_ports;

 /* Link-local traffic received on casc_port will be forwarded
 * to host_port without embedding the source port and device ID
 * info in the destination MAC address, and no RX timestamps will be
 * taken either (presumably because it is a cascaded port and a
 * downstream SJA switch already did that).
 * To disable the feature, we need to do different things depending on
 * switch generation. On SJA1105 we need to set an invalid port, while
 * on SJA1110 which support multiple cascaded ports, this field is a
 * bitmask so it must be left zero.
 */
 if (!priv->info->multiple_cascade_ports)
  general_params->casc_port = ds->num_ports;

 for (port = 0; port < ds->num_ports; port++) {
  bool is_upstream = dsa_is_upstream_port(ds, port);
  bool is_dsa_link = dsa_is_dsa_port(ds, port);

  /* Upstream ports can be dedicated CPU ports or
 * upstream-facing DSA links
 */
  if (is_upstream) {
   if (general_params->host_port == ds->num_ports) {
    general_params->host_port = port;
   } else {
    dev_err(ds->dev,
     "Port %llu is already a host port, configuring %d as one too is not supported\n",
     general_params->host_port, port);
    return -EINVAL;
   }
  }

  /* Cascade ports are downstream-facing DSA links */
  if (is_dsa_link && !is_upstream) {
   if (priv->info->multiple_cascade_ports) {
    general_params->casc_port |= BIT(port);
   } else if (general_params->casc_port == ds->num_ports) {
    general_params->casc_port = port;
   } else {
    dev_err(ds->dev,
     "Port %llu is already a cascade port, configuring %d as one too is not supported\n",
     general_params->casc_port, port);
    return -EINVAL;
   }
  }
 }

 if (general_params->host_port == ds->num_ports) {
  dev_err(ds->dev, "No host port configured\n");
  return -EINVAL;
 }

 return 0;
}

static int sja1105_init_general_params(struct sja1105_private *priv)
{
 struct sja1105_general_params_entry default_general_params = {
  /* Allow dynamic changing of the mirror port */
  .mirr_ptacu = true,
  .switchid = priv->ds->index,
  /* Priority queue for link-local management frames
 * (both ingress to and egress from CPU - PTP, STP etc)
 */
  .hostprio = 7,
  .mac_fltres1 = SJA1105_LINKLOCAL_FILTER_A,
  .mac_flt1    = SJA1105_LINKLOCAL_FILTER_A_MASK,
  .incl_srcpt1 = true,
  .send_meta1  = true,
  .mac_fltres0 = SJA1105_LINKLOCAL_FILTER_B,
  .mac_flt0    = SJA1105_LINKLOCAL_FILTER_B_MASK,
  .incl_srcpt0 = true,
  .send_meta0  = true,
  /* Default to an invalid value */
  .mirr_port = priv->ds->num_ports,
  /* No TTEthernet */
  .vllupformat = SJA1105_VL_FORMAT_PSFP,
  .vlmarker = 0,
  .vlmask = 0,
  /* Only update correctionField for 1-step PTP (L2 transport) */
  .ignore2stf = 0,
  /* Forcefully disable VLAN filtering by telling
 * the switch that VLAN has a different EtherType.
 */
  .tpid = ETH_P_SJA1105,
  .tpid2 = ETH_P_SJA1105,
  /* Enable the TTEthernet engine on SJA1110 */
  .tte_en = true,
  /* Set up the EtherType for control packets on SJA1110 */
  .header_type = ETH_P_SJA1110,
 };
 struct sja1105_general_params_entry *general_params;
 struct sja1105_table *table;
 int rc;

 rc = sja1105_init_topology(priv, &default_general_params);
 if (rc)
  return rc;

 table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];

 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 general_params = table->entries;

 /* This table only has a single entry */
 general_params[0] = default_general_params;

 sja1110_select_tdmaconfigidx(priv);

 return 0;
}

static int sja1105_init_avb_params(struct sja1105_private *priv)
{
 struct sja1105_avb_params_entry *avb;
 struct sja1105_table *table;

 table = &priv->static_config.tables[BLK_IDX_AVB_PARAMS];

 /* Discard previous AVB Parameters Table */
 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 avb = table->entries;

 /* Configure the MAC addresses for meta frames */
 avb->destmeta = SJA1105_META_DMAC;
 avb->srcmeta  = SJA1105_META_SMAC;
 /* On P/Q/R/S, configure the direction of the PTP_CLK pin as input by
 * default. This is because there might be boards with a hardware
 * layout where enabling the pin as output might cause an electrical
 * clash. On E/T the pin is always an output, which the board designers
 * probably already knew, so even if there are going to be electrical
 * issues, there's nothing we can do.
 */
 avb->cas_master = false;

 return 0;
}

/* The L2 policing table is 2-stage. The table is looked up for each frame
 * according to the ingress port, whether it was broadcast or not, and the
 * classified traffic class (given by VLAN PCP). This portion of the lookup is
 * fixed, and gives access to the SHARINDX, an indirection register pointing
 * within the policing table itself, which is used to resolve the policer that
 * will be used for this frame.
 *
 *  Stage 1                              Stage 2
 * +------------+--------+              +---------------------------------+
 * |Port 0 TC 0 |SHARINDX|              | Policer 0: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 * |Port 0 TC 1 |SHARINDX|              | Policer 1: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 *    ...                               | Policer 2: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 * |Port 0 TC 7 |SHARINDX|              | Policer 3: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 * |Port 1 TC 0 |SHARINDX|              | Policer 4: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 *    ...                               | Policer 5: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 * |Port 1 TC 7 |SHARINDX|              | Policer 6: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 *    ...                               | Policer 7: Rate, Burst, MTU     |
 * +------------+--------+              +---------------------------------+
 * |Port 4 TC 7 |SHARINDX|                 ...
 * +------------+--------+
 * |Port 0 BCAST|SHARINDX|                 ...
 * +------------+--------+
 * |Port 1 BCAST|SHARINDX|                 ...
 * +------------+--------+
 *    ...                                  ...
 * +------------+--------+              +---------------------------------+
 * |Port 4 BCAST|SHARINDX|              | Policer 44: Rate, Burst, MTU    |
 * +------------+--------+              +---------------------------------+
 *
 * In this driver, we shall use policers 0-4 as statically alocated port
 * (matchall) policers. So we need to make the SHARINDX for all lookups
 * corresponding to this ingress port (8 VLAN PCP lookups and 1 broadcast
 * lookup) equal.
 * The remaining policers (40) shall be dynamically allocated for flower
 * policers, where the key is either vlan_prio or dst_mac ff:ff:ff:ff:ff:ff.
 */
#define SJA1105_RATE_MBPS(speed) (((speed) * 64000) / 1000)

static int sja1105_init_l2_policing(struct sja1105_private *priv)
{
 struct sja1105_l2_policing_entry *policing;
 struct dsa_switch *ds = priv->ds;
 struct sja1105_table *table;
 int port, tc;

 table = &priv->static_config.tables[BLK_IDX_L2_POLICING];

 /* Discard previous L2 Policing Table */
 if (table->entry_count) {
  kfree(table->entries);
  table->entry_count = 0;
 }

 table->entries = kcalloc(table->ops->max_entry_count,
     table->ops->unpacked_entry_size, GFP_KERNEL);
 if (!table->entries)
  return -ENOMEM;

 table->entry_count = table->ops->max_entry_count;

 policing = table->entries;

 /* Setup shared indices for the matchall policers */
 for (port = 0; port < ds->num_ports; port++) {
  int mcast = (ds->num_ports * (SJA1105_NUM_TC + 1)) + port;
  int bcast = (ds->num_ports * SJA1105_NUM_TC) + port;

  for (tc = 0; tc < SJA1105_NUM_TC; tc++)
   policing[port * SJA1105_NUM_TC + tc].sharindx = port;

  policing[bcast].sharindx = port;
  /* Only SJA1110 has multicast policers */
  if (mcast < table->ops->max_entry_count)
   policing[mcast].sharindx = port;
 }

 /* Setup the matchall policer parameters */
 for (port = 0; port < ds->num_ports; port++) {
  int mtu = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;

  if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port))
   mtu += VLAN_HLEN;

  policing[port].smax = 65535; /* Burst size in bytes */
  policing[port].rate = SJA1105_RATE_MBPS(1000);
  policing[port].maxlen = mtu;
  policing[port].partition = 0;
 }

 return 0;
}

static int sja1105_static_config_load(struct sja1105_private *priv)
{
 int rc;

 sja1105_static_config_free(&priv->static_config);
 rc = sja1105_static_config_init(&priv->static_config,
     priv->info->static_ops,
     priv->info->device_id);
 if (rc)
  return rc;

 /* Build static configuration */
 rc = sja1105_init_mac_settings(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_mii_settings(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_static_fdb(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_static_vlan(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_l2_lookup_params(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_l2_forwarding(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_l2_forwarding_params(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_l2_policing(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_general_params(priv);
 if (rc < 0)
  return rc;
 rc = sja1105_init_avb_params(priv);
 if (rc < 0)
  return rc;
 rc = sja1110_init_pcp_remapping(priv);
 if (rc < 0)
  return rc;

 /* Send initial configuration to hardware via SPI */
 return sja1105_static_config_upload(priv);
}

/* This is the "new way" for a MAC driver to configure its RGMII delay lines,
 * based on the explicit "rx-internal-delay-ps" and "tx-internal-delay-ps"
 * properties. It has the advantage of working with fixed links and with PHYs
 * that apply RGMII delays too, and the MAC driver needs not perform any
 * special checks.
 *
 * Previously we were acting upon the "phy-mode" property when we were
 * operating in fixed-link, basically acting as a PHY, but with a reversed
 * interpretation: PHY_INTERFACE_MODE_RGMII_TXID means that the MAC should
 * behave as if it is connected to a PHY which has applied RGMII delays in the
 * TX direction. So if anything, RX delays should have been added by the MAC,
 * but we were adding TX delays.
 *
 * If the "{rx,tx}-internal-delay-ps" properties are not specified, we fall
 * back to the legacy behavior and apply delays on fixed-link ports based on
 * the reverse interpretation of the phy-mode. This is a deviation from the
 * expected default behavior which is to simply apply no delays. To achieve
 * that behavior with the new bindings, it is mandatory to specify
 * "{rx,tx}-internal-delay-ps" with a value of 0.
 */
static int sja1105_parse_rgmii_delays(struct sja1105_private *priv, int port,
          struct device_node *port_dn)
{
 phy_interface_t phy_mode = priv->phy_mode[port];
 struct device *dev = &priv->spidev->dev;
 int rx_delay = -1, tx_delay = -1;

 if (!phy_interface_mode_is_rgmii(phy_mode))
  return 0;

 of_property_read_u32(port_dn, "rx-internal-delay-ps", &rx_delay);
 of_property_read_u32(port_dn, "tx-internal-delay-ps", &tx_delay);

 if (rx_delay == -1 && tx_delay == -1 && priv->fixed_link[port]) {
  dev_warn(dev,
    "Port %d interpreting RGMII delay settings based on \"phy-mode\" property, "
    "please update device tree to specify \"rx-internal-delay-ps\" and "
    "\"tx-internal-delay-ps\"",
    port);

  if (phy_mode == PHY_INTERFACE_MODE_RGMII_RXID ||
      phy_mode == PHY_INTERFACE_MODE_RGMII_ID)
   rx_delay = 2000;

  if (phy_mode == PHY_INTERFACE_MODE_RGMII_TXID ||
      phy_mode == PHY_INTERFACE_MODE_RGMII_ID)
   tx_delay = 2000;
 }

 if (rx_delay < 0)
  rx_delay = 0;
 if (tx_delay < 0)
  tx_delay = 0;

 if ((rx_delay || tx_delay) && !priv->info->setup_rgmii_delay) {
  dev_err(dev, "Chip cannot apply RGMII delays\n");
  return -EINVAL;
 }

 if ((rx_delay && rx_delay < SJA1105_RGMII_DELAY_MIN_PS) ||
     (tx_delay && tx_delay < SJA1105_RGMII_DELAY_MIN_PS) ||
     (rx_delay > SJA1105_RGMII_DELAY_MAX_PS) ||
     (tx_delay > SJA1105_RGMII_DELAY_MAX_PS)) {
  dev_err(dev,
   "port %d RGMII delay values out of range, must be between %d and %d ps\n",
   port, SJA1105_RGMII_DELAY_MIN_PS, SJA1105_RGMII_DELAY_MAX_PS);
  return -ERANGE;
 }

 priv->rgmii_rx_delay_ps[port] = rx_delay;
 priv->rgmii_tx_delay_ps[port] = tx_delay;

 return 0;
}

static int sja1105_parse_ports_node(struct sja1105_private *priv,
        struct device_node *ports_node)
{
 struct device *dev = &priv->spidev->dev;

 for_each_available_child_of_node_scoped(ports_node, child) {
  struct device_node *phy_node;
  phy_interface_t phy_mode;
  u32 index;
  int err;

  /* Get switch port number from DT */
  if (of_property_read_u32(child, "reg", &index) < 0) {
   dev_err(dev, "Port number not defined in device tree "
    "(property \"reg\")\n");
   return -ENODEV;
  }

  /* Get PHY mode from DT */
  err = of_get_phy_mode(child, &phy_mode);
  if (err) {
   dev_err(dev, "Failed to read phy-mode or "
    "phy-interface-type property for port %d\n",
    index);
   return -ENODEV;
  }

  phy_node = of_parse_phandle(child, "phy-handle", 0);
  if (!phy_node) {
   if (!of_phy_is_fixed_link(child)) {
    dev_err(dev, "phy-handle or fixed-link "
     "properties missing!\n");
    return -ENODEV;
   }
   /* phy-handle is missing, but fixed-link isn't.
 * So it's a fixed link. Default to PHY role.
 */
   priv->fixed_link[index] = true;
  } else {
   of_node_put(phy_node);
  }

  priv->phy_mode[index] = phy_mode;

  err = sja1105_parse_rgmii_delays(priv, index, child);
  if (err)
   return err;
 }

 return 0;
}

static int sja1105_parse_dt(struct sja1105_private *priv)
{
 struct device *dev = &priv->spidev->dev;
 struct device_node *switch_node = dev->of_node;
 struct device_node *ports_node;
 int rc;

 ports_node = of_get_child_by_name(switch_node, "ports");
 if (!ports_node)
  ports_node = of_get_child_by_name(switch_node, "ethernet-ports");
 if (!ports_node) {
  dev_err(dev, "Incorrect bindings: absent \"ports\" node\n");
  return -ENODEV;
 }

 rc = sja1105_parse_ports_node(priv, ports_node);
 of_node_put(ports_node);

 return rc;
}

static int sja1105_set_port_speed(struct sja1105_private *priv, int port,
      int speed_mbps)
{
 struct sja1105_mac_config_entry *mac;
 u64 speed;

 /* On P/Q/R/S, one can read from the device via the MAC reconfiguration
 * tables. On E/T, MAC reconfig tables are not readable, only writable.
 * We have to *know* what the MAC looks like.  For the sake of keeping
 * the code common, we'll use the static configuration tables as a
 * reasonable approximation for both E/T and P/Q/R/S.
 */
 mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;

 switch (speed_mbps) {
 case SPEED_UNKNOWN:
  /* PHYLINK called sja1105_mac_config() to inform us about
 * the state->interface, but AN has not completed and the
 * speed is not yet valid. UM10944.pdf says that setting
 * SJA1105_SPEED_AUTO at runtime disables the port, so that is
 * ok for power consumption in case AN will never complete -
 * otherwise PHYLINK should come back with a new update.
 */
  speed = priv->info->port_speed[SJA1105_SPEED_AUTO];
  break;
 case SPEED_10:
  speed = priv->info->port_speed[SJA1105_SPEED_10MBPS];
  break;
 case SPEED_100:
  speed = priv->info->port_speed[SJA1105_SPEED_100MBPS];
  break;
 case SPEED_1000:
  speed = priv->info->port_speed[SJA1105_SPEED_1000MBPS];
  break;
 case SPEED_2500:
  speed = priv->info->port_speed[SJA1105_SPEED_2500MBPS];
  break;
 default:
  dev_err(priv->ds->dev, "Invalid speed %iMbps\n", speed_mbps);
  return -EINVAL;
 }

 /* Overwrite SJA1105_SPEED_AUTO from the static MAC configuration
 * table, since this will be used for the clocking setup, and we no
 * longer need to store it in the static config (already told hardware
 * we want auto during upload phase).
 * Actually for the SGMII port, the MAC is fixed at 1 Gbps and
 * we need to configure the PCS only (if even that).
 */
 if (priv->phy_mode[port] == PHY_INTERFACE_MODE_SGMII)
  speed = priv->info->port_speed[SJA1105_SPEED_1000MBPS];
 else if (priv->phy_mode[port] == PHY_INTERFACE_MODE_2500BASEX)
  speed = priv->info->port_speed[SJA1105_SPEED_2500MBPS];

 mac[port].speed = speed;

 return 0;
}

/* Write the MAC Configuration Table entry and, if necessary, the CGU settings,
 * after a link speedchange for this port.
 */
static int sja1105_set_port_config(struct sja1105_private *priv, int port)
{
 struct sja1105_mac_config_entry *mac;
 struct device *dev = priv->ds->dev;
 int rc;

 /* On P/Q/R/S, one can read from the device via the MAC reconfiguration
 * tables. On E/T, MAC reconfig tables are not readable, only writable.
 * We have to *know* what the MAC looks like.  For the sake of keeping
 * the code common, we'll use the static configuration tables as a
 * reasonable approximation for both E/T and P/Q/R/S.
 */
 mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;

 /* Write to the dynamic reconfiguration tables */
 rc = sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
       &mac[port], true);
 if (rc < 0) {
  dev_err(dev, "Failed to write MAC config: %d\n", rc);
  return rc;
 }

 /* Reconfigure the PLLs for the RGMII interfaces (required 125 MHz at
 * gigabit, 25 MHz at 100 Mbps and 2.5 MHz at 10 Mbps). For MII and
 * RMII no change of the clock setup is required. Actually, changing
 * the clock setup does interrupt the clock signal for a certain time
 * which causes trouble for all PHYs relying on this signal.
 */
 if (!phy_interface_mode_is_rgmii(priv->phy_mode[port]))
  return 0;

 return sja1105_clocking_setup_port(priv, port);
}

static struct phylink_pcs *
sja1105_mac_select_pcs(struct phylink_config *config, phy_interface_t iface)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct sja1105_private *priv = dp->ds->priv;

 return priv->pcs[dp->index];
}

static void sja1105_mac_config(struct phylink_config *config,
          unsigned int mode,
          const struct phylink_link_state *state)
{
}

static void sja1105_mac_link_down(struct phylink_config *config,
      unsigned int mode,
      phy_interface_t interface)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);

 sja1105_inhibit_tx(dp->ds->priv, BIT(dp->index), true);
}

static void sja1105_mac_link_up(struct phylink_config *config,
    struct phy_device *phydev,
    unsigned int mode,
    phy_interface_t interface,
    int speed, int duplex,
    bool tx_pause, bool rx_pause)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct sja1105_private *priv = dp->ds->priv;
 int port = dp->index;

 if (!sja1105_set_port_speed(priv, port, speed))
  sja1105_set_port_config(priv, port);

 sja1105_inhibit_tx(priv, BIT(port), false);
}

static void sja1105_phylink_get_caps(struct dsa_switch *ds, int port,
         struct phylink_config *config)
{
 struct sja1105_private *priv = ds->priv;
 struct sja1105_xmii_params_entry *mii;
 phy_interface_t phy_mode;

 phy_mode = priv->phy_mode[port];
 if (phy_mode == PHY_INTERFACE_MODE_SGMII ||
     phy_mode == PHY_INTERFACE_MODE_2500BASEX) {
  /* Changing the PHY mode on SERDES ports is possible and makes
 * sense, because that is done through the XPCS. We allow
 * changes between SGMII and 2500base-X.
 */
  if (priv->info->supports_sgmii[port])
   __set_bit(PHY_INTERFACE_MODE_SGMII,
      config->supported_interfaces);

  if (priv->info->supports_2500basex[port])
   __set_bit(PHY_INTERFACE_MODE_2500BASEX,
      config->supported_interfaces);
 } else {
  /* The SJA1105 MAC programming model is through the static
 * config (the xMII Mode table cannot be dynamically
 * reconfigured), and we have to program that early.
 */
  __set_bit(phy_mode, config->supported_interfaces);
 }

 /* The MAC does not support pause frames, and also doesn't
 * support half-duplex traffic modes.
 */
 config->mac_capabilities = MAC_10FD | MAC_100FD;

 mii = priv->static_config.tables[BLK_IDX_XMII_PARAMS].entries;
 if (mii->xmii_mode[port] == XMII_MODE_RGMII ||
     mii->xmii_mode[port] == XMII_MODE_SGMII)
  config->mac_capabilities |= MAC_1000FD;

 if (priv->info->supports_2500basex[port])
  config->mac_capabilities |= MAC_2500FD;
}

static int
sja1105_find_static_fdb_entry(struct sja1105_private *priv, int port,
         const struct sja1105_l2_lookup_entry *requested)
{
 struct sja1105_l2_lookup_entry *l2_lookup;
 struct sja1105_table *table;
 int i;

 table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
 l2_lookup = table->entries;

 for (i = 0; i < table->entry_count; i++)
  if (l2_lookup[i].macaddr == requested->macaddr &&
      l2_lookup[i].vlanid == requested->vlanid &&
      l2_lookup[i].destports & BIT(port))
   return i;

 return -1;
}

/* We want FDB entries added statically through the bridge command to persist
 * across switch resets, which are a common thing during normal SJA1105
 * operation. So we have to back them up in the static configuration tables
 * and hence apply them on next static config upload... yay!
 */
static int
sja1105_static_fdb_change(struct sja1105_private *priv, int port,
     const struct sja1105_l2_lookup_entry *requested,
     bool keep)
{
 struct sja1105_l2_lookup_entry *l2_lookup;
 struct sja1105_table *table;
 int rc, match;

 table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];

 match = sja1105_find_static_fdb_entry(priv, port, requested);
 if (match < 0) {
  /* Can't delete a missing entry. */
  if (!keep)
   return 0;

  /* No match => new entry */
  rc = sja1105_table_resize(table, table->entry_count + 1);
  if (rc)
   return rc;

  match = table->entry_count - 1;
 }

 /* Assign pointer after the resize (it may be new memory) */
 l2_lookup = table->entries;

 /* We have a match.
 * If the job was to add this FDB entry, it's already done (mostly
 * anyway, since the port forwarding mask may have changed, case in
 * which we update it).
 * Otherwise we have to delete it.
 */
 if (keep) {
  l2_lookup[match] = *requested;
  return 0;
 }

 /* To remove, the strategy is to overwrite the element with
 * the last one, and then reduce the array size by 1
 */
 l2_lookup[match] = l2_lookup[table->entry_count - 1];
 return sja1105_table_resize(table, table->entry_count - 1);
}

/* First-generation switches have a 4-way set associative TCAM that
 * holds the FDB entries. An FDB index spans from 0 to 1023 and is comprised of
 * a "bin" (grouping of 4 entries) and a "way" (an entry within a bin).
 * For the placement of a newly learnt FDB entry, the switch selects the bin
 * based on a hash function, and the way within that bin incrementally.
 */
static int sja1105et_fdb_index(int bin, int way)
{
 return bin * SJA1105ET_FDB_BIN_SIZE + way;
}

static int sja1105et_is_fdb_entry_in_bin(struct sja1105_private *priv, int bin,
      const u8 *addr, u16 vid,
      struct sja1105_l2_lookup_entry *match,
      int *last_unused)
{
 int way;

 for (way = 0; way < SJA1105ET_FDB_BIN_SIZE; way++) {
  struct sja1105_l2_lookup_entry l2_lookup = {0};
  int index = sja1105et_fdb_index(bin, way);

  /* Skip unused entries, optionally marking them
 * into the return value
 */
  if (sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
      index, &l2_lookup)) {
   if (last_unused)
    *last_unused = way;
   continue;
  }

  if (l2_lookup.macaddr == ether_addr_to_u64(addr) &&
      l2_lookup.vlanid == vid) {
   if (match)
    *match = l2_lookup;
   return way;
  }
 }
 /* Return an invalid entry index if not found */
 return -1;
}

int sja1105et_fdb_add(struct dsa_switch *ds, int port,
        const unsigned char *addr, u16 vid)
{
 struct sja1105_l2_lookup_entry l2_lookup = {0}, tmp;
 struct sja1105_private *priv = ds->priv;
 struct device *dev = ds->dev;
 int last_unused = -1;
 int start, end, i;
 int bin, way, rc;

 bin = sja1105et_fdb_hash(priv, addr, vid);

 way = sja1105et_is_fdb_entry_in_bin(priv, bin, addr, vid,
         &l2_lookup, &last_unused);
 if (way >= 0) {
  /* We have an FDB entry. Is our port in the destination
 * mask? If yes, we need to do nothing. If not, we need
 * to rewrite the entry by adding this port to it.
 */
  if ((l2_lookup.destports & BIT(port)) && l2_lookup.lockeds)
   return 0;
  l2_lookup.destports |= BIT(port);
 } else {
  int index = sja1105et_fdb_index(bin, way);

  /* We don't have an FDB entry. We construct a new one and
 * try to find a place for it within the FDB table.
 */
  l2_lookup.macaddr = ether_addr_to_u64(addr);
  l2_lookup.destports = BIT(port);
  l2_lookup.vlanid = vid;

  if (last_unused >= 0) {
   way = last_unused;
  } else {
   /* Bin is full, need to evict somebody.
 * Choose victim at random. If you get these messages
 * often, you may need to consider changing the
 * distribution function:
 * static_config[BLK_IDX_L2_LOOKUP_PARAMS].entries->poly
 */
   get_random_bytes(&way, sizeof(u8));
   way %= SJA1105ET_FDB_BIN_SIZE;
   dev_warn(dev, "Warning, FDB bin %d full while adding entry for %pM. Evicting entry %u.\n",
     bin, addr, way);
   /* Evict entry */
   sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
           index, NULL, false);
  }
 }
 l2_lookup.lockeds = true;
 l2_lookup.index = sja1105et_fdb_index(bin, way);

 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
       l2_lookup.index, &l2_lookup,
       true);
 if (rc < 0)
  return rc;

 /* Invalidate a dynamically learned entry if that exists */
 start = sja1105et_fdb_index(bin, 0);
 end = sja1105et_fdb_index(bin, way);

 for (i = start; i < end; i++) {
  rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
       i, &tmp);
  if (rc == -ENOENT)
   continue;
  if (rc)
   return rc;

  if (tmp.macaddr != ether_addr_to_u64(addr) || tmp.vlanid != vid)
   continue;

  rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
        i, NULL, false);
  if (rc)
   return rc;

  break;
 }

 return sja1105_static_fdb_change(priv, port, &l2_lookup, true);
}

int sja1105et_fdb_del(struct dsa_switch *ds, int port,
        const unsigned char *addr, u16 vid)
{
 struct sja1105_l2_lookup_entry l2_lookup = {0};
 struct sja1105_private *priv = ds->priv;
 int index, bin, way, rc;
 bool keep;

 bin = sja1105et_fdb_hash(priv, addr, vid);
 way = sja1105et_is_fdb_entry_in_bin(priv, bin, addr, vid,
         &l2_lookup, NULL);
 if (way < 0)
  return 0;
 index = sja1105et_fdb_index(bin, way);

 /* We have an FDB entry. Is our port in the destination mask? If yes,
 * we need to remove it. If the resulting port mask becomes empty, we
 * need to completely evict the FDB entry.
 * Otherwise we just write it back.
 */
 l2_lookup.destports &= ~BIT(port);

 if (l2_lookup.destports)
  keep = true;
 else
  keep = false;

 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
       index, &l2_lookup, keep);
 if (rc < 0)
  return rc;

 return sja1105_static_fdb_change(priv, port, &l2_lookup, keep);
}

int sja1105pqrs_fdb_add(struct dsa_switch *ds, int port,
   const unsigned char *addr, u16 vid)
{
 struct sja1105_l2_lookup_entry l2_lookup = {0}, tmp;
 struct sja1105_private *priv = ds->priv;
 int rc, i;

 /* Search for an existing entry in the FDB table */
 l2_lookup.macaddr = ether_addr_to_u64(addr);
 l2_lookup.vlanid = vid;
 l2_lookup.mask_macaddr = GENMASK_ULL(ETH_ALEN * 8 - 1, 0);
 l2_lookup.mask_vlanid = VLAN_VID_MASK;
 l2_lookup.destports = BIT(port);

 tmp = l2_lookup;

 rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
      SJA1105_SEARCH, &tmp);
 if (rc == 0 && tmp.index != SJA1105_MAX_L2_LOOKUP_COUNT - 1) {
  /* Found a static entry and this port is already in the entry's
 * port mask => job done
 */
  if ((tmp.destports & BIT(port)) && tmp.lockeds)
   return 0;

  l2_lookup = tmp;

  /* l2_lookup.index is populated by the switch in case it
 * found something.
 */
  l2_lookup.destports |= BIT(port);
  goto skip_finding_an_index;
 }

 /* Not found, so try to find an unused spot in the FDB.
 * This is slightly inefficient because the strategy is knock-knock at
 * every possible position from 0 to 1023.
 */
 for (i = 0; i < SJA1105_MAX_L2_LOOKUP_COUNT; i++) {
  rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
       i, NULL);
  if (rc < 0)
   break;
 }
 if (i == SJA1105_MAX_L2_LOOKUP_COUNT) {
  dev_err(ds->dev, "FDB is full, cannot add entry.\n");
  return -EINVAL;
 }
 l2_lookup.index = i;

skip_finding_an_index:
 l2_lookup.lockeds = true;

 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
       l2_lookup.index, &l2_lookup,
       true);
 if (rc < 0)
  return rc;

 /* The switch learns dynamic entries and looks up the FDB left to
 * right. It is possible that our addition was concurrent with the
 * dynamic learning of the same address, so now that the static entry
 * has been installed, we are certain that address learning for this
 * particular address has been turned off, so the dynamic entry either
 * is in the FDB at an index smaller than the static one, or isn't (it
 * can also be at a larger index, but in that case it is inactive
 * because the static FDB entry will match first, and the dynamic one
 * will eventually age out). Search for a dynamically learned address
 * prior to our static one and invalidate it.
 */
 tmp = l2_lookup;

 rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
      SJA1105_SEARCH, &tmp);
 if (rc < 0) {
  dev_err(ds->dev,
   "port %d failed to read back entry for %pM vid %d: %pe\n",
   port, addr, vid, ERR_PTR(rc));
  return rc;
 }

 if (tmp.index < l2_lookup.index) {
  rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
        tmp.index, NULL, false);
  if (rc < 0)
   return rc;
 }

 return sja1105_static_fdb_change(priv, port, &l2_lookup, true);
}

int sja1105pqrs_fdb_del(struct dsa_switch *ds, int port,
   const unsigned char *addr, u16 vid)
{
 struct sja1105_l2_lookup_entry l2_lookup = {0};
 struct sja1105_private *priv = ds->priv;
 bool keep;
 int rc;

 l2_lookup.macaddr = ether_addr_to_u64(addr);
 l2_lookup.vlanid = vid;
 l2_lookup.mask_macaddr = GENMASK_ULL(ETH_ALEN * 8 - 1, 0);
 l2_lookup.mask_vlanid = VLAN_VID_MASK;
 l2_lookup.destports = BIT(port);

 rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
      SJA1105_SEARCH, &l2_lookup);
 if (rc < 0)
  return 0;

 l2_lookup.destports &= ~BIT(port);

 /* Decide whether we remove just this port from the FDB entry,
 * or if we remove it completely.
 */
 if (l2_lookup.destports)
  keep = true;
 else
  keep = false;

 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
       l2_lookup.index, &l2_lookup, keep);
 if (rc < 0)
  return rc;

 return sja1105_static_fdb_change(priv, port, &l2_lookup, keep);
}

static int sja1105_fdb_add(struct dsa_switch *ds, int port,
      const unsigned char *addr, u16 vid,
      struct dsa_db db)
{
 struct sja1105_private *priv = ds->priv;
 int rc;

 if (!vid) {
  switch (db.type) {
  case DSA_DB_PORT:
   vid = dsa_tag_8021q_standalone_vid(db.dp);
   break;
  case DSA_DB_BRIDGE:
   vid = dsa_tag_8021q_bridge_vid(db.bridge.num);
   break;
  default:
   return -EOPNOTSUPP;
  }
 }

 mutex_lock(&priv->fdb_lock);
 rc = priv->info->fdb_add_cmd(ds, port, addr, vid);
 mutex_unlock(&priv->fdb_lock);

 return rc;
}

static int __sja1105_fdb_del(struct dsa_switch *ds, int port,
        const unsigned char *addr, u16 vid,
        struct dsa_db db)
{
 struct sja1105_private *priv = ds->priv;

 if (!vid) {
  switch (db.type) {
  case DSA_DB_PORT:
   vid = dsa_tag_8021q_standalone_vid(db.dp);
   break;
  case DSA_DB_BRIDGE:
   vid = dsa_tag_8021q_bridge_vid(db.bridge.num);
   break;
  default:
   return -EOPNOTSUPP;
  }
 }

 return priv->info->fdb_del_cmd(ds, port, addr, vid);
}

static int sja1105_fdb_del(struct dsa_switch *ds, int port,
      const unsigned char *addr, u16 vid,
      struct dsa_db db)
{
 struct sja1105_private *priv = ds->priv;
 int rc;

 mutex_lock(&priv->fdb_lock);
 rc = __sja1105_fdb_del(ds, port, addr, vid, db);
 mutex_unlock(&priv->fdb_lock);

 return rc;
}

static int sja1105_fdb_dump(struct dsa_switch *ds, int port,
       dsa_fdb_dump_cb_t *cb, void *data)
{
 struct sja1105_private *priv = ds->priv;
 struct device *dev = ds->dev;
 int i;

 for (i = 0; i < SJA1105_MAX_L2_LOOKUP_COUNT; i++) {
  struct sja1105_l2_lookup_entry l2_lookup = {0};
  u8 macaddr[ETH_ALEN];
  int rc;

  rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
       i, &l2_lookup);
  /* No fdb entry at i, not an issue */
  if (rc == -ENOENT)
   continue;
  if (rc) {
   dev_err(dev, "Failed to dump FDB: %d\n", rc);
   return rc;
  }

  /* FDB dump callback is per port. This means we have to
 * disregard a valid entry if it's not for this port, even if
 * only to revisit it later. This is inefficient because the
 * 1024-sized FDB table needs to be traversed 4 times through
 * SPI during a 'bridge fdb show' command.
 */
  if (!(l2_lookup.destports & BIT(port)))
   continue;

  u64_to_ether_addr(l2_lookup.macaddr, macaddr);

  /* Hardware FDB is shared for fdb and mdb, "bridge fdb show"
 * only wants to see unicast
 */
  if (is_multicast_ether_addr(macaddr))
   continue;

  /* We need to hide the dsa_8021q VLANs from the user. */
  if (vid_is_dsa_8021q(l2_lookup.vlanid))
   l2_lookup.vlanid = 0;
  rc = cb(macaddr, l2_lookup.vlanid, l2_lookup.lockeds, data);
  if (rc)
   return rc;
 }
 return 0;
}

static void sja1105_fast_age(struct dsa_switch *ds, int port)
{
 struct dsa_port *dp = dsa_to_port(ds, port);
 struct sja1105_private *priv = ds->priv;
 struct dsa_db db = {
  .type = DSA_DB_BRIDGE,
  .bridge = {
   .dev = dsa_port_bridge_dev_get(dp),
   .num = dsa_port_bridge_num_get(dp),
  },
 };
 int i;

 mutex_lock(&priv->fdb_lock);

 for (i = 0; i < SJA1105_MAX_L2_LOOKUP_COUNT; i++) {
  struct sja1105_l2_lookup_entry l2_lookup = {0};
  u8 macaddr[ETH_ALEN];
  int rc;

  rc = sja1105_dynamic_config_read(priv, BLK_IDX_L2_LOOKUP,
       i, &l2_lookup);
  /* No fdb entry at i, not an issue */
  if (rc == -ENOENT)
   continue;
  if (rc) {
   dev_err(ds->dev, "Failed to read FDB: %pe\n",
    ERR_PTR(rc));
   break;
  }

  if (!(l2_lookup.destports & BIT(port)))
   continue;

  /* Don't delete static FDB entries */
  if (l2_lookup.lockeds)
   continue;

  u64_to_ether_addr(l2_lookup.macaddr, macaddr);

  rc = __sja1105_fdb_del(ds, port, macaddr, l2_lookup.vlanid, db);
  if (rc) {
   dev_err(ds->dev,
    "Failed to delete FDB entry %pM vid %lld: %pe\n",
    macaddr, l2_lookup.vlanid, ERR_PTR(rc));
   break;
  }
 }

 mutex_unlock(&priv->fdb_lock);
}

static int sja1105_mdb_add(struct dsa_switch *ds, int port,
      const struct switchdev_obj_port_mdb *mdb,
      struct dsa_db db)
{
 return sja1105_fdb_add(ds, port, mdb->addr, mdb->vid, db);
}

static int sja1105_mdb_del(struct dsa_switch *ds, int port,
      const struct switchdev_obj_port_mdb *mdb,
      struct dsa_db db)
{
 return sja1105_fdb_del(ds, port, mdb->addr, mdb->vid, db);
}

/* Common function for unicast and broadcast flood configuration.
 * Flooding is configured between each {ingress, egress} port pair, and since
 * the bridge's semantics are those of "egress flooding", it means we must
 * enable flooding towards this port from all ingress ports that are in the
 * same forwarding domain.
 */
static int sja1105_manage_flood_domains(struct sja1105_private *priv)
{
 struct sja1105_l2_forwarding_entry *l2_fwd;
 struct dsa_switch *ds = priv->ds;
 int from, to, rc;

 l2_fwd = priv->static_config.tables[BLK_IDX_L2_FORWARDING].entries;

 for (from = 0; from < ds->num_ports; from++) {
  u64 fl_domain = 0, bc_domain = 0;

  for (to = 0; to < priv->ds->num_ports; to++) {
   if (!sja1105_can_forward(l2_fwd, from, to))
    continue;

   if (priv->ucast_egress_floods & BIT(to))
    fl_domain |= BIT(to);
   if (priv->bcast_egress_floods & BIT(to))
    bc_domain |= BIT(to);
  }

  /* Nothing changed, nothing to do */
  if (l2_fwd[from].fl_domain == fl_domain &&
      l2_fwd[from].bc_domain == bc_domain)
   continue;

  l2_fwd[from].fl_domain = fl_domain;
  l2_fwd[from].bc_domain = bc_domain;

  rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_FORWARDING,
        from, &l2_fwd[from], true);
  if (rc < 0)
   return rc;
 }

 return 0;
}

static int sja1105_bridge_member(struct dsa_switch *ds, int port,
     struct dsa_bridge bridge, bool member)
{
 struct sja1105_l2_forwarding_entry *l2_fwd;
 struct sja1105_private *priv = ds->priv;
 int i, rc;

 l2_fwd = priv->static_config.tables[BLK_IDX_L2_FORWARDING].entries;

 for (i = 0; i < ds->num_ports; i++) {
  /* Add this port to the forwarding matrix of the
 * other ports in the same bridge, and viceversa.
 */
  if (!dsa_is_user_port(ds, i))
   continue;
  /* For the ports already under the bridge, only one thing needs
 * to be done, and that is to add this port to their
 * reachability domain. So we can perform the SPI write for
 * them immediately. However, for this port itself (the one
 * that is new to the bridge), we need to add all other ports
 * to its reachability domain. So we do that incrementally in
 * this loop, and perform the SPI write only at the end, once
 * the domain contains all other bridge ports.
 */
  if (i == port)
   continue;
  if (!dsa_port_offloads_bridge(dsa_to_port(ds, i), &bridge))
   continue;
  sja1105_port_allow_traffic(l2_fwd, i, port, member);
  sja1105_port_allow_traffic(l2_fwd, port, i, member);

  rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_FORWARDING,
        i, &l2_fwd[i], true);
  if (rc < 0)
   return rc;
 }

 rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_FORWARDING,
       port, &l2_fwd[port], true);
 if (rc)
  return rc;

 rc = sja1105_commit_pvid(ds, port);
 if (rc)
  return rc;

 return sja1105_manage_flood_domains(priv);
}

static void sja1105_bridge_stp_state_set(struct dsa_switch *ds, int port,
      u8 state)
{
 struct dsa_port *dp = dsa_to_port(ds, port);
 struct sja1105_private *priv = ds->priv;
 struct sja1105_mac_config_entry *mac;

 mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;

 switch (state) {
 case BR_STATE_DISABLED:
 case BR_STATE_BLOCKING:
 case BR_STATE_LISTENING:
  /* From UM10944 description of DRPDTAG (why put this there?):
 * "Management traffic flows to the port regardless of the state
 * of the INGRESS flag". So BPDUs are still be allowed to pass.
 * At the moment no difference between DISABLED and BLOCKING.
 */
  mac[port].ingress   = false;
  mac[port].egress    = false;
  mac[port].dyn_learn = false;
  break;
 case BR_STATE_LEARNING:
  mac[port].ingress   = true;
  mac[port].egress    = false;
  mac[port].dyn_learn = dp->learning;
  break;
 case BR_STATE_FORWARDING:
  mac[port].ingress   = true;
  mac[port].egress    = true;
  mac[port].dyn_learn = dp->learning;
  break;
 default:
  dev_err(ds->dev, "invalid STP state: %d\n", state);
  return;
 }

 sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
         &mac[port], true);
}

static int sja1105_bridge_join(struct dsa_switch *ds, int port,
          struct dsa_bridge bridge,
          bool *tx_fwd_offload,
          struct netlink_ext_ack *extack)
{
 int rc;

 rc = sja1105_bridge_member(ds, port, bridge, true);
 if (rc)
  return rc;

 rc = dsa_tag_8021q_bridge_join(ds, port, bridge, tx_fwd_offload,
           extack);
 if (rc) {
  sja1105_bridge_member(ds, port, bridge, false);
  return rc;
 }

 return 0;
}

static void sja1105_bridge_leave(struct dsa_switch *ds, int port,
     struct dsa_bridge bridge)
{
 dsa_tag_8021q_bridge_leave(ds, port, bridge);
 sja1105_bridge_member(ds, port, bridge, false);
}

/* Port 0 (the uC port) does not have CBS shapers */
#define SJA1110_FIXED_CBS(port, prio) ((((port) - 1) * SJA1105_NUM_TC) + (prio))

static int sja1105_find_cbs_shaper(struct sja1105_private *priv,
       int port, int prio)
{
 int i;

 if (priv->info->fixed_cbs_mapping) {
  i = SJA1110_FIXED_CBS(port, prio);
  if (i >= 0 && i < priv->info->num_cbs_shapers)
   return i;

  return -1;
 }

 for (i = 0; i < priv->info->num_cbs_shapers; i++)
  if (priv->cbs[i].port == port && priv->cbs[i].prio == prio)
   return i;

 return -1;
}

static int sja1105_find_unused_cbs_shaper(struct sja1105_private *priv)
{
 int i;

 if (priv->info->fixed_cbs_mapping)
  return -1;

 for (i = 0; i < priv->info->num_cbs_shapers; i++)
  if (!priv->cbs[i].idle_slope && !priv->cbs[i].send_slope)
   return i;

 return -1;
}

static int sja1105_delete_cbs_shaper(struct sja1105_private *priv, int port,
         int prio)
{
 int i;

 for (i = 0; i < priv->info->num_cbs_shapers; i++) {
  struct sja1105_cbs_entry *cbs = &priv->cbs[i];

  if (cbs->port == port && cbs->prio == prio) {
   memset(cbs, 0, sizeof(*cbs));
   return sja1105_dynamic_config_write(priv, BLK_IDX_CBS,
           i, cbs, true);
  }
 }

 return 0;
}

static int sja1105_setup_tc_cbs(struct dsa_switch *ds, int port,
    struct tc_cbs_qopt_offload *offload)
{
 struct sja1105_private *priv = ds->priv;
 struct sja1105_cbs_entry *cbs;
 s64 port_transmit_rate_kbps;
 int index;

 if (!offload->enable)
  return sja1105_delete_cbs_shaper(priv, port, offload->queue);

 /* The user may be replacing an existing shaper */
 index = sja1105_find_cbs_shaper(priv, port, offload->queue);
 if (index < 0) {
  /* That isn't the case - see if we can allocate a new one */
  index = sja1105_find_unused_cbs_shaper(priv);
  if (index < 0)
   return -ENOSPC;
 }

 cbs = &priv->cbs[index];
 cbs->port = port;
 cbs->prio = offload->queue;
 /* locredit and sendslope are negative by definition. In hardware,
 * positive values must be provided, and the negative sign is implicit.
 */
 cbs->credit_hi = offload->hicredit;
 cbs->credit_lo = abs(offload->locredit);
 /* User space is in kbits/sec, while the hardware in bytes/sec times
 * link speed. Since the given offload->sendslope is good only for the
 * current link speed anyway, and user space is likely to reprogram it
 * when that changes, don't even bother to track the port's link speed,
 * but deduce the port transmit rate from idleslope - sendslope.
 */
 port_transmit_rate_kbps = offload->idleslope - offload->sendslope;
 cbs->idle_slope = div_s64(offload->idleslope * BYTES_PER_KBIT,
      port_transmit_rate_kbps);
 cbs->send_slope = div_s64(abs(offload->sendslope * BYTES_PER_KBIT),
      port_transmit_rate_kbps);
 /* Convert the negative values from 64-bit 2's complement
 * to 32-bit 2's complement (for the case of 0x80000000 whose
 * negative is still negative).
 */
 cbs->credit_lo &= GENMASK_ULL(31, 0);
--> --------------------

--> maximum size reached

--> --------------------