// 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/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_device.h>
#include <linux/of_irq.h>
#include <linux/of_mdio.h>
#include <linux/platform_data/mv88e6xxx.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)
{
        u16 data;
        int err;
        int i;

        /* There's no bus specific operation to wait for a mask */
        for (i = 0; i < 16; i++) {
                err = mv88e6xxx_read(chip, addr, reg, &data);
                if (err)
                        return err;

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

                usleep_range(1000, 2000);
        }

        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(&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, &reg);
        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, &reg);
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, &reg);
        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_add_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, &reg);
        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_create_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 (speed == SPEED_MAX && chip->info->ops->port_max_speed_mode)
                mode = chip->info->ops->port_max_speed_mode(port);

        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 dsa_switch *ds, int port)
{
        struct mv88e6xxx_chip *chip = ds->priv;

        return port < chip->info->num_internal_phys;
}

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

        err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
        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 int mv88e6xxx_serdes_pcs_get_state(struct dsa_switch *ds, int port,
                                          struct phylink_link_state *state)
{
        struct mv88e6xxx_chip *chip = ds->priv;
        int lane;
        int err;

        mv88e6xxx_reg_lock(chip);
        lane = mv88e6xxx_serdes_get_lane(chip, port);
        if (lane >= 0 && chip->info->ops->serdes_pcs_get_state)
                err = chip->info->ops->serdes_pcs_get_state(chip, port, lane,
                                                            state);
        else
                err = -EOPNOTSUPP;
        mv88e6xxx_reg_unlock(chip);

        return err;
}

static int mv88e6xxx_serdes_pcs_config(struct mv88e6xxx_chip *chip, int port,
                                       unsigned int mode,
                                       phy_interface_t interface,
                                       const unsigned long *advertise)
{
        const struct mv88e6xxx_ops *ops = chip->info->ops;
        int lane;

        if (ops->serdes_pcs_config) {
                lane = mv88e6xxx_serdes_get_lane(chip, port);
                if (lane >= 0)
                        return ops->serdes_pcs_config(chip, port, lane, mode,
                                                      interface, advertise);
        }

        return 0;
}

static void mv88e6xxx_serdes_pcs_an_restart(struct dsa_switch *ds, int port)
{
        struct mv88e6xxx_chip *chip = ds->priv;
        const struct mv88e6xxx_ops *ops;
        int err = 0;
        int lane;

        ops = chip->info->ops;

        if (ops->serdes_pcs_an_restart) {
                mv88e6xxx_reg_lock(chip);
                lane = mv88e6xxx_serdes_get_lane(chip, port);
                if (lane >= 0)
                        err = ops->serdes_pcs_an_restart(chip, port, lane);
                mv88e6xxx_reg_unlock(chip);

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

static int mv88e6xxx_serdes_pcs_link_up(struct mv88e6xxx_chip *chip, int port,
                                        unsigned int mode,
                                        int speed, int duplex)
{
        const struct mv88e6xxx_ops *ops = chip->info->ops;
        int lane;

        if (!phylink_autoneg_inband(mode) && ops->serdes_pcs_link_up) {
                lane = mv88e6xxx_serdes_get_lane(chip, port);
                if (lane >= 0)
                        return ops->serdes_pcs_link_up(chip, port, lane,
                                                       speed, duplex);
        }

        return 0;
}

static void mv88e6065_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                       unsigned long *mask,
                                       struct phylink_link_state *state)
{
        if (!phy_interface_mode_is_8023z(state->interface)) {
                /* 10M and 100M are only supported in non-802.3z mode */
                phylink_set(mask, 10baseT_Half);
                phylink_set(mask, 10baseT_Full);
                phylink_set(mask, 100baseT_Half);
                phylink_set(mask, 100baseT_Full);
        }
}

static void mv88e6185_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                       unsigned long *mask,
                                       struct phylink_link_state *state)
{
        /* FIXME: if the port is in 1000Base-X mode, then it only supports
         * 1000M FD speeds.  In this case, CMODE will indicate 5.
         */
        phylink_set(mask, 1000baseT_Full);
        phylink_set(mask, 1000baseX_Full);

        mv88e6065_phylink_validate(chip, port, mask, state);
}

static void mv88e6341_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                       unsigned long *mask,
                                       struct phylink_link_state *state)
{
        if (port >= 5)
                phylink_set(mask, 2500baseX_Full);

        /* No ethtool bits for 200Mbps */
        phylink_set(mask, 1000baseT_Full);
        phylink_set(mask, 1000baseX_Full);

        mv88e6065_phylink_validate(chip, port, mask, state);
}

static void mv88e6352_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                       unsigned long *mask,
                                       struct phylink_link_state *state)
{
        /* No ethtool bits for 200Mbps */
        phylink_set(mask, 1000baseT_Full);
        phylink_set(mask, 1000baseX_Full);

        mv88e6065_phylink_validate(chip, port, mask, state);
}

static void mv88e6390_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                       unsigned long *mask,
                                       struct phylink_link_state *state)
{
        if (port >= 9) {
                phylink_set(mask, 2500baseX_Full);
                phylink_set(mask, 2500baseT_Full);
        }

        /* No ethtool bits for 200Mbps */
        phylink_set(mask, 1000baseT_Full);
        phylink_set(mask, 1000baseX_Full);

        mv88e6065_phylink_validate(chip, port, mask, state);
}

static void mv88e6390x_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                        unsigned long *mask,
                                        struct phylink_link_state *state)
{
        if (port >= 9) {
                phylink_set(mask, 10000baseT_Full);
                phylink_set(mask, 10000baseKR_Full);
        }

        mv88e6390_phylink_validate(chip, port, mask, state);
}

static void mv88e6393x_phylink_validate(struct mv88e6xxx_chip *chip, int port,
                                        unsigned long *mask,
                                        struct phylink_link_state *state)
{
        if (port == 0 || port == 9 || port == 10) {
                phylink_set(mask, 10000baseT_Full);
                phylink_set(mask, 10000baseKR_Full);
                phylink_set(mask, 10000baseCR_Full);
                phylink_set(mask, 10000baseSR_Full);
                phylink_set(mask, 10000baseLR_Full);
                phylink_set(mask, 10000baseLRM_Full);
                phylink_set(mask, 10000baseER_Full);
                phylink_set(mask, 5000baseT_Full);
                phylink_set(mask, 2500baseX_Full);
                phylink_set(mask, 2500baseT_Full);
        }

        phylink_set(mask, 1000baseT_Full);
        phylink_set(mask, 1000baseX_Full);

        mv88e6065_phylink_validate(chip, port, mask, state);
}

static void mv88e6xxx_validate(struct dsa_switch *ds, int port,
                               unsigned long *supported,
                               struct phylink_link_state *state)
{
        __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
        struct mv88e6xxx_chip *chip = ds->priv;

        /* Allow all the expected bits */
        phylink_set(mask, Autoneg);
        phylink_set(mask, Pause);
        phylink_set_port_modes(mask);

        if (chip->info->ops->phylink_validate)
                chip->info->ops->phylink_validate(chip, port, mask, state);

        bitmap_and(supported, supported, mask, __ETHTOOL_LINK_MODE_MASK_NBITS);
        bitmap_and(state->advertising, state->advertising, mask,
                   __ETHTOOL_LINK_MODE_MASK_NBITS);

        /* We can only operate at 2500BaseX or 1000BaseX.  If requested
         * to advertise both, only report advertising at 2500BaseX.
         */
        phylink_helper_basex_speed(state);
}

static void mv88e6xxx_mac_config(struct dsa_switch *ds, int port,
                                 unsigned int mode,
                                 const struct phylink_link_state *state)
{
        struct mv88e6xxx_chip *chip = ds->priv;
        struct mv88e6xxx_port *p;
        int err;

        p = &chip->ports[port];

        /* FIXME: is this the correct test? If we're in fixed mode on an
         * internal port, why should we process this any different from
         * PHY mode? On the other hand, the port may be automedia between
         * an internal PHY and the serdes...
         */
        if ((mode == MLO_AN_PHY) && mv88e6xxx_phy_is_internal(ds, port))
                return;

        mv88e6xxx_reg_lock(chip);
        /* 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 && p->interface != state->interface &&
            chip->info->ops->port_set_link)
                chip->info->ops->port_set_link(chip, port, LINK_FORCED_DOWN);

        err = mv88e6xxx_port_config_interface(chip, port, state->interface);
        if (err && err != -EOPNOTSUPP)
                goto err_unlock;

        err = mv88e6xxx_serdes_pcs_config(chip, port, mode, state->interface,
                                          state->advertising);
        /* FIXME: we should restart negotiation if something changed - which
         * is something we get if we convert to using phylinks PCS operations.
         */
        if (err > 0)
                err = 0;

        /* Undo the forced down state above after completing configuration
         * irrespective of its state on entry, which allows the link to come up.
         */
        if (mode == MLO_AN_INBAND && p->interface != state->interface &&
            chip->info->ops->port_set_link)
                chip->info->ops->port_set_link(chip, port, LINK_UNFORCED);

        p->interface = state->interface;

err_unlock:
        mv88e6xxx_reg_unlock(chip);

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

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

        ops = chip->info->ops;

        mv88e6xxx_reg_lock(chip);
        if ((!mv88e6xxx_port_ppu_updates(chip, port) ||
             mode == MLO_AN_FIXED) && ops->port_sync_link)
                err = ops->port_sync_link(chip, port, mode, false);
        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 dsa_switch *ds, int port,
                                  unsigned int mode, phy_interface_t interface,
                                  struct phy_device *phydev,
                                  int speed, int duplex,
                                  bool tx_pause, bool rx_pause)
{
        struct mv88e6xxx_chip *chip = ds->priv;
        const struct mv88e6xxx_ops *ops;
        int err = 0;

        ops = chip->info->ops;

        mv88e6xxx_reg_lock(chip);
        if (!mv88e6xxx_port_ppu_updates(chip, port) || mode == MLO_AN_FIXED) {
                /* FIXME: for an automedia port, should we force the link
                 * down here - what if the link comes up due to "other" media
                 * while we're bringing the port up, how is the exclusivity
                 * handled in the Marvell hardware? E.g. port 2 on 88E6390
                 * shared between internal PHY and Serdes.
                 */
                err = mv88e6xxx_serdes_pcs_link_up(chip, port, mode, speed,
                                                   duplex);
                if (err)
                        goto error;

                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(ds->dev,
                        "p%d: failed to configure MAC link up\n", port);
}

static int mv88e6xxx_stats_snapshot(struct mv88e6xxx_chip *chip, int port)
{
        if (!chip->info->ops->stats_snapshot)
                return -EOPNOTSUPP;

        return chip->info->ops->stats_snapshot(chip, port);
}

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

static uint64_t _mv88e6xxx_get_ethtool_stat(struct mv88e6xxx_chip *chip,
                                            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, &reg);
                if (err)
                        return U64_MAX;

                low = reg;
                if (s->size == 4) {
                        err = mv88e6xxx_port_read(chip, port, s->reg + 1, &reg);
                        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 int mv88e6xxx_stats_get_strings(struct mv88e6xxx_chip *chip,
                                       uint8_t *data, int types)
{
        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) {
                        memcpy(data + j * ETH_GSTRING_LEN, stat->string,
                               ETH_GSTRING_LEN);
                        j++;
                }
        }

        return j;
}

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

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

static int mv88e6320_stats_get_strings(struct mv88e6xxx_chip *chip,
                                       uint8_t *data)
{
        return 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++)
                strlcpy(data + i * ETH_GSTRING_LEN,
                        mv88e6xxx_atu_vtu_stats_strings[i],
                        ETH_GSTRING_LEN);
}

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

        if (stringset != ETH_SS_STATS)
                return;

        mv88e6xxx_reg_lock(chip);

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

        if (chip->info->ops->serdes_get_strings) {
                data += count * ETH_GSTRING_LEN;
                count = chip->info->ops->serdes_get_strings(chip, port, data);
        }

        data += count * ETH_GSTRING_LEN;
        mv88e6xxx_atu_vtu_get_strings(data);

        mv88e6xxx_reg_unlock(chip);
}

static int mv88e6xxx_stats_get_sset_count(struct mv88e6xxx_chip *chip,
                                          int types)
{
        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 int mv88e6xxx_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
                                     uint64_t *data, int types,
                                     u16 bank1_select, u16 histogram)
{
        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) {
                        mv88e6xxx_reg_lock(chip);
                        data[j] = _mv88e6xxx_get_ethtool_stat(chip, stat, port,
                                                              bank1_select,
                                                              histogram);
                        mv88e6xxx_reg_unlock(chip);

                        j++;
                }
        }
        return j;
}

static int mv88e6095_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
                                     uint64_t *data)
{
        return mv88e6xxx_stats_get_stats(chip, port, data,
                                         STATS_TYPE_BANK0 | STATS_TYPE_PORT,
                                         0, MV88E6XXX_G1_STATS_OP_HIST_RX_TX);
}

static int mv88e6250_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
                                     uint64_t *data)
{
        return mv88e6xxx_stats_get_stats(chip, port, data, STATS_TYPE_BANK0,
                                         0, MV88E6XXX_G1_STATS_OP_HIST_RX_TX);
}

static int mv88e6320_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
                                     uint64_t *data)
{
        return mv88e6xxx_stats_get_stats(chip, port, data,
                                         STATS_TYPE_BANK0 | STATS_TYPE_BANK1,
                                         MV88E6XXX_G1_STATS_OP_BANK_1_BIT_9,
                                         MV88E6XXX_G1_STATS_OP_HIST_RX_TX);
}

static int mv88e6390_stats_get_stats(struct mv88e6xxx_chip *chip, int port,
                                     uint64_t *data)
{
        return mv88e6xxx_stats_get_stats(chip, port, data,
                                         STATS_TYPE_BANK0 | STATS_TYPE_BANK1,
                                         MV88E6XXX_G1_STATS_OP_BANK_1_BIT_10,
                                         0);
}

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)
{
        int count = 0;

        if (chip->info->ops->stats_get_stats)
                count = chip->info->ops->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;

        mv88e6xxx_reg_lock(chip);

        ret = mv88e6xxx_stats_snapshot(chip, port);
        mv88e6xxx_reg_unlock(chip);

        if (ret < 0)
                return;

        mv88e6xxx_get_stats(chip, port, data);

}

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, &reg);
                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_get_mac_eee(struct dsa_switch *ds, int port,
                                 struct ethtool_eee *e)
{
        /* Nothing to do on the port's MAC */
        return 0;
}

static int mv88e6xxx_set_mac_eee(struct dsa_switch *ds, int port,
                                 struct ethtool_eee *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 net_device *br;
        struct dsa_port *dp;
        bool found = false;
        u16 pvlan;

        list_for_each_entry(dp, &dst->ports, list) {
                if (dp->ds->index == dev && dp->index == port) {
                        found = true;
                        break;
                }
        }

        /* Prevent frames from unknown switch or port */
        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);

        br = dp->bridge_dev;
        pvlan = 0;

        /* Frames from user ports can egress any local DSA links and CPU ports,
         * as well as any local member of their bridge group.
         */
        list_for_each_entry(dp, &dst->ports, list)
                if (dp->ds == ds &&
                    (dp->type == DSA_PORT_TYPE_CPU ||
                     dp->type == DSA_PORT_TYPE_DSA ||
                     (br && dp->bridge_dev == br)))
                        pvlan |= BIT(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_dev) {
                        /* 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 as the port number.
                         */
                        dev = MV88E6XXX_G2_PVT_ADDR_DEV_TRUNK;
                        port = dsa_lag_id(dst, dp->lag_dev);
                }
        }

        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 void mv88e6xxx_port_fast_age(struct dsa_switch *ds, int port)
{
        struct mv88e6xxx_chip *chip = ds->priv;
        int err;

        if (dsa_to_port(ds, port)->lag_dev)
                /* 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;

        mv88e6xxx_reg_lock(chip);
        err = mv88e6xxx_g1_atu_remove(chip, 0, port, false);
        mv88e6xxx_reg_unlock(chip);

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

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;

        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;
}

static 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_fid_map_vlan(struct mv88e6xxx_chip *chip,
                                  const struct mv88e6xxx_vtu_entry *entry,
                                  void *_fid_bitmap)
{
        unsigned long *fid_bitmap = _fid_bitmap;

        set_bit(entry->fid, fid_bitmap);
        return 0;
}

int mv88e6xxx_fid_map(struct mv88e6xxx_chip *chip, unsigned long *fid_bitmap)
{
        int i, err;
        u16 fid;

        bitmap_zero(fid_bitmap, MV88E6XXX_N_FID);

        /* Set every FID bit used by the (un)bridged ports */
        for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
                err = mv88e6xxx_port_get_fid(chip, i, &fid);
                if (err)
                        return err;

                set_bit(fid, fid_bitmap);
        }

        /* Set every FID bit used by the VLAN entries */
        return mv88e6xxx_vtu_walk(chip, mv88e6xxx_fid_map_vlan, fid_bitmap);
}

static int mv88e6xxx_atu_new(struct mv88e6xxx_chip *chip, u16 *fid)
{
        DECLARE_BITMAP(fid_bitmap, MV88E6XXX_N_FID);
        int err;

        err = mv88e6xxx_fid_map(chip, fid_bitmap);
        if (err)
                return err;

        /* The reset value 0x000 is used to indicate that multiple address
         * databases are not needed. Return the next positive available.
         */
        *fid = find_next_zero_bit(fid_bitmap, MV88E6XXX_N_FID, 1);
        if (unlikely(*fid >= mv88e6xxx_num_databases(chip)))
                return -ENOSPC;

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

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

        /* DSA and CPU ports have to be members of multiple vlans */
        if (dsa_is_dsa_port(ds, port) || dsa_is_cpu_port(ds, port))
                return 0;

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

        if (!vlan.valid)
                return 0;

        for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
                if (dsa_is_dsa_port(ds, i) || dsa_is_cpu_port(ds, i))
                        continue;

                if (!dsa_to_port(ds, i)->slave)
                        continue;

                if (vlan.member[i] ==
                    MV88E6XXX_G1_VTU_DATA_MEMBER_TAG_NON_MEMBER)
                        continue;

                if (dsa_to_port(ds, i)->bridge_dev ==
                    dsa_to_port(ds, port)->bridge_dev)
                        break; /* same bridge, check next VLAN */

                if (!dsa_to_port(ds, i)->bridge_dev)
                        continue;

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

        return 0;
}

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);
        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_load_purge(struct mv88e6xxx_chip *chip, int port,
                                        const unsigned char *addr, u16 vid,
                                        u8 state)
{
        struct mv88e6xxx_atu_entry entry;
        struct mv88e6xxx_vtu_entry vlan;
        u16 fid;
        int err;

        /* Null VLAN ID corresponds to the port private database */
        if (vid == 0) {
                err = mv88e6xxx_port_get_fid(chip, port, &fid);
                if (err)
                        return err;
        } 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);

        err = mv88e6xxx_g1_atu_getnext(chip, 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,
                               struct ethtool_rxnfc *rxnfc)
{
        struct ethtool_rx_flow_spec *fs = &rxnfc->fs;
        struct mv88e6xxx_chip *chip = ds->priv;
        struct mv88e6xxx_policy *policy;
        int err;

        mv88e6xxx_reg_lock(chip);

        switch (rxnfc->cmd) {
        case ETHTOOL_SRXCLSRLINS:
                err = mv88e6xxx_policy_insert(chip, port, fs);
                break;
        case ETHTOOL_SRXCLSRLDEL:
                err = -ENOENT;
                policy = idr_remove(&chip->policies, fs->location);
                if (policy) {
                        policy->action = MV88E6XXX_POLICY_ACTION_NORMAL;
                        err = mv88e6xxx_policy_apply(chip, port, policy);
                        devm_kfree(chip->dev, policy);
                }
                break;
        default:
                err = -EOPNOTSUPP;
                break;
        }

        mv88e6xxx_reg_unlock(chip);

        return err;
}

static int mv88e6xxx_port_add_broadcast(struct mv88e6xxx_chip *chip, int port,
                                        u16 vid)
{
        u8 state = MV88E6XXX_G1_ATU_DATA_STATE_MC_STATIC;
        u8 broadcast[ETH_ALEN];

        eth_broadcast_addr(broadcast);

        return mv88e6xxx_port_db_load_purge(chip, port, broadcast, vid, state);
}

static int mv88e6xxx_broadcast_setup(struct mv88e6xxx_chip *chip, u16 vid)
{
        int port;
        int err;

        for (port = 0; port < mv88e6xxx_num_ports(chip); port++) {
                struct dsa_port *dp = dsa_to_port(chip->ds, port);
                struct net_device *brport;

                if (dsa_is_unused_port(chip->ds, port))
                        continue;

                brport = dsa_port_to_bridge_port(dp);
                if (brport && !br_port_flag_is_set(brport, BR_BCAST_FLOOD))
                        /* Skip bridged user ports where broadcast
                         * flooding is disabled.
                         */
                        continue;

                err = mv88e6xxx_port_add_broadcast(chip, port, vid);
                if (err)
                        return err;
        }

        return 0;