Quelle  chip.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Marvell 88e6xxx Ethernet switch single-chip support
 *
 * Copyright (c) 2008 Marvell Semiconductor
 *
 * Copyright (c) 2016 Andrew Lunn <andrew@lunn.ch>
 *
 * Copyright (c) 2016-2017 Savoir-faire Linux Inc.
 * Vivien Didelot <vivien.didelot@savoirfairelinux.com>
 */

#include <linux/bitfield.h>
#include <linux/delay.h>
#include <linux/dsa/mv88e6xxx.h>
#include <linux/etherdevice.h>
#include <linux/ethtool.h>
#include <linux/if_bridge.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/irqdomain.h>
#include <linux/jiffies.h>
#include <linux/list.h>
#include <linux/mdio.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_irq.h>
#include <linux/of_mdio.h>
#include <linux/platform_data/mv88e6xxx.h>
#include <linux/property.h>
#include <linux/netdevice.h>
#include <linux/gpio/consumer.h>
#include <linux/phylink.h>
#include <net/dsa.h>

#include "chip.h"
#include "devlink.h"
#include "global1.h"
#include "global2.h"
#include "hwtstamp.h"
#include "phy.h"
#include "port.h"
#include "ptp.h"
#include "serdes.h"
#include "smi.h"

static void assert_reg_lock(struct mv88e6xxx_chip *chip)
{
 if (unlikely(!mutex_is_locked(&chip->reg_lock))) {
  dev_err(chip->dev, "Switch registers lock not held!\n");
  dump_stack();
 }
}

int mv88e6xxx_read(struct mv88e6xxx_chip *chip, int addr, int reg, u16 *val)
{
 int err;

 assert_reg_lock(chip);

 err = mv88e6xxx_smi_read(chip, addr, reg, val);
 if (err)
  return err;

 dev_dbg(chip->dev, "<- addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
  addr, reg, *val);

 return 0;
}

int mv88e6xxx_write(struct mv88e6xxx_chip *chip, int addr, int reg, u16 val)
{
 int err;

 assert_reg_lock(chip);

 err = mv88e6xxx_smi_write(chip, addr, reg, val);
 if (err)
  return err;

 dev_dbg(chip->dev, "-> addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
  addr, reg, val);

 return 0;
}

int mv88e6xxx_wait_mask(struct mv88e6xxx_chip *chip, int addr, int reg,
   u16 mask, u16 val)
{
 const unsigned long timeout = jiffies + msecs_to_jiffies(50);
 u16 data;
 int err;
 int i;

 /* There's no bus specific operation to wait for a mask. Even
 * if the initial poll takes longer than 50ms, always do at
 * least one more attempt.
 */
 for (i = 0; time_before(jiffies, timeout) || (i < 2); i++) {
  err = mv88e6xxx_read(chip, addr, reg, &data);
  if (err)
   return err;

  if ((data & mask) == val)
   return 0;

  if (i < 2)
   cpu_relax();
  else
   usleep_range(1000, 2000);
 }

 err = mv88e6xxx_read(chip, addr, reg, &data);
 if (err)
  return err;

 if ((data & mask) == val)
  return 0;

 dev_err(chip->dev, "Timeout while waiting for switch\n");
 return -ETIMEDOUT;
}

int mv88e6xxx_wait_bit(struct mv88e6xxx_chip *chip, int addr, int reg,
         int bit, int val)
{
 return mv88e6xxx_wait_mask(chip, addr, reg, BIT(bit),
       val ? BIT(bit) : 0x0000);
}

struct mii_bus *mv88e6xxx_default_mdio_bus(struct mv88e6xxx_chip *chip)
{
 struct mv88e6xxx_mdio_bus *mdio_bus;

 mdio_bus = list_first_entry_or_null(&chip->mdios,
         struct mv88e6xxx_mdio_bus, list);
 if (!mdio_bus)
  return NULL;

 return mdio_bus->bus;
}

static void mv88e6xxx_g1_irq_mask(struct irq_data *d)
{
 struct mv88e6xxx_chip *chip = irq_data_get_irq_chip_data(d);
 unsigned int n = d->hwirq;

 chip->g1_irq.masked |= (1 << n);
}

static void mv88e6xxx_g1_irq_unmask(struct irq_data *d)
{
 struct mv88e6xxx_chip *chip = irq_data_get_irq_chip_data(d);
 unsigned int n = d->hwirq;

 chip->g1_irq.masked &= ~(1 << n);
}

static irqreturn_t mv88e6xxx_g1_irq_thread_work(struct mv88e6xxx_chip *chip)
{
 unsigned int nhandled = 0;
 unsigned int sub_irq;
 unsigned int n;
 u16 reg;
 u16 ctl1;
 int err;

 mv88e6xxx_reg_lock(chip);
 err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, ®);
 mv88e6xxx_reg_unlock(chip);

 if (err)
  goto out;

 do {
  for (n = 0; n < chip->g1_irq.nirqs; ++n) {
   if (reg & (1 << n)) {
    sub_irq = irq_find_mapping(chip->g1_irq.domain,
          n);
    handle_nested_irq(sub_irq);
    ++nhandled;
   }
  }

  mv88e6xxx_reg_lock(chip);
  err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &ctl1);
  if (err)
   goto unlock;
  err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, ®);
unlock:
  mv88e6xxx_reg_unlock(chip);
  if (err)
   goto out;
  ctl1 &= GENMASK(chip->g1_irq.nirqs, 0);
 } while (reg & ctl1);

out:
 return (nhandled > 0 ? IRQ_HANDLED : IRQ_NONE);
}

static irqreturn_t mv88e6xxx_g1_irq_thread_fn(int irq, void *dev_id)
{
 struct mv88e6xxx_chip *chip = dev_id;

 return mv88e6xxx_g1_irq_thread_work(chip);
}

static void mv88e6xxx_g1_irq_bus_lock(struct irq_data *d)
{
 struct mv88e6xxx_chip *chip = irq_data_get_irq_chip_data(d);

 mv88e6xxx_reg_lock(chip);
}

static void mv88e6xxx_g1_irq_bus_sync_unlock(struct irq_data *d)
{
 struct mv88e6xxx_chip *chip = irq_data_get_irq_chip_data(d);
 u16 mask = GENMASK(chip->g1_irq.nirqs, 0);
 u16 reg;
 int err;

 err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, ®);
 if (err)
  goto out;

 reg &= ~mask;
 reg |= (~chip->g1_irq.masked & mask);

 err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, reg);
 if (err)
  goto out;

out:
 mv88e6xxx_reg_unlock(chip);
}

static const struct irq_chip mv88e6xxx_g1_irq_chip = {
 .name   = "mv88e6xxx-g1",
 .irq_mask  = mv88e6xxx_g1_irq_mask,
 .irq_unmask  = mv88e6xxx_g1_irq_unmask,
 .irq_bus_lock  = mv88e6xxx_g1_irq_bus_lock,
 .irq_bus_sync_unlock = mv88e6xxx_g1_irq_bus_sync_unlock,
};

static int mv88e6xxx_g1_irq_domain_map(struct irq_domain *d,
           unsigned int irq,
           irq_hw_number_t hwirq)
{
 struct mv88e6xxx_chip *chip = d->host_data;

 irq_set_chip_data(irq, d->host_data);
 irq_set_chip_and_handler(irq, &chip->g1_irq.chip, handle_level_irq);
 irq_set_noprobe(irq);

 return 0;
}

static const struct irq_domain_ops mv88e6xxx_g1_irq_domain_ops = {
 .map = mv88e6xxx_g1_irq_domain_map,
 .xlate = irq_domain_xlate_twocell,
};

/* To be called with reg_lock held */
static void mv88e6xxx_g1_irq_free_common(struct mv88e6xxx_chip *chip)
{
 int irq, virq;
 u16 mask;

 mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &mask);
 mask &= ~GENMASK(chip->g1_irq.nirqs, 0);
 mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, mask);

 for (irq = 0; irq < chip->g1_irq.nirqs; irq++) {
  virq = irq_find_mapping(chip->g1_irq.domain, irq);
  irq_dispose_mapping(virq);
 }

 irq_domain_remove(chip->g1_irq.domain);
}

static void mv88e6xxx_g1_irq_free(struct mv88e6xxx_chip *chip)
{
 /*
 * free_irq must be called without reg_lock taken because the irq
 * handler takes this lock, too.
 */
 free_irq(chip->irq, chip);

 mv88e6xxx_reg_lock(chip);
 mv88e6xxx_g1_irq_free_common(chip);
 mv88e6xxx_reg_unlock(chip);
}

static int mv88e6xxx_g1_irq_setup_common(struct mv88e6xxx_chip *chip)
{
 int err, irq, virq;
 u16 reg, mask;

 chip->g1_irq.nirqs = chip->info->g1_irqs;
 chip->g1_irq.domain = irq_domain_create_simple(
  NULL, chip->g1_irq.nirqs, 0,
  &mv88e6xxx_g1_irq_domain_ops, chip);
 if (!chip->g1_irq.domain)
  return -ENOMEM;

 for (irq = 0; irq < chip->g1_irq.nirqs; irq++)
  irq_create_mapping(chip->g1_irq.domain, irq);

 chip->g1_irq.chip = mv88e6xxx_g1_irq_chip;
 chip->g1_irq.masked = ~0;

 err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_CTL1, &mask);
 if (err)
  goto out_mapping;

 mask &= ~GENMASK(chip->g1_irq.nirqs, 0);

 err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, mask);
 if (err)
  goto out_disable;

 /* Reading the interrupt status clears (most of) them */
 err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_STS, ®);
 if (err)
  goto out_disable;

 return 0;

out_disable:
 mask &= ~GENMASK(chip->g1_irq.nirqs, 0);
 mv88e6xxx_g1_write(chip, MV88E6XXX_G1_CTL1, mask);

out_mapping:
 for (irq = 0; irq < 16; irq++) {
  virq = irq_find_mapping(chip->g1_irq.domain, irq);
  irq_dispose_mapping(virq);
 }

 irq_domain_remove(chip->g1_irq.domain);

 return err;
}

static int mv88e6xxx_g1_irq_setup(struct mv88e6xxx_chip *chip)
{
 static struct lock_class_key lock_key;
 static struct lock_class_key request_key;
 int err;

 err = mv88e6xxx_g1_irq_setup_common(chip);
 if (err)
  return err;

 /* These lock classes tells lockdep that global 1 irqs are in
 * a different category than their parent GPIO, so it won't
 * report false recursion.
 */
 irq_set_lockdep_class(chip->irq, &lock_key, &request_key);

 snprintf(chip->irq_name, sizeof(chip->irq_name),
   "mv88e6xxx-%s", dev_name(chip->dev));

 mv88e6xxx_reg_unlock(chip);
 err = request_threaded_irq(chip->irq, NULL,
       mv88e6xxx_g1_irq_thread_fn,
       IRQF_ONESHOT | IRQF_SHARED,
       chip->irq_name, chip);
 mv88e6xxx_reg_lock(chip);
 if (err)
  mv88e6xxx_g1_irq_free_common(chip);

 return err;
}

static void mv88e6xxx_irq_poll(struct kthread_work *work)
{
 struct mv88e6xxx_chip *chip = container_of(work,
         struct mv88e6xxx_chip,
         irq_poll_work.work);
 mv88e6xxx_g1_irq_thread_work(chip);

 kthread_queue_delayed_work(chip->kworker, &chip->irq_poll_work,
       msecs_to_jiffies(100));
}

static int mv88e6xxx_irq_poll_setup(struct mv88e6xxx_chip *chip)
{
 int err;

 err = mv88e6xxx_g1_irq_setup_common(chip);
 if (err)
  return err;

 kthread_init_delayed_work(&chip->irq_poll_work,
      mv88e6xxx_irq_poll);

 chip->kworker = kthread_run_worker(0, "%s", dev_name(chip->dev));
 if (IS_ERR(chip->kworker))
  return PTR_ERR(chip->kworker);

 kthread_queue_delayed_work(chip->kworker, &chip->irq_poll_work,
       msecs_to_jiffies(100));

 return 0;
}

static void mv88e6xxx_irq_poll_free(struct mv88e6xxx_chip *chip)
{
 kthread_cancel_delayed_work_sync(&chip->irq_poll_work);
 kthread_destroy_worker(chip->kworker);

 mv88e6xxx_reg_lock(chip);
 mv88e6xxx_g1_irq_free_common(chip);
 mv88e6xxx_reg_unlock(chip);
}

static int mv88e6xxx_port_config_interface(struct mv88e6xxx_chip *chip,
        int port, phy_interface_t interface)
{
 int err;

 if (chip->info->ops->port_set_rgmii_delay) {
  err = chip->info->ops->port_set_rgmii_delay(chip, port,
           interface);
  if (err && err != -EOPNOTSUPP)
   return err;
 }

 if (chip->info->ops->port_set_cmode) {
  err = chip->info->ops->port_set_cmode(chip, port,
            interface);
  if (err && err != -EOPNOTSUPP)
   return err;
 }

 return 0;
}

static int mv88e6xxx_port_setup_mac(struct mv88e6xxx_chip *chip, int port,
        int link, int speed, int duplex, int pause,
        phy_interface_t mode)
{
 int err;

 if (!chip->info->ops->port_set_link)
  return 0;

 /* Port's MAC control must not be changed unless the link is down */
 err = chip->info->ops->port_set_link(chip, port, LINK_FORCED_DOWN);
 if (err)
  return err;

 if (chip->info->ops->port_set_speed_duplex) {
  err = chip->info->ops->port_set_speed_duplex(chip, port,
            speed, duplex);
  if (err && err != -EOPNOTSUPP)
   goto restore_link;
 }

 if (chip->info->ops->port_set_pause) {
  err = chip->info->ops->port_set_pause(chip, port, pause);
  if (err)
   goto restore_link;
 }

 err = mv88e6xxx_port_config_interface(chip, port, mode);
restore_link:
 if (chip->info->ops->port_set_link(chip, port, link))
  dev_err(chip->dev, "p%d: failed to restore MAC's link\n", port);

 return err;
}

static int mv88e6xxx_phy_is_internal(struct mv88e6xxx_chip *chip, int port)
{
 return port >= chip->info->internal_phys_offset &&
  port < chip->info->num_internal_phys +
   chip->info->internal_phys_offset;
}

static int mv88e6xxx_port_ppu_updates(struct mv88e6xxx_chip *chip, int port)
{
 u16 reg;
 int err;

 /* The 88e6250 family does not have the PHY detect bit. Instead,
 * report whether the port is internal.
 */
 if (chip->info->family == MV88E6XXX_FAMILY_6250)
  return mv88e6xxx_phy_is_internal(chip, port);

 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, ®);
 if (err) {
  dev_err(chip->dev,
   "p%d: %s: failed to read port status\n",
   port, __func__);
  return err;
 }

 return !!(reg & MV88E6XXX_PORT_STS_PHY_DETECT);
}

static const u8 mv88e6185_phy_interface_modes[] = {
 [MV88E6185_PORT_STS_CMODE_GMII_FD]  = PHY_INTERFACE_MODE_GMII,
 [MV88E6185_PORT_STS_CMODE_MII_100_FD_PS] = PHY_INTERFACE_MODE_MII,
 [MV88E6185_PORT_STS_CMODE_MII_100]  = PHY_INTERFACE_MODE_MII,
 [MV88E6185_PORT_STS_CMODE_MII_10]  = PHY_INTERFACE_MODE_MII,
 [MV88E6185_PORT_STS_CMODE_SERDES]  = PHY_INTERFACE_MODE_1000BASEX,
 [MV88E6185_PORT_STS_CMODE_1000BASE_X]  = PHY_INTERFACE_MODE_1000BASEX,
 [MV88E6185_PORT_STS_CMODE_PHY]   = PHY_INTERFACE_MODE_SGMII,
};

static void mv88e6095_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 u8 cmode = chip->ports[port].cmode;

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100;

 if (mv88e6xxx_phy_is_internal(chip, port)) {
  __set_bit(PHY_INTERFACE_MODE_MII, config->supported_interfaces);
 } else {
  if (cmode < ARRAY_SIZE(mv88e6185_phy_interface_modes) &&
      mv88e6185_phy_interface_modes[cmode])
   __set_bit(mv88e6185_phy_interface_modes[cmode],
      config->supported_interfaces);

  config->mac_capabilities |= MAC_1000FD;
 }
}

static void mv88e6185_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 u8 cmode = chip->ports[port].cmode;

 if (cmode < ARRAY_SIZE(mv88e6185_phy_interface_modes) &&
     mv88e6185_phy_interface_modes[cmode])
  __set_bit(mv88e6185_phy_interface_modes[cmode],
     config->supported_interfaces);

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;
}

static const u8 mv88e6xxx_phy_interface_modes[] = {
 [MV88E6XXX_PORT_STS_CMODE_MII_PHY] = PHY_INTERFACE_MODE_REVMII,
 [MV88E6XXX_PORT_STS_CMODE_MII]  = PHY_INTERFACE_MODE_MII,
 [MV88E6XXX_PORT_STS_CMODE_GMII]  = PHY_INTERFACE_MODE_GMII,
 [MV88E6XXX_PORT_STS_CMODE_RMII_PHY] = PHY_INTERFACE_MODE_REVRMII,
 [MV88E6XXX_PORT_STS_CMODE_RMII]  = PHY_INTERFACE_MODE_RMII,
 [MV88E6XXX_PORT_STS_CMODE_100BASEX] = PHY_INTERFACE_MODE_100BASEX,
 [MV88E6XXX_PORT_STS_CMODE_1000BASEX] = PHY_INTERFACE_MODE_1000BASEX,
 [MV88E6XXX_PORT_STS_CMODE_SGMII] = PHY_INTERFACE_MODE_SGMII,
 /* higher interface modes are not needed here, since ports supporting
 * them are writable, and so the supported interfaces are filled in the
 * corresponding .phylink_set_interfaces() implementation below
 */
};

static void mv88e6xxx_translate_cmode(u8 cmode, unsigned long *supported)
{
 if (cmode < ARRAY_SIZE(mv88e6xxx_phy_interface_modes) &&
     mv88e6xxx_phy_interface_modes[cmode])
  __set_bit(mv88e6xxx_phy_interface_modes[cmode], supported);
 else if (cmode == MV88E6XXX_PORT_STS_CMODE_RGMII)
  phy_interface_set_rgmii(supported);
}

static void
mv88e6250_setup_supported_interfaces(struct mv88e6xxx_chip *chip, int port,
         struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;
 int err;
 u16 reg;

 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, ®);
 if (err) {
  dev_err(chip->dev, "p%d: failed to read port status\n", port);
  return;
 }

 switch (reg & MV88E6250_PORT_STS_PORTMODE_MASK) {
 case MV88E6250_PORT_STS_PORTMODE_MII_10_HALF_PHY:
 case MV88E6250_PORT_STS_PORTMODE_MII_100_HALF_PHY:
 case MV88E6250_PORT_STS_PORTMODE_MII_10_FULL_PHY:
 case MV88E6250_PORT_STS_PORTMODE_MII_100_FULL_PHY:
  __set_bit(PHY_INTERFACE_MODE_REVMII, supported);
  break;

 case MV88E6250_PORT_STS_PORTMODE_MII_HALF:
 case MV88E6250_PORT_STS_PORTMODE_MII_FULL:
  __set_bit(PHY_INTERFACE_MODE_MII, supported);
  break;

 case MV88E6250_PORT_STS_PORTMODE_MII_DUAL_100_RMII_FULL_PHY:
 case MV88E6250_PORT_STS_PORTMODE_MII_200_RMII_FULL_PHY:
 case MV88E6250_PORT_STS_PORTMODE_MII_10_100_RMII_HALF_PHY:
 case MV88E6250_PORT_STS_PORTMODE_MII_10_100_RMII_FULL_PHY:
  __set_bit(PHY_INTERFACE_MODE_REVRMII, supported);
  break;

 case MV88E6250_PORT_STS_PORTMODE_MII_DUAL_100_RMII_FULL:
 case MV88E6250_PORT_STS_PORTMODE_MII_10_100_RMII_FULL:
  __set_bit(PHY_INTERFACE_MODE_RMII, supported);
  break;

 case MV88E6250_PORT_STS_PORTMODE_MII_100_RGMII:
  __set_bit(PHY_INTERFACE_MODE_RGMII, supported);
  break;

 default:
  dev_err(chip->dev,
   "p%d: invalid port mode in status register: %04x\n",
   port, reg);
 }
}

static void mv88e6250_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 if (!mv88e6xxx_phy_is_internal(chip, port))
  mv88e6250_setup_supported_interfaces(chip, port, config);

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100;
}

static void mv88e6351_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;

 /* Translate the default cmode */
 mv88e6xxx_translate_cmode(chip->ports[port].cmode, supported);

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;
}

static int mv88e63xx_get_port_serdes_cmode(struct mv88e6xxx_chip *chip, int port)
{
 u16 reg, val;
 int err;

 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, ®);
 if (err)
  return err;

 /* If PHY_DETECT is zero, then we are not in auto-media mode */
 if (!(reg & MV88E6XXX_PORT_STS_PHY_DETECT))
  return 0xf;

 val = reg & ~MV88E6XXX_PORT_STS_PHY_DETECT;
 err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_STS, val);
 if (err)
  return err;

 err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &val);
 if (err)
  return err;

 /* Restore PHY_DETECT value */
 err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_STS, reg);
 if (err)
  return err;

 return val & MV88E6XXX_PORT_STS_CMODE_MASK;
}

static void mv88e6352_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;
 int err, cmode;

 /* Translate the default cmode */
 mv88e6xxx_translate_cmode(chip->ports[port].cmode, supported);

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;

 /* Port 4 supports automedia if the serdes is associated with it. */
 if (port == 4) {
  err = mv88e6352_g2_scratch_port_has_serdes(chip, port);
  if (err < 0)
   dev_err(chip->dev, "p%d: failed to read scratch\n",
    port);
  if (err <= 0)
   return;

  cmode = mv88e63xx_get_port_serdes_cmode(chip, port);
  if (cmode < 0)
   dev_err(chip->dev, "p%d: failed to read serdes cmode\n",
    port);
  else
   mv88e6xxx_translate_cmode(cmode, supported);
 }
}

static void mv88e632x_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;
 int cmode;

 /* Translate the default cmode */
 mv88e6xxx_translate_cmode(chip->ports[port].cmode, supported);

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;

 /* Port 0/1 are serdes only ports */
 if (port == 0 || port == 1) {
  cmode = mv88e63xx_get_port_serdes_cmode(chip, port);
  if (cmode < 0)
   dev_err(chip->dev, "p%d: failed to read serdes cmode\n",
    port);
  else
   mv88e6xxx_translate_cmode(cmode, supported);
 }
}

static void mv88e6341_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;

 /* Translate the default cmode */
 mv88e6xxx_translate_cmode(chip->ports[port].cmode, supported);

 /* No ethtool bits for 200Mbps */
 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;

 /* The C_Mode field is programmable on port 5 */
 if (port == 5) {
  __set_bit(PHY_INTERFACE_MODE_SGMII, supported);
  __set_bit(PHY_INTERFACE_MODE_1000BASEX, supported);
  __set_bit(PHY_INTERFACE_MODE_2500BASEX, supported);

  config->mac_capabilities |= MAC_2500FD;
 }
}

static void mv88e6390_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
           struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;

 /* Translate the default cmode */
 mv88e6xxx_translate_cmode(chip->ports[port].cmode, supported);

 /* No ethtool bits for 200Mbps */
 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;

 /* The C_Mode field is programmable on ports 9 and 10 */
 if (port == 9 || port == 10) {
  __set_bit(PHY_INTERFACE_MODE_SGMII, supported);
  __set_bit(PHY_INTERFACE_MODE_1000BASEX, supported);
  __set_bit(PHY_INTERFACE_MODE_2500BASEX, supported);

  config->mac_capabilities |= MAC_2500FD;
 }
}

static void mv88e6390x_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
     struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;

 mv88e6390_phylink_get_caps(chip, port, config);

 /* For the 6x90X, ports 2-7 can be in automedia mode.
 * (Note that 6x90 doesn't support RXAUI nor XAUI).
 *
 * Port 2 can also support 1000BASE-X in automedia mode if port 9 is
 * configured for 1000BASE-X, SGMII or 2500BASE-X.
 * Port 3-4 can also support 1000BASE-X in automedia mode if port 9 is
 * configured for RXAUI, 1000BASE-X, SGMII or 2500BASE-X.
 *
 * Port 5 can also support 1000BASE-X in automedia mode if port 10 is
 * configured for 1000BASE-X, SGMII or 2500BASE-X.
 * Port 6-7 can also support 1000BASE-X in automedia mode if port 10 is
 * configured for RXAUI, 1000BASE-X, SGMII or 2500BASE-X.
 *
 * For now, be permissive (as the old code was) and allow 1000BASE-X
 * on ports 2..7.
 */
 if (port >= 2 && port <= 7)
  __set_bit(PHY_INTERFACE_MODE_1000BASEX, supported);

 /* The C_Mode field can also be programmed for 10G speeds */
 if (port == 9 || port == 10) {
  __set_bit(PHY_INTERFACE_MODE_XAUI, supported);
  __set_bit(PHY_INTERFACE_MODE_RXAUI, supported);

  config->mac_capabilities |= MAC_10000FD;
 }
}

static void mv88e6393x_phylink_get_caps(struct mv88e6xxx_chip *chip, int port,
     struct phylink_config *config)
{
 unsigned long *supported = config->supported_interfaces;
 bool is_6191x =
  chip->info->prod_num == MV88E6XXX_PORT_SWITCH_ID_PROD_6191X;
 bool is_6361 =
  chip->info->prod_num == MV88E6XXX_PORT_SWITCH_ID_PROD_6361;

 mv88e6xxx_translate_cmode(chip->ports[port].cmode, supported);

 config->mac_capabilities = MAC_SYM_PAUSE | MAC_10 | MAC_100 |
       MAC_1000FD;

 /* The C_Mode field can be programmed for ports 0, 9 and 10 */
 if (port == 0 || port == 9 || port == 10) {
  __set_bit(PHY_INTERFACE_MODE_SGMII, supported);
  __set_bit(PHY_INTERFACE_MODE_1000BASEX, supported);

  /* 6191X supports >1G modes only on port 10 */
  if (!is_6191x || port == 10) {
   __set_bit(PHY_INTERFACE_MODE_2500BASEX, supported);
   config->mac_capabilities |= MAC_2500FD;

   /* 6361 only supports up to 2500BaseX */
   if (!is_6361) {
    __set_bit(PHY_INTERFACE_MODE_5GBASER, supported);
    __set_bit(PHY_INTERFACE_MODE_10GBASER, supported);
    __set_bit(PHY_INTERFACE_MODE_USXGMII, supported);
    config->mac_capabilities |= MAC_5000FD |
     MAC_10000FD;
   }
  }
 }

 if (port == 0) {
  __set_bit(PHY_INTERFACE_MODE_RMII, supported);
  __set_bit(PHY_INTERFACE_MODE_RGMII, supported);
  __set_bit(PHY_INTERFACE_MODE_RGMII_ID, supported);
  __set_bit(PHY_INTERFACE_MODE_RGMII_RXID, supported);
  __set_bit(PHY_INTERFACE_MODE_RGMII_TXID, supported);
 }
}

static void mv88e6xxx_get_caps(struct dsa_switch *ds, int port,
          struct phylink_config *config)
{
 struct mv88e6xxx_chip *chip = ds->priv;

 mv88e6xxx_reg_lock(chip);
 chip->info->ops->phylink_get_caps(chip, port, config);
 mv88e6xxx_reg_unlock(chip);

 if (mv88e6xxx_phy_is_internal(chip, port)) {
  __set_bit(PHY_INTERFACE_MODE_INTERNAL,
     config->supported_interfaces);
  /* Internal ports with no phy-mode need GMII for PHYLIB */
  __set_bit(PHY_INTERFACE_MODE_GMII,
     config->supported_interfaces);
 }
}

static struct phylink_pcs *
mv88e6xxx_mac_select_pcs(struct phylink_config *config,
    phy_interface_t interface)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct mv88e6xxx_chip *chip = dp->ds->priv;
 struct phylink_pcs *pcs = NULL;

 if (chip->info->ops->pcs_ops)
  pcs = chip->info->ops->pcs_ops->pcs_select(chip, dp->index,
          interface);

 return pcs;
}

static int mv88e6xxx_mac_prepare(struct phylink_config *config,
     unsigned int mode, phy_interface_t interface)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct mv88e6xxx_chip *chip = dp->ds->priv;
 int port = dp->index;
 int err = 0;

 /* In inband mode, the link may come up at any time while the link
 * is not forced down. Force the link down while we reconfigure the
 * interface mode.
 */
 if (mode == MLO_AN_INBAND &&
     chip->ports[port].interface != interface &&
     chip->info->ops->port_set_link) {
  mv88e6xxx_reg_lock(chip);
  err = chip->info->ops->port_set_link(chip, port,
           LINK_FORCED_DOWN);
  mv88e6xxx_reg_unlock(chip);
 }

 return err;
}

static void mv88e6xxx_mac_config(struct phylink_config *config,
     unsigned int mode,
     const struct phylink_link_state *state)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct mv88e6xxx_chip *chip = dp->ds->priv;
 int port = dp->index;
 int err = 0;

 mv88e6xxx_reg_lock(chip);

 if (mode != MLO_AN_PHY || !mv88e6xxx_phy_is_internal(chip, port)) {
  err = mv88e6xxx_port_config_interface(chip, port,
            state->interface);
  if (err && err != -EOPNOTSUPP)
   goto err_unlock;
 }

err_unlock:
 mv88e6xxx_reg_unlock(chip);

 if (err && err != -EOPNOTSUPP)
  dev_err(chip->dev, "p%d: failed to configure MAC/PCS\n", port);
}

static int mv88e6xxx_mac_finish(struct phylink_config *config,
    unsigned int mode, phy_interface_t interface)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct mv88e6xxx_chip *chip = dp->ds->priv;
 int port = dp->index;
 int err = 0;

 /* Undo the forced down state above after completing configuration
 * irrespective of its state on entry, which allows the link to come
 * up in the in-band case where there is no separate SERDES. Also
 * ensure that the link can come up if the PPU is in use and we are
 * in PHY mode (we treat the PPU as an effective in-band mechanism.)
 */
 mv88e6xxx_reg_lock(chip);

 if (chip->info->ops->port_set_link &&
     ((mode == MLO_AN_INBAND &&
       chip->ports[port].interface != interface) ||
      (mode == MLO_AN_PHY && mv88e6xxx_port_ppu_updates(chip, port))))
  err = chip->info->ops->port_set_link(chip, port, LINK_UNFORCED);

 mv88e6xxx_reg_unlock(chip);

 chip->ports[port].interface = interface;

 return err;
}

static void mv88e6xxx_mac_link_down(struct phylink_config *config,
        unsigned int mode,
        phy_interface_t interface)
{
 struct dsa_port *dp = dsa_phylink_to_port(config);
 struct mv88e6xxx_chip *chip = dp->ds->priv;
 const struct mv88e6xxx_ops *ops;
 int port = dp->index;
 int err = 0;

 ops = chip->info->ops;

 mv88e6xxx_reg_lock(chip);
 /* Force the link down if we know the port may not be automatically
 * updated by the switch or if we are using fixed-link mode.
 */
 if ((!mv88e6xxx_port_ppu_updates(chip, port) ||
      mode == MLO_AN_FIXED) && ops->port_sync_link)
  err = ops->port_sync_link(chip, port, mode, false);

 if (!err && ops->port_set_speed_duplex)
  err = ops->port_set_speed_duplex(chip, port, SPEED_UNFORCED,
       DUPLEX_UNFORCED);
 mv88e6xxx_reg_unlock(chip);

 if (err)
  dev_err(chip->dev,
   "p%d: failed to force MAC link down\n", port);
}

static void mv88e6xxx_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 mv88e6xxx_chip *chip = dp->ds->priv;
 const struct mv88e6xxx_ops *ops;
 int port = dp->index;
 int err = 0;

 ops = chip->info->ops;

 mv88e6xxx_reg_lock(chip);
 /* Configure and force the link up if we know that the port may not
 * automatically updated by the switch or if we are using fixed-link
 * mode.
 */
 if (!mv88e6xxx_port_ppu_updates(chip, port) ||
     mode == MLO_AN_FIXED) {
  if (ops->port_set_speed_duplex) {
   err = ops->port_set_speed_duplex(chip, port,
        speed, duplex);
   if (err && err != -EOPNOTSUPP)
    goto error;
  }

  if (ops->port_sync_link)
   err = ops->port_sync_link(chip, port, mode, true);
 }
error:
 mv88e6xxx_reg_unlock(chip);

 if (err && err != -EOPNOTSUPP)
  dev_err(chip->dev,
   "p%d: failed to configure MAC link up\n", port);
}

static int mv88e6xxx_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
{
 int err;

 if (!chip->info->ops->stats_snapshot)
  return -EOPNOTSUPP;

 mv88e6xxx_reg_lock(chip);
 err = chip->info->ops->stats_snapshot(chip, port);
 mv88e6xxx_reg_unlock(chip);

 return err;
}

#define MV88E6XXX_HW_STAT_MAPPER(_fn)        \
 _fn(in_good_octets,  8, 0x00, STATS_TYPE_BANK0), \
 _fn(in_bad_octets,  4, 0x02, STATS_TYPE_BANK0), \
 _fn(in_unicast,   4, 0x04, STATS_TYPE_BANK0), \
 _fn(in_broadcasts,  4, 0x06, STATS_TYPE_BANK0), \
 _fn(in_multicasts,  4, 0x07, STATS_TYPE_BANK0), \
 _fn(in_pause,   4, 0x16, STATS_TYPE_BANK0), \
 _fn(in_undersize,  4, 0x18, STATS_TYPE_BANK0), \
 _fn(in_fragments,  4, 0x19, STATS_TYPE_BANK0), \
 _fn(in_oversize,  4, 0x1a, STATS_TYPE_BANK0), \
 _fn(in_jabber,   4, 0x1b, STATS_TYPE_BANK0), \
 _fn(in_rx_error,  4, 0x1c, STATS_TYPE_BANK0), \
 _fn(in_fcs_error,  4, 0x1d, STATS_TYPE_BANK0), \
 _fn(out_octets,   8, 0x0e, STATS_TYPE_BANK0), \
 _fn(out_unicast,  4, 0x10, STATS_TYPE_BANK0), \
 _fn(out_broadcasts,  4, 0x13, STATS_TYPE_BANK0), \
 _fn(out_multicasts,  4, 0x12, STATS_TYPE_BANK0), \
 _fn(out_pause,   4, 0x15, STATS_TYPE_BANK0), \
 _fn(excessive,   4, 0x11, STATS_TYPE_BANK0), \
 _fn(collisions,   4, 0x1e, STATS_TYPE_BANK0), \
 _fn(deferred,   4, 0x05, STATS_TYPE_BANK0), \
 _fn(single,   4, 0x14, STATS_TYPE_BANK0), \
 _fn(multiple,   4, 0x17, STATS_TYPE_BANK0), \
 _fn(out_fcs_error,  4, 0x03, STATS_TYPE_BANK0), \
 _fn(late,   4, 0x1f, STATS_TYPE_BANK0), \
 _fn(hist_64bytes,  4, 0x08, STATS_TYPE_BANK0), \
 _fn(hist_65_127bytes,  4, 0x09, STATS_TYPE_BANK0), \
 _fn(hist_128_255bytes,  4, 0x0a, STATS_TYPE_BANK0), \
 _fn(hist_256_511bytes,  4, 0x0b, STATS_TYPE_BANK0), \
 _fn(hist_512_1023bytes,  4, 0x0c, STATS_TYPE_BANK0), \
 _fn(hist_1024_max_bytes, 4, 0x0d, STATS_TYPE_BANK0), \
 _fn(sw_in_discards,  4, 0x10, STATS_TYPE_PORT), \
 _fn(sw_in_filtered,  2, 0x12, STATS_TYPE_PORT), \
 _fn(sw_out_filtered,  2, 0x13, STATS_TYPE_PORT), \
 _fn(in_discards,  4, 0x00, STATS_TYPE_BANK1), \
 _fn(in_filtered,  4, 0x01, STATS_TYPE_BANK1), \
 _fn(in_accepted,  4, 0x02, STATS_TYPE_BANK1), \
 _fn(in_bad_accepted,  4, 0x03, STATS_TYPE_BANK1), \
 _fn(in_good_avb_class_a, 4, 0x04, STATS_TYPE_BANK1), \
 _fn(in_good_avb_class_b, 4, 0x05, STATS_TYPE_BANK1), \
 _fn(in_bad_avb_class_a,  4, 0x06, STATS_TYPE_BANK1), \
 _fn(in_bad_avb_class_b,  4, 0x07, STATS_TYPE_BANK1), \
 _fn(tcam_counter_0,  4, 0x08, STATS_TYPE_BANK1), \
 _fn(tcam_counter_1,  4, 0x09, STATS_TYPE_BANK1), \
 _fn(tcam_counter_2,  4, 0x0a, STATS_TYPE_BANK1), \
 _fn(tcam_counter_3,  4, 0x0b, STATS_TYPE_BANK1), \
 _fn(in_da_unknown,  4, 0x0e, STATS_TYPE_BANK1), \
 _fn(in_management,  4, 0x0f, STATS_TYPE_BANK1), \
 _fn(out_queue_0,  4, 0x10, STATS_TYPE_BANK1), \
 _fn(out_queue_1,  4, 0x11, STATS_TYPE_BANK1), \
 _fn(out_queue_2,  4, 0x12, STATS_TYPE_BANK1), \
 _fn(out_queue_3,  4, 0x13, STATS_TYPE_BANK1), \
 _fn(out_queue_4,  4, 0x14, STATS_TYPE_BANK1), \
 _fn(out_queue_5,  4, 0x15, STATS_TYPE_BANK1), \
 _fn(out_queue_6,  4, 0x16, STATS_TYPE_BANK1), \
 _fn(out_queue_7,  4, 0x17, STATS_TYPE_BANK1), \
 _fn(out_cut_through,  4, 0x18, STATS_TYPE_BANK1), \
 _fn(out_octets_a,  4, 0x1a, STATS_TYPE_BANK1), \
 _fn(out_octets_b,  4, 0x1b, STATS_TYPE_BANK1), \
 _fn(out_management,  4, 0x1f, STATS_TYPE_BANK1), \
 /*  */

#define MV88E6XXX_HW_STAT_ENTRY(_string, _size, _reg, _type) \
 { #_string, _size, _reg, _type }
static const struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
 MV88E6XXX_HW_STAT_MAPPER(MV88E6XXX_HW_STAT_ENTRY)
};

#define MV88E6XXX_HW_STAT_ENUM(_string, _size, _reg, _type) \
 MV88E6XXX_HW_STAT_ID_ ## _string
enum mv88e6xxx_hw_stat_id {
 MV88E6XXX_HW_STAT_MAPPER(MV88E6XXX_HW_STAT_ENUM)
};

static uint64_t _mv88e6xxx_get_ethtool_stat(struct mv88e6xxx_chip *chip,
         const struct mv88e6xxx_hw_stat *s,
         int port, u16 bank1_select,
         u16 histogram)
{
 u32 low;
 u32 high = 0;
 u16 reg = 0;
 int err;
 u64 value;

 switch (s->type) {
 case STATS_TYPE_PORT:
  err = mv88e6xxx_port_read(chip, port, s->reg, ®);
  if (err)
   return U64_MAX;

  low = reg;
  if (s->size == 4) {
   err = mv88e6xxx_port_read(chip, port, s->reg + 1, ®);
   if (err)
    return U64_MAX;
   low |= ((u32)reg) << 16;
  }
  break;
 case STATS_TYPE_BANK1:
  reg = bank1_select;
  fallthrough;
 case STATS_TYPE_BANK0:
  reg |= s->reg | histogram;
  mv88e6xxx_g1_stats_read(chip, reg, &low);
  if (s->size == 8)
   mv88e6xxx_g1_stats_read(chip, reg + 1, &high);
  break;
 default:
  return U64_MAX;
 }
 value = (((u64)high) << 32) | low;
 return value;
}

static void mv88e6xxx_stats_get_strings(struct mv88e6xxx_chip *chip,
     uint8_t **data, int types)
{
 const struct mv88e6xxx_hw_stat *stat;
 int i;

 for (i = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
  stat = &mv88e6xxx_hw_stats[i];
  if (stat->type & types)
   ethtool_puts(data, stat->string);
 }
}

static void mv88e6095_stats_get_strings(struct mv88e6xxx_chip *chip,
     uint8_t **data)
{
 mv88e6xxx_stats_get_strings(chip, data,
        STATS_TYPE_BANK0 | STATS_TYPE_PORT);
}

static void mv88e6250_stats_get_strings(struct mv88e6xxx_chip *chip,
     uint8_t **data)
{
 mv88e6xxx_stats_get_strings(chip, data, STATS_TYPE_BANK0);
}

static void mv88e6320_stats_get_strings(struct mv88e6xxx_chip *chip,
     uint8_t **data)
{
 mv88e6xxx_stats_get_strings(chip, data,
        STATS_TYPE_BANK0 | STATS_TYPE_BANK1);
}

static const uint8_t *mv88e6xxx_atu_vtu_stats_strings[] = {
 "atu_member_violation",
 "atu_miss_violation",
 "atu_full_violation",
 "vtu_member_violation",
 "vtu_miss_violation",
};

static void mv88e6xxx_atu_vtu_get_strings(uint8_t **data)
{
 unsigned int i;

 for (i = 0; i < ARRAY_SIZE(mv88e6xxx_atu_vtu_stats_strings); i++)
  ethtool_puts(data, mv88e6xxx_atu_vtu_stats_strings[i]);
}

static void mv88e6xxx_get_strings(struct dsa_switch *ds, int port,
      u32 stringset, uint8_t *data)
{
 struct mv88e6xxx_chip *chip = ds->priv;

 if (stringset != ETH_SS_STATS)
  return;

 mv88e6xxx_reg_lock(chip);

 if (chip->info->ops->stats_get_strings)
  chip->info->ops->stats_get_strings(chip, &data);

 if (chip->info->ops->serdes_get_strings)
  chip->info->ops->serdes_get_strings(chip, port, &data);

 mv88e6xxx_atu_vtu_get_strings(&data);

 mv88e6xxx_reg_unlock(chip);
}

static int mv88e6xxx_stats_get_sset_count(struct mv88e6xxx_chip *chip,
       int types)
{
 const struct mv88e6xxx_hw_stat *stat;
 int i, j;

 for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
  stat = &mv88e6xxx_hw_stats[i];
  if (stat->type & types)
   j++;
 }
 return j;
}

static int mv88e6095_stats_get_sset_count(struct mv88e6xxx_chip *chip)
{
 return mv88e6xxx_stats_get_sset_count(chip, STATS_TYPE_BANK0 |
           STATS_TYPE_PORT);
}

static int mv88e6250_stats_get_sset_count(struct mv88e6xxx_chip *chip)
{
 return mv88e6xxx_stats_get_sset_count(chip, STATS_TYPE_BANK0);
}

static int mv88e6320_stats_get_sset_count(struct mv88e6xxx_chip *chip)
{
 return mv88e6xxx_stats_get_sset_count(chip, STATS_TYPE_BANK0 |
           STATS_TYPE_BANK1);
}

static int mv88e6xxx_get_sset_count(struct dsa_switch *ds, int port, int sset)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int serdes_count = 0;
 int count = 0;

 if (sset != ETH_SS_STATS)
  return 0;

 mv88e6xxx_reg_lock(chip);
 if (chip->info->ops->stats_get_sset_count)
  count = chip->info->ops->stats_get_sset_count(chip);
 if (count < 0)
  goto out;

 if (chip->info->ops->serdes_get_sset_count)
  serdes_count = chip->info->ops->serdes_get_sset_count(chip,
              port);
 if (serdes_count < 0) {
  count = serdes_count;
  goto out;
 }
 count += serdes_count;
 count += ARRAY_SIZE(mv88e6xxx_atu_vtu_stats_strings);

out:
 mv88e6xxx_reg_unlock(chip);

 return count;
}

static size_t mv88e6095_stats_get_stat(struct mv88e6xxx_chip *chip, int port,
           const struct mv88e6xxx_hw_stat *stat,
           uint64_t *data)
{
 *data = _mv88e6xxx_get_ethtool_stat(chip, stat, port, 0,
         MV88E6XXX_G1_STATS_OP_HIST_RX);
 return 1;
}

static size_t mv88e6250_stats_get_stat(struct mv88e6xxx_chip *chip, int port,
           const struct mv88e6xxx_hw_stat *stat,
           uint64_t *data)
{
 *data = _mv88e6xxx_get_ethtool_stat(chip, stat, port, 0,
         MV88E6XXX_G1_STATS_OP_HIST_RX);
 return 1;
}

static size_t mv88e6320_stats_get_stat(struct mv88e6xxx_chip *chip, int port,
           const struct mv88e6xxx_hw_stat *stat,
           uint64_t *data)
{
 *data = _mv88e6xxx_get_ethtool_stat(chip, stat, port,
         MV88E6XXX_G1_STATS_OP_BANK_1_BIT_9,
         MV88E6XXX_G1_STATS_OP_HIST_RX);
 return 1;
}

static size_t mv88e6390_stats_get_stat(struct mv88e6xxx_chip *chip, int port,
           const struct mv88e6xxx_hw_stat *stat,
           uint64_t *data)
{
 *data = _mv88e6xxx_get_ethtool_stat(chip, stat, port,
         MV88E6XXX_G1_STATS_OP_BANK_1_BIT_10,
         0);
 return 1;
}

static size_t mv88e6xxx_stats_get_stat(struct mv88e6xxx_chip *chip, int port,
           const struct mv88e6xxx_hw_stat *stat,
           uint64_t *data)
{
 int ret = 0;

 if (!(stat->type & chip->info->stats_type))
  return 0;

 if (chip->info->ops->stats_get_stat) {
  mv88e6xxx_reg_lock(chip);
  ret = chip->info->ops->stats_get_stat(chip, port, stat, data);
  mv88e6xxx_reg_unlock(chip);
 }

 return ret;
}

static size_t mv88e6xxx_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
     uint64_t *data)
{
 const struct mv88e6xxx_hw_stat *stat;
 size_t i, j;

 for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
  stat = &mv88e6xxx_hw_stats[i];
  j += mv88e6xxx_stats_get_stat(chip, port, stat, &data[j]);
 }
 return j;
}

static void mv88e6xxx_atu_vtu_get_stats(struct mv88e6xxx_chip *chip, int port,
     uint64_t *data)
{
 *data++ = chip->ports[port].atu_member_violation;
 *data++ = chip->ports[port].atu_miss_violation;
 *data++ = chip->ports[port].atu_full_violation;
 *data++ = chip->ports[port].vtu_member_violation;
 *data++ = chip->ports[port].vtu_miss_violation;
}

static void mv88e6xxx_get_stats(struct mv88e6xxx_chip *chip, int port,
    uint64_t *data)
{
 size_t count;

 count = mv88e6xxx_stats_get_stats(chip, port, data);

 mv88e6xxx_reg_lock(chip);
 if (chip->info->ops->serdes_get_stats) {
  data += count;
  count = chip->info->ops->serdes_get_stats(chip, port, data);
 }
 data += count;
 mv88e6xxx_atu_vtu_get_stats(chip, port, data);
 mv88e6xxx_reg_unlock(chip);
}

static void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds, int port,
     uint64_t *data)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int ret;

 ret = mv88e6xxx_stats_snapshot(chip, port);
 if (ret < 0)
  return;

 mv88e6xxx_get_stats(chip, port, data);
}

static void mv88e6xxx_get_eth_mac_stats(struct dsa_switch *ds, int port,
     struct ethtool_eth_mac_stats *mac_stats)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int ret;

 ret = mv88e6xxx_stats_snapshot(chip, port);
 if (ret < 0)
  return;

#define MV88E6XXX_ETH_MAC_STAT_MAP(_id, _member)   \
 mv88e6xxx_stats_get_stat(chip, port,    \
     &mv88e6xxx_hw_stats[MV88E6XXX_HW_STAT_ID_ ## _id], \
     &mac_stats->stats._member)

 MV88E6XXX_ETH_MAC_STAT_MAP(out_unicast, FramesTransmittedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(single, SingleCollisionFrames);
 MV88E6XXX_ETH_MAC_STAT_MAP(multiple, MultipleCollisionFrames);
 MV88E6XXX_ETH_MAC_STAT_MAP(in_unicast, FramesReceivedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(in_fcs_error, FrameCheckSequenceErrors);
 MV88E6XXX_ETH_MAC_STAT_MAP(out_octets, OctetsTransmittedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(deferred, FramesWithDeferredXmissions);
 MV88E6XXX_ETH_MAC_STAT_MAP(late, LateCollisions);
 MV88E6XXX_ETH_MAC_STAT_MAP(in_good_octets, OctetsReceivedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(out_multicasts, MulticastFramesXmittedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(out_broadcasts, BroadcastFramesXmittedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(excessive, FramesWithExcessiveDeferral);
 MV88E6XXX_ETH_MAC_STAT_MAP(in_multicasts, MulticastFramesReceivedOK);
 MV88E6XXX_ETH_MAC_STAT_MAP(in_broadcasts, BroadcastFramesReceivedOK);

#undef MV88E6XXX_ETH_MAC_STAT_MAP

 mac_stats->stats.FramesTransmittedOK += mac_stats->stats.MulticastFramesXmittedOK;
 mac_stats->stats.FramesTransmittedOK += mac_stats->stats.BroadcastFramesXmittedOK;
 mac_stats->stats.FramesReceivedOK += mac_stats->stats.MulticastFramesReceivedOK;
 mac_stats->stats.FramesReceivedOK += mac_stats->stats.BroadcastFramesReceivedOK;
}

static void mv88e6xxx_get_rmon_stats(struct dsa_switch *ds, int port,
         struct ethtool_rmon_stats *rmon_stats,
         const struct ethtool_rmon_hist_range **ranges)
{
 static const struct ethtool_rmon_hist_range rmon_ranges[] = {
  {   64,    64 },
  {   65,   127 },
  {  128,   255 },
  {  256,   511 },
  {  512,  1023 },
  { 1024, 65535 },
  {}
 };
 struct mv88e6xxx_chip *chip = ds->priv;
 int ret;

 ret = mv88e6xxx_stats_snapshot(chip, port);
 if (ret < 0)
  return;

#define MV88E6XXX_RMON_STAT_MAP(_id, _member)    \
 mv88e6xxx_stats_get_stat(chip, port,    \
     &mv88e6xxx_hw_stats[MV88E6XXX_HW_STAT_ID_ ## _id], \
     &rmon_stats->stats._member)

 MV88E6XXX_RMON_STAT_MAP(in_undersize, undersize_pkts);
 MV88E6XXX_RMON_STAT_MAP(in_oversize, oversize_pkts);
 MV88E6XXX_RMON_STAT_MAP(in_fragments, fragments);
 MV88E6XXX_RMON_STAT_MAP(in_jabber, jabbers);
 MV88E6XXX_RMON_STAT_MAP(hist_64bytes, hist[0]);
 MV88E6XXX_RMON_STAT_MAP(hist_65_127bytes, hist[1]);
 MV88E6XXX_RMON_STAT_MAP(hist_128_255bytes, hist[2]);
 MV88E6XXX_RMON_STAT_MAP(hist_256_511bytes, hist[3]);
 MV88E6XXX_RMON_STAT_MAP(hist_512_1023bytes, hist[4]);
 MV88E6XXX_RMON_STAT_MAP(hist_1024_max_bytes, hist[5]);

#undef MV88E6XXX_RMON_STAT_MAP

 *ranges = rmon_ranges;
}

static int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int len;

 len = 32 * sizeof(u16);
 if (chip->info->ops->serdes_get_regs_len)
  len += chip->info->ops->serdes_get_regs_len(chip, port);

 return len;
}

static void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
          struct ethtool_regs *regs, void *_p)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int err;
 u16 reg;
 u16 *p = _p;
 int i;

 regs->version = chip->info->prod_num;

 memset(p, 0xff, 32 * sizeof(u16));

 mv88e6xxx_reg_lock(chip);

 for (i = 0; i < 32; i++) {

  err = mv88e6xxx_port_read(chip, port, i, ®);
  if (!err)
   p[i] = reg;
 }

 if (chip->info->ops->serdes_get_regs)
  chip->info->ops->serdes_get_regs(chip, port, &p[i]);

 mv88e6xxx_reg_unlock(chip);
}

static int mv88e6xxx_set_mac_eee(struct dsa_switch *ds, int port,
     struct ethtool_keee *e)
{
 /* Nothing to do on the port's MAC */
 return 0;
}

/* Mask of the local ports allowed to receive frames from a given fabric port */
static u16 mv88e6xxx_port_vlan(struct mv88e6xxx_chip *chip, int dev, int port)
{
 struct dsa_switch *ds = chip->ds;
 struct dsa_switch_tree *dst = ds->dst;
 struct dsa_port *dp, *other_dp;
 bool found = false;
 u16 pvlan;

 /* dev is a physical switch */
 if (dev <= dst->last_switch) {
  list_for_each_entry(dp, &dst->ports, list) {
   if (dp->ds->index == dev && dp->index == port) {
    /* dp might be a DSA link or a user port, so it
 * might or might not have a bridge.
 * Use the "found" variable for both cases.
 */
    found = true;
    break;
   }
  }
 /* dev is a virtual bridge */
 } else {
  list_for_each_entry(dp, &dst->ports, list) {
   unsigned int bridge_num = dsa_port_bridge_num_get(dp);

   if (!bridge_num)
    continue;

   if (bridge_num + dst->last_switch != dev)
    continue;

   found = true;
   break;
  }
 }

 /* Prevent frames from unknown switch or virtual bridge */
 if (!found)
  return 0;

 /* Frames from DSA links and CPU ports can egress any local port */
 if (dp->type == DSA_PORT_TYPE_CPU || dp->type == DSA_PORT_TYPE_DSA)
  return mv88e6xxx_port_mask(chip);

 pvlan = 0;

 /* Frames from standalone user ports can only egress on the
 * upstream port.
 */
 if (!dsa_port_bridge_dev_get(dp))
  return BIT(dsa_switch_upstream_port(ds));

 /* Frames from bridged user ports can egress any local DSA
 * links and CPU ports, as well as any local member of their
 * bridge group.
 */
 dsa_switch_for_each_port(other_dp, ds)
  if (other_dp->type == DSA_PORT_TYPE_CPU ||
      other_dp->type == DSA_PORT_TYPE_DSA ||
      dsa_port_bridge_same(dp, other_dp))
   pvlan |= BIT(other_dp->index);

 return pvlan;
}

static int mv88e6xxx_port_vlan_map(struct mv88e6xxx_chip *chip, int port)
{
 u16 output_ports = mv88e6xxx_port_vlan(chip, chip->ds->index, port);

 /* prevent frames from going back out of the port they came in on */
 output_ports &= ~BIT(port);

 return mv88e6xxx_port_set_vlan_map(chip, port, output_ports);
}

static void mv88e6xxx_port_stp_state_set(struct dsa_switch *ds, int port,
      u8 state)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int err;

 mv88e6xxx_reg_lock(chip);
 err = mv88e6xxx_port_set_state(chip, port, state);
 mv88e6xxx_reg_unlock(chip);

 if (err)
  dev_err(ds->dev, "p%d: failed to update state\n", port);
}

static int mv88e6xxx_pri_setup(struct mv88e6xxx_chip *chip)
{
 int err;

 if (chip->info->ops->ieee_pri_map) {
  err = chip->info->ops->ieee_pri_map(chip);
  if (err)
   return err;
 }

 if (chip->info->ops->ip_pri_map) {
  err = chip->info->ops->ip_pri_map(chip);
  if (err)
   return err;
 }

 return 0;
}

static int mv88e6xxx_devmap_setup(struct mv88e6xxx_chip *chip)
{
 struct dsa_switch *ds = chip->ds;
 int target, port;
 int err;

 if (!chip->info->global2_addr)
  return 0;

 /* Initialize the routing port to the 32 possible target devices */
 for (target = 0; target < 32; target++) {
  port = dsa_routing_port(ds, target);
  if (port == ds->num_ports)
   port = 0x1f;

  err = mv88e6xxx_g2_device_mapping_write(chip, target, port);
  if (err)
   return err;
 }

 if (chip->info->ops->set_cascade_port) {
  port = MV88E6XXX_CASCADE_PORT_MULTIPLE;
  err = chip->info->ops->set_cascade_port(chip, port);
  if (err)
   return err;
 }

 err = mv88e6xxx_g1_set_device_number(chip, chip->ds->index);
 if (err)
  return err;

 return 0;
}

static int mv88e6xxx_trunk_setup(struct mv88e6xxx_chip *chip)
{
 /* Clear all trunk masks and mapping */
 if (chip->info->global2_addr)
  return mv88e6xxx_g2_trunk_clear(chip);

 return 0;
}

static int mv88e6xxx_rmu_setup(struct mv88e6xxx_chip *chip)
{
 if (chip->info->ops->rmu_disable)
  return chip->info->ops->rmu_disable(chip);

 return 0;
}

static int mv88e6xxx_pot_setup(struct mv88e6xxx_chip *chip)
{
 if (chip->info->ops->pot_clear)
  return chip->info->ops->pot_clear(chip);

 return 0;
}

static int mv88e6xxx_rsvd2cpu_setup(struct mv88e6xxx_chip *chip)
{
 if (chip->info->ops->mgmt_rsvd2cpu)
  return chip->info->ops->mgmt_rsvd2cpu(chip);

 return 0;
}

static int mv88e6xxx_atu_setup(struct mv88e6xxx_chip *chip)
{
 int err;

 err = mv88e6xxx_g1_atu_flush(chip, 0, true);
 if (err)
  return err;

 /* The chips that have a "learn2all" bit in Global1, ATU
 * Control are precisely those whose port registers have a
 * Message Port bit in Port Control 1 and hence implement
 * ->port_setup_message_port.
 */
 if (chip->info->ops->port_setup_message_port) {
  err = mv88e6xxx_g1_atu_set_learn2all(chip, true);
  if (err)
   return err;
 }

 return mv88e6xxx_g1_atu_set_age_time(chip, 300000);
}

static int mv88e6xxx_irl_setup(struct mv88e6xxx_chip *chip)
{
 int port;
 int err;

 if (!chip->info->ops->irl_init_all)
  return 0;

 for (port = 0; port < mv88e6xxx_num_ports(chip); port++) {
  /* Disable ingress rate limiting by resetting all per port
 * ingress rate limit resources to their initial state.
 */
  err = chip->info->ops->irl_init_all(chip, port);
  if (err)
   return err;
 }

 return 0;
}

static int mv88e6xxx_mac_setup(struct mv88e6xxx_chip *chip)
{
 if (chip->info->ops->set_switch_mac) {
  u8 addr[ETH_ALEN];

  eth_random_addr(addr);

  return chip->info->ops->set_switch_mac(chip, addr);
 }

 return 0;
}

static int mv88e6xxx_pvt_map(struct mv88e6xxx_chip *chip, int dev, int port)
{
 struct dsa_switch_tree *dst = chip->ds->dst;
 struct dsa_switch *ds;
 struct dsa_port *dp;
 u16 pvlan = 0;

 if (!mv88e6xxx_has_pvt(chip))
  return 0;

 /* Skip the local source device, which uses in-chip port VLAN */
 if (dev != chip->ds->index) {
  pvlan = mv88e6xxx_port_vlan(chip, dev, port);

  ds = dsa_switch_find(dst->index, dev);
  dp = ds ? dsa_to_port(ds, port) : NULL;
  if (dp && dp->lag) {
   /* As the PVT is used to limit flooding of
 * FORWARD frames, which use the LAG ID as the
 * source port, we must translate dev/port to
 * the special "LAG device" in the PVT, using
 * the LAG ID (one-based) as the port number
 * (zero-based).
 */
   dev = MV88E6XXX_G2_PVT_ADDR_DEV_TRUNK;
   port = dsa_port_lag_id_get(dp) - 1;
  }
 }

 return mv88e6xxx_g2_pvt_write(chip, dev, port, pvlan);
}

static int mv88e6xxx_pvt_setup(struct mv88e6xxx_chip *chip)
{
 int dev, port;
 int err;

 if (!mv88e6xxx_has_pvt(chip))
  return 0;

 /* Clear 5 Bit Port for usage with Marvell Link Street devices:
 * use 4 bits for the Src_Port/Src_Trunk and 5 bits for the Src_Dev.
 */
 err = mv88e6xxx_g2_misc_4_bit_port(chip);
 if (err)
  return err;

 for (dev = 0; dev < MV88E6XXX_MAX_PVT_SWITCHES; ++dev) {
  for (port = 0; port < MV88E6XXX_MAX_PVT_PORTS; ++port) {
   err = mv88e6xxx_pvt_map(chip, dev, port);
   if (err)
    return err;
  }
 }

 return 0;
}

static int mv88e6xxx_port_fast_age_fid(struct mv88e6xxx_chip *chip, int port,
           u16 fid)
{
 if (dsa_to_port(chip->ds, port)->lag)
  /* Hardware is incapable of fast-aging a LAG through a
 * regular ATU move operation. Until we have something
 * more fancy in place this is a no-op.
 */
  return -EOPNOTSUPP;

 return mv88e6xxx_g1_atu_remove(chip, fid, port, false);
}

static void mv88e6xxx_port_fast_age(struct dsa_switch *ds, int port)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int err;

 mv88e6xxx_reg_lock(chip);
 err = mv88e6xxx_port_fast_age_fid(chip, port, 0);
 mv88e6xxx_reg_unlock(chip);

 if (err)
  dev_err(chip->ds->dev, "p%d: failed to flush ATU: %d\n",
   port, err);
}

static int mv88e6xxx_vtu_setup(struct mv88e6xxx_chip *chip)
{
 if (!mv88e6xxx_max_vid(chip))
  return 0;

 return mv88e6xxx_g1_vtu_flush(chip);
}

static int mv88e6xxx_vtu_get(struct mv88e6xxx_chip *chip, u16 vid,
        struct mv88e6xxx_vtu_entry *entry)
{
 int err;

 if (!chip->info->ops->vtu_getnext)
  return -EOPNOTSUPP;

 memset(entry, 0, sizeof(*entry));

 entry->vid = vid ? vid - 1 : mv88e6xxx_max_vid(chip);
 entry->valid = false;

 err = chip->info->ops->vtu_getnext(chip, entry);

 if (entry->vid != vid)
  entry->valid = false;

 return err;
}

int mv88e6xxx_vtu_walk(struct mv88e6xxx_chip *chip,
         int (*cb)(struct mv88e6xxx_chip *chip,
     const struct mv88e6xxx_vtu_entry *entry,
     void *priv),
         void *priv)
{
 struct mv88e6xxx_vtu_entry entry = {
  .vid = mv88e6xxx_max_vid(chip),
  .valid = false,
 };
 int err;

 if (!chip->info->ops->vtu_getnext)
  return -EOPNOTSUPP;

 do {
  err = chip->info->ops->vtu_getnext(chip, &entry);
  if (err)
   return err;

  if (!entry.valid)
   break;

  err = cb(chip, &entry, priv);
  if (err)
   return err;
 } while (entry.vid < mv88e6xxx_max_vid(chip));

 return 0;
}

static int mv88e6xxx_vtu_loadpurge(struct mv88e6xxx_chip *chip,
       struct mv88e6xxx_vtu_entry *entry)
{
 if (!chip->info->ops->vtu_loadpurge)
  return -EOPNOTSUPP;

 return chip->info->ops->vtu_loadpurge(chip, entry);
}

static int mv88e6xxx_atu_new(struct mv88e6xxx_chip *chip, u16 *fid)
{
 *fid = find_first_zero_bit(chip->fid_bitmap, MV88E6XXX_N_FID);
 if (unlikely(*fid >= mv88e6xxx_num_databases(chip)))
  return -ENOSPC;

 /* Clear the database */
 return mv88e6xxx_g1_atu_flush(chip, *fid, true);
}

static int mv88e6xxx_stu_loadpurge(struct mv88e6xxx_chip *chip,
       struct mv88e6xxx_stu_entry *entry)
{
 if (!chip->info->ops->stu_loadpurge)
  return -EOPNOTSUPP;

 return chip->info->ops->stu_loadpurge(chip, entry);
}

static int mv88e6xxx_stu_setup(struct mv88e6xxx_chip *chip)
{
 struct mv88e6xxx_stu_entry stu = {
  .valid = true,
  .sid = 0
 };

 if (!mv88e6xxx_has_stu(chip))
  return 0;

 /* Make sure that SID 0 is always valid. This is used by VTU
 * entries that do not make use of the STU, e.g. when creating
 * a VLAN upper on a port that is also part of a VLAN
 * filtering bridge.
 */
 return mv88e6xxx_stu_loadpurge(chip, &stu);
}

static int mv88e6xxx_sid_get(struct mv88e6xxx_chip *chip, u8 *sid)
{
 DECLARE_BITMAP(busy, MV88E6XXX_N_SID) = { 0 };
 struct mv88e6xxx_mst *mst;

 __set_bit(0, busy);

 list_for_each_entry(mst, &chip->msts, node)
  __set_bit(mst->stu.sid, busy);

 *sid = find_first_zero_bit(busy, MV88E6XXX_N_SID);

 return (*sid >= mv88e6xxx_max_sid(chip)) ? -ENOSPC : 0;
}

static int mv88e6xxx_mst_put(struct mv88e6xxx_chip *chip, u8 sid)
{
 struct mv88e6xxx_mst *mst, *tmp;
 int err;

 /* If the SID is zero, it is for a VLAN mapped to the default MSTI,
 * and mv88e6xxx_stu_setup() made sure it is always present, and thus,
 * should not be removed here.
 *
 * If the chip lacks STU support, numerically the "sid" variable will
 * happen to also be zero, but we don't want to rely on that fact, so
 * we explicitly test that first. In that case, there is also nothing
 * to do here.
 */
 if (!mv88e6xxx_has_stu(chip) || !sid)
  return 0;

 list_for_each_entry_safe(mst, tmp, &chip->msts, node) {
  if (mst->stu.sid != sid)
   continue;

  if (!refcount_dec_and_test(&mst->refcnt))
   return 0;

  mst->stu.valid = false;
  err = mv88e6xxx_stu_loadpurge(chip, &mst->stu);
  if (err) {
   refcount_set(&mst->refcnt, 1);
   return err;
  }

  list_del(&mst->node);
  kfree(mst);
  return 0;
 }

 return -ENOENT;
}

static int mv88e6xxx_mst_get(struct mv88e6xxx_chip *chip, struct net_device *br,
        u16 msti, u8 *sid)
{
 struct mv88e6xxx_mst *mst;
 int err, i;

 if (!mv88e6xxx_has_stu(chip)) {
  err = -EOPNOTSUPP;
  goto err;
 }

 if (!msti) {
  *sid = 0;
  return 0;
 }

 list_for_each_entry(mst, &chip->msts, node) {
  if (mst->br == br && mst->msti == msti) {
   refcount_inc(&mst->refcnt);
   *sid = mst->stu.sid;
   return 0;
  }
 }

 err = mv88e6xxx_sid_get(chip, sid);
 if (err)
  goto err;

 mst = kzalloc(sizeof(*mst), GFP_KERNEL);
 if (!mst) {
  err = -ENOMEM;
  goto err;
 }

 INIT_LIST_HEAD(&mst->node);
 refcount_set(&mst->refcnt, 1);
 mst->br = br;
 mst->msti = msti;
 mst->stu.valid = true;
 mst->stu.sid = *sid;

 /* The bridge starts out all ports in the disabled state. But
 * a STU state of disabled means to go by the port-global
 * state. So we set all user port's initial state to blocking,
 * to match the bridge's behavior.
 */
 for (i = 0; i < mv88e6xxx_num_ports(chip); i++)
  mst->stu.state[i] = dsa_is_user_port(chip->ds, i) ?
   MV88E6XXX_PORT_CTL0_STATE_BLOCKING :
   MV88E6XXX_PORT_CTL0_STATE_DISABLED;

 err = mv88e6xxx_stu_loadpurge(chip, &mst->stu);
 if (err)
  goto err_free;

 list_add_tail(&mst->node, &chip->msts);
 return 0;

err_free:
 kfree(mst);
err:
 return err;
}

static int mv88e6xxx_port_mst_state_set(struct dsa_switch *ds, int port,
     const struct switchdev_mst_state *st)
{
 struct dsa_port *dp = dsa_to_port(ds, port);
 struct mv88e6xxx_chip *chip = ds->priv;
 struct mv88e6xxx_mst *mst;
 u8 state;
 int err;

 if (!mv88e6xxx_has_stu(chip))
  return -EOPNOTSUPP;

 switch (st->state) {
 case BR_STATE_DISABLED:
 case BR_STATE_BLOCKING:
 case BR_STATE_LISTENING:
  state = MV88E6XXX_PORT_CTL0_STATE_BLOCKING;
  break;
 case BR_STATE_LEARNING:
  state = MV88E6XXX_PORT_CTL0_STATE_LEARNING;
  break;
 case BR_STATE_FORWARDING:
  state = MV88E6XXX_PORT_CTL0_STATE_FORWARDING;
  break;
 default:
  return -EINVAL;
 }

 list_for_each_entry(mst, &chip->msts, node) {
  if (mst->br == dsa_port_bridge_dev_get(dp) &&
      mst->msti == st->msti) {
   if (mst->stu.state[port] == state)
    return 0;

   mst->stu.state[port] = state;
   mv88e6xxx_reg_lock(chip);
   err = mv88e6xxx_stu_loadpurge(chip, &mst->stu);
   mv88e6xxx_reg_unlock(chip);
   return err;
  }
 }

 return -ENOENT;
}

static int mv88e6xxx_port_check_hw_vlan(struct dsa_switch *ds, int port,
     u16 vid)
{
 struct dsa_port *dp = dsa_to_port(ds, port), *other_dp;
 struct mv88e6xxx_chip *chip = ds->priv;
 struct mv88e6xxx_vtu_entry vlan;
 int err;

 /* DSA and CPU ports have to be members of multiple vlans */
 if (dsa_port_is_dsa(dp) || dsa_port_is_cpu(dp))
  return 0;

 err = mv88e6xxx_vtu_get(chip, vid, &vlan);
 if (err)
  return err;

 if (!vlan.valid)
  return 0;

 dsa_switch_for_each_user_port(other_dp, ds) {
  struct net_device *other_br;

  if (vlan.member[other_dp->index] ==
      MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER)
   continue;

  if (dsa_port_bridge_same(dp, other_dp))
   break; /* same bridge, check next VLAN */

  other_br = dsa_port_bridge_dev_get(other_dp);
  if (!other_br)
   continue;

  dev_err(ds->dev, "p%d: hw VLAN %d already used by port %d in %s\n",
   port, vlan.vid, other_dp->index, netdev_name(other_br));
  return -EOPNOTSUPP;
 }

 return 0;
}

static int mv88e6xxx_port_commit_pvid(struct mv88e6xxx_chip *chip, int port)
{
 struct dsa_port *dp = dsa_to_port(chip->ds, port);
 struct net_device *br = dsa_port_bridge_dev_get(dp);
 struct mv88e6xxx_port *p = &chip->ports[port];
 u16 pvid = MV88E6XXX_VID_STANDALONE;
 bool drop_untagged = false;
 int err;

 if (br) {
  if (br_vlan_enabled(br)) {
   pvid = p->bridge_pvid.vid;
   drop_untagged = !p->bridge_pvid.valid;
  } else {
   pvid = MV88E6XXX_VID_BRIDGED;
  }
 }

 err = mv88e6xxx_port_set_pvid(chip, port, pvid);
 if (err)
  return err;

 return mv88e6xxx_port_drop_untagged(chip, port, drop_untagged);
}

static int mv88e6xxx_port_vlan_filtering(struct dsa_switch *ds, int port,
      bool vlan_filtering,
      struct netlink_ext_ack *extack)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 u16 mode = vlan_filtering ? MV88E6XXX_PORT_CTL2_8021Q_MODE_SECURE :
  MV88E6XXX_PORT_CTL2_8021Q_MODE_DISABLED;
 int err;

 if (!mv88e6xxx_max_vid(chip))
  return -EOPNOTSUPP;

 mv88e6xxx_reg_lock(chip);

 err = mv88e6xxx_port_set_8021q_mode(chip, port, mode);
 if (err)
  goto unlock;

 err = mv88e6xxx_port_commit_pvid(chip, port);
 if (err)
  goto unlock;

unlock:
 mv88e6xxx_reg_unlock(chip);

 return err;
}

static int
mv88e6xxx_port_vlan_prepare(struct dsa_switch *ds, int port,
       const struct switchdev_obj_port_vlan *vlan)
{
 struct mv88e6xxx_chip *chip = ds->priv;
 int err;

 if (!mv88e6xxx_max_vid(chip))
  return -EOPNOTSUPP;

 /* If the requested port doesn't belong to the same bridge as the VLAN
 * members, do not support it (yet) and fallback to software VLAN.
 */
 mv88e6xxx_reg_lock(chip);
 err = mv88e6xxx_port_check_hw_vlan(ds, port, vlan->vid);
 mv88e6xxx_reg_unlock(chip);

 return err;
}

static int mv88e6xxx_port_db_get(struct mv88e6xxx_chip *chip,
     const unsigned char *addr, u16 vid,
     u16 *fid, struct mv88e6xxx_atu_entry *entry)
{
 struct mv88e6xxx_vtu_entry vlan;
 int err;

 /* Ports have two private address databases: one for when the port is
 * standalone and one for when the port is under a bridge and the
 * 802.1Q mode is disabled. When the port is standalone, DSA wants its
 * address database to remain 100% empty, so we never load an ATU entry
 * into a standalone port's database. Therefore, translate the null
 * VLAN ID into the port's database used for VLAN-unaware bridging.
 */
 if (vid == 0) {
  *fid = MV88E6XXX_FID_BRIDGED;
 } else {
  err = mv88e6xxx_vtu_get(chip, vid, &vlan);
  if (err)
   return err;

  /* switchdev expects -EOPNOTSUPP to honor software VLANs */
  if (!vlan.valid)
   return -EOPNOTSUPP;

  *fid = vlan.fid;
 }

 entry->state = 0;
 ether_addr_copy(entry->mac, addr);
 eth_addr_dec(entry->mac);

 return mv88e6xxx_g1_atu_getnext(chip, *fid, entry);
}

static bool mv88e6xxx_port_db_find(struct mv88e6xxx_chip *chip,
       const unsigned char *addr, u16 vid)
{
 struct mv88e6xxx_atu_entry entry;
 u16 fid;
 int err;

 err = mv88e6xxx_port_db_get(chip, addr, vid, &fid, &entry);
 if (err)
  return false;

 return entry.state && ether_addr_equal(entry.mac, addr);
}

static int mv88e6xxx_port_db_load_purge(struct mv88e6xxx_chip *chip, int port,
     const unsigned char *addr, u16 vid,
     u8 state)
{
 struct mv88e6xxx_atu_entry entry;
 u16 fid;
 int err;

 err = mv88e6xxx_port_db_get(chip, addr, vid, &fid, &entry);
 if (err)
  return err;

 /* Initialize a fresh ATU entry if it isn't found */
 if (!entry.state || !ether_addr_equal(entry.mac, addr)) {
  memset(&entry, 0, sizeof(entry));
  ether_addr_copy(entry.mac, addr);
 }

 /* Purge the ATU entry only if no port is using it anymore */
 if (!state) {
  entry.portvec &= ~BIT(port);
  if (!entry.portvec)
   entry.state = 0;
 } else {
  if (state == MV88E6XXX_G1_ATU_DATA_STATE_UC_STATIC)
   entry.portvec = BIT(port);
  else
   entry.portvec |= BIT(port);

  entry.state = state;
 }

 return mv88e6xxx_g1_atu_loadpurge(chip, fid, &entry);
}

static int mv88e6xxx_policy_apply(struct mv88e6xxx_chip *chip, int port,
      const struct mv88e6xxx_policy *policy)
{
 enum mv88e6xxx_policy_mapping mapping = policy->mapping;
 enum mv88e6xxx_policy_action action = policy->action;
 const u8 *addr = policy->addr;
 u16 vid = policy->vid;
 u8 state;
 int err;
 int id;

 if (!chip->info->ops->port_set_policy)
  return -EOPNOTSUPP;

 switch (mapping) {
 case MV88E6XXX_POLICY_MAPPING_DA:
 case MV88E6XXX_POLICY_MAPPING_SA:
  if (action == MV88E6XXX_POLICY_ACTION_NORMAL)
   state = 0; /* Dissociate the port and address */
  else if (action == MV88E6XXX_POLICY_ACTION_DISCARD &&
    is_multicast_ether_addr(addr))
   state = MV88E6XXX_G1_ATU_DATA_STATE_MC_STATIC_POLICY;
  else if (action == MV88E6XXX_POLICY_ACTION_DISCARD &&
    is_unicast_ether_addr(addr))
   state = MV88E6XXX_G1_ATU_DATA_STATE_UC_STATIC_POLICY;
  else
   return -EOPNOTSUPP;

  err = mv88e6xxx_port_db_load_purge(chip, port, addr, vid,
         state);
  if (err)
   return err;
  break;
 default:
  return -EOPNOTSUPP;
 }

 /* Skip the port's policy clearing if the mapping is still in use */
 if (action == MV88E6XXX_POLICY_ACTION_NORMAL)
  idr_for_each_entry(&chip->policies, policy, id)
   if (policy->port == port &&
       policy->mapping == mapping &&
       policy->action != action)
    return 0;

 return chip->info->ops->port_set_policy(chip, port, mapping, action);
}

static int mv88e6xxx_policy_insert(struct mv88e6xxx_chip *chip, int port,
       struct ethtool_rx_flow_spec *fs)
{
 struct ethhdr *mac_entry = &fs->h_u.ether_spec;
 struct ethhdr *mac_mask = &fs->m_u.ether_spec;
 enum mv88e6xxx_policy_mapping mapping;
 enum mv88e6xxx_policy_action action;
 struct mv88e6xxx_policy *policy;
 u16 vid = 0;
 u8 *addr;
 int err;
 int id;

 if (fs->location != RX_CLS_LOC_ANY)
  return -EINVAL;

 if (fs->ring_cookie == RX_CLS_FLOW_DISC)
  action = MV88E6XXX_POLICY_ACTION_DISCARD;
 else
  return -EOPNOTSUPP;

 switch (fs->flow_type & ~FLOW_EXT) {
 case ETHER_FLOW:
  if (!is_zero_ether_addr(mac_mask->h_dest) &&
      is_zero_ether_addr(mac_mask->h_source)) {
   mapping = MV88E6XXX_POLICY_MAPPING_DA;
   addr = mac_entry->h_dest;
  } else if (is_zero_ether_addr(mac_mask->h_dest) &&
      !is_zero_ether_addr(mac_mask->h_source)) {
   mapping = MV88E6XXX_POLICY_MAPPING_SA;
   addr = mac_entry->h_source;
  } else {
   /* Cannot support DA and SA mapping in the same rule */
   return -EOPNOTSUPP;
  }
  break;
 default:
  return -EOPNOTSUPP;
 }

 if ((fs->flow_type & FLOW_EXT) && fs->m_ext.vlan_tci) {
  if (fs->m_ext.vlan_tci != htons(0xffff))
   return -EOPNOTSUPP;
  vid = be16_to_cpu(fs->h_ext.vlan_tci) & VLAN_VID_MASK;
 }

 idr_for_each_entry(&chip->policies, policy, id) {
  if (policy->port == port && policy->mapping == mapping &&
      policy->action == action && policy->vid == vid &&
      ether_addr_equal(policy->addr, addr))
   return -EEXIST;
 }

 policy = devm_kzalloc(chip->dev, sizeof(*policy), GFP_KERNEL);
 if (!policy)
  return -ENOMEM;

 fs->location = 0;
 err = idr_alloc_u32(&chip->policies, policy, &fs->location, 0xffffffff,
       GFP_KERNEL);
 if (err) {
  devm_kfree(chip->dev, policy);
  return err;
 }

 memcpy(&policy->fs, fs, sizeof(*fs));
 ether_addr_copy(policy->addr, addr);
 policy->mapping = mapping;
 policy->action = action;
 policy->port = port;
 policy->vid = vid;

 err = mv88e6xxx_policy_apply(chip, port, policy);
 if (err) {
  idr_remove(&chip->policies, fs->location);
  devm_kfree(chip->dev, policy);
  return err;
 }

 return 0;
}

static int mv88e6xxx_get_rxnfc(struct dsa_switch *ds, int port,
          struct ethtool_rxnfc *rxnfc, u32 *rule_locs)
{
 struct ethtool_rx_flow_spec *fs = &rxnfc->fs;
 struct mv88e6xxx_chip *chip = ds->priv;
 struct mv88e6xxx_policy *policy;
 int err;
 int id;

 mv88e6xxx_reg_lock(chip);

 switch (rxnfc->cmd) {
 case ETHTOOL_GRXCLSRLCNT:
  rxnfc->data = 0;
  rxnfc->data |= RX_CLS_LOC_SPECIAL;
  rxnfc->rule_cnt = 0;
  idr_for_each_entry(&chip->policies, policy, id)
   if (policy->port == port)
    rxnfc->rule_cnt++;
  err = 0;
  break;
 case ETHTOOL_GRXCLSRULE:
  err = -ENOENT;
  policy = idr_find(&chip->policies, fs->location);
  if (policy) {
   memcpy(fs, &policy->fs, sizeof(*fs));
   err = 0;
  }
  break;
 case ETHTOOL_GRXCLSRLALL:
  rxnfc->data = 0;
  rxnfc->rule_cnt = 0;
  idr_for_each_entry(&chip->policies, policy, id)
   if (policy->port == port)
    rule_locs[rxnfc->rule_cnt++] = id;
  err = 0;
  break;
 default:
  err = -EOPNOTSUPP;
  break;
 }

 mv88e6xxx_reg_unlock(chip);

 return err;
}

static int mv88e6xxx_set_rxnfc(struct dsa_switch *ds, int port,
--> --------------------

--> maximum size reached

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