path: root/drivers/net/dsa/realtek/rtl8365mb_vlan.c

// SPDX-License-Identifier: GPL-2.0
/* VLAN configuration interface for the rtl8365mb switch family
 *
 * Copyright (C) 2022 Alvin Šipraga <alsi@bang-olufsen.dk>
 *
 * VLAN configuration takes place in two separate domains of the switch: the
 * VLAN4k table and the VLAN membership configuration (MC) database. While the
 * VLAN4k table is exhaustive and can be fully populated with 4096 VLAN
 * configurations, the same does not hold for the VLAN membership configuration
 * database, which is limited to 32 entries.
 *
 * The switch will normally only use the VLAN4k table when making forwarding
 * decisions. The VLAN membership configuration database is a vestigial ASIC
 * design and is only used for a few specific features in the rtl8365mb
 * family. This means that the limit of 32 entries should not hinder us in
 * programming a huge number of VLANs into the switch.
 *
 * One necessary use of the VLAN membership configuration database is for the
 * programming of a port-based VLAN ID (PVID). The PVID is programmed on a
 * per-port basis via register field, which refers to a specific VLAN membership
 * configuration via an index 0~31. In order to maintain coherent behaviour on a
 * port with a PVID, it is necessary to keep the VLAN configuration synchronized
 * between the VLAN4k table and the VLAN membership configuration database.
 *
 * Since VLAN membership configs are a scarce resource, it will only be used
 * when strictly needed (i.e. a VLAN with members using PVID). Otherwise, the
 * VLAN4k will be enough.
 *
 * With some exceptions, the entries in both the VLAN4k table and the VLAN
 * membership configuration database offer the same configuration options. The
 * differences are as follows:
 *
 * 1. VLAN4k entries can specify whether to use Independent or Shared VLAN
 *    Learning (IVL or SVL respectively). VLAN membership config entries
 *    cannot. This underscores the fact that VLAN membership configs are not
 *    involved in the learning process of the ASIC.
 *
 * 2. VLAN membership config entries use an "enhanced VLAN ID" (efid), which has
 *    a range 0~8191 compared with the standard 0~4095 range of the VLAN4k
 *    table. This underscores the fact that VLAN membership configs can be used
 *    to group ports on a layer beyond the standard VLAN configuration, which
 *    may be useful for ACL rules which specify alternative forwarding
 *    decisions.
 *
 * VLANMC index 0 is reserved as a neutral PVID, used for standalone ports.
 *
 */

#include "rtl8365mb_vlan.h"
#include "rtl8365mb_table.h"
#include <linux/if_bridge.h>
#include <linux/lockdep.h>
#include <linux/regmap.h>

/* CVLAN (i.e. VLAN4k) table entry layout, u16[3] */
#define RTL8365MB_CVLAN_ENTRY_SIZE			3 /* 48-bits */
#define RTL8365MB_CVLAN_ENTRY_D0_MBR_MASK		GENMASK(7, 0)
#define   RTL8365MB_CVLAN_MBR_LO_MASK			GENMASK(7, 0)
#define RTL8365MB_CVLAN_ENTRY_D0_UNTAG_MASK		GENMASK(15, 8)
#define   RTL8365MB_CVLAN_UNTAG_LO_MASK			GENMASK(7, 0)
#define RTL8365MB_CVLAN_ENTRY_D1_FID_MASK		GENMASK(3, 0)
#define RTL8365MB_CVLAN_ENTRY_D1_VBPEN_MASK		GENMASK(4, 4)
#define RTL8365MB_CVLAN_ENTRY_D1_VBPRI_MASK		GENMASK(7, 5)
#define RTL8365MB_CVLAN_ENTRY_D1_ENVLANPOL_MASK		GENMASK(8, 8)
#define RTL8365MB_CVLAN_ENTRY_D1_METERIDX_MASK		GENMASK(13, 9)
#define   RTL8365MB_CVLAN_METERIDX_LO_MASK		GENMASK(4, 0)
#define RTL8365MB_CVLAN_ENTRY_D1_IVL_SVL_MASK		GENMASK(14, 14)
/* extends RTL8365MB_CVLAN_ENTRY_D0_MBR_MASK */
#define RTL8365MB_CVLAN_ENTRY_D2_MBR_EXT_MASK		GENMASK(2, 0)
#define   RTL8365MB_CVLAN_MBR_HI_MASK			GENMASK(10, 8)
/* extends RTL8365MB_CVLAN_ENTRY_D0_UNTAG_MASK */
#define RTL8365MB_CVLAN_ENTRY_D2_UNTAG_EXT_MASK		GENMASK(5, 3)
#define   RTL8365MB_CVLAN_UNTAG_HI_MASK			GENMASK(10, 8)
/* extends RTL8365MB_CVLAN_ENTRY_D1_METERIDX_MASK */
#define RTL8365MB_CVLAN_ENTRY_D2_METERIDX_EXT_MASK	GENMASK(6, 6)
#define   RTL8365MB_CVLAN_METERIDX_HI_MASK		GENMASK(5, 5)

/* VLAN member configuration registers 0~31, u16[3] */
#define RTL8365MB_VLAN_MC_BASE				0x0728
#define RTL8365MB_VLAN_MC_ENTRY_SIZE			4 /* 64-bit */
#define RTL8365MB_VLAN_MC_REG(index) \
		(RTL8365MB_VLAN_MC_BASE + \
		 (RTL8365MB_VLAN_MC_ENTRY_SIZE * (index)))
#define   RTL8365MB_VLAN_MC_D0_MBR_MASK			GENMASK(10, 0)
#define   RTL8365MB_VLAN_MC_D1_FID_MASK			GENMASK(3, 0)

#define   RTL8365MB_VLAN_MC_D2_VBPEN_MASK		GENMASK(0, 0)
#define   RTL8365MB_VLAN_MC_D2_VBPRI_MASK		GENMASK(3, 1)
#define   RTL8365MB_VLAN_MC_D2_ENVLANPOL_MASK		GENMASK(4, 4)
#define   RTL8365MB_VLAN_MC_D2_METERIDX_MASK		GENMASK(10, 5)
#define   RTL8365MB_VLAN_MC_D3_EVID_MASK		GENMASK(12, 0)

/* Some limits for VLAN4k/VLAN membership config entries */
#define RTL8365MB_PRIORITYMAX	7
#define RTL8365MB_FIDMAX	15
#define RTL8365MB_METERMAX	63
#define RTL8365MB_VLAN_MCMAX	31

/* RTL8367S supports 4k vlans (vid<=4095) and 32 enhanced vlans
 * for VIDs up to 8191
 */
#define RTL8365MB_MAX_4K_VID	0x0FFF /* 4095 */
#define RTL8365MB_MAX_MC_VID	0x1FFF /* 8191 */

 /* Port-based VID registers 0~5 - each one holds an MC index for two ports */
#define RTL8365MB_VLAN_PVID_CTRL_BASE			0x0700
#define RTL8365MB_VLAN_PVID_CTRL_REG(_p) \
		(RTL8365MB_VLAN_PVID_CTRL_BASE + ((_p) >> 1))
#define   RTL8365MB_VLAN_PVID_CTRL_PORT0_MCIDX_MASK	0x001F
#define   RTL8365MB_VLAN_PVID_CTRL_PORT1_MCIDX_MASK	0x1F00
#define   RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_OFFSET(_p) \
		(((_p) & 1) << 3)
#define   RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_MASK(_p) \
		(0x1F << RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_OFFSET(_p))

/* Frame type filtering registers */
#define RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_BASE	0x07aa
#define RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_REG(port) \
		(RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_BASE + ((port) >> 3))
/* required as FIELD_PREP cannot use non-constant masks */
#define RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_MASK(port) \
		(0x3 << RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_OFFSET(port))
#define RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_OFFSET(port) \
		(((port) & 0x7) << 1)

/*
 * struct rtl8365mb_vlan4k - VLAN4k table entry
 * @vid: VLAN ID (0~4095)
 * @member: port mask of ports in this VLAN
 * @untag: port mask of ports which untag on egress
 * @fid: filter ID - only used with SVL (unused)
 * @priority: priority classification (unused)
 * @priority_en: enable priority (unused)
 * @policing_en: enable policing (unused)
 * @ivl_en: enable IVL instead of default SVL
 * @meteridx: metering index (unused)
 *
 * This structure is used to get/set entries in the VLAN4k table. The
 * VLAN4k table dictates the VLAN configuration for the switch for the
 * vast majority of features.
 */
struct rtl8365mb_vlan4k {
	u16 vid;
	u16 member;
	u16 untag;
	u8 fid : 4;
	u8 priority : 3;
	u8 priority_en : 1;
	u8 policing_en : 1;
	u8 ivl_en : 1;
	u8 meteridx : 6;
};

/*
 * struct rtl8365mb_vlanmc - VLAN membership config
 * @evid: Enhanced VLAN ID (0~8191)
 * @member: port mask of ports in this VLAN
 * @fid: filter ID - only used with SVL (unused)
 * @priority: priority classification (unused)
 * @priority_en: enable priority (unused)
 * @policing_en: enable policing (unused)
 * @meteridx: metering index (unused)
 *
 * This structure is used to get/set entries in the VLAN membership
 * configuration database. This feature is largely vestigial, but
 * still needed for at least the following features:
 *   - PVID configuration
 *   - ACL configuration
 *   - selection of VLAN by the CPU tag when VSEL=1, although the switch
 *     can also select VLAN based on the VLAN tag if VSEL=0
 *
 * This is a low-level structure and it is recommended to interface with
 * the VLAN membership config database via &struct rtl8365mb_vlanmc_entry.
 */
struct rtl8365mb_vlanmc {
	u16 evid;
	u16 member;
	u8 fid : 4;
	u8 priority : 3;
	u8 priority_en : 1;
	u8 policing_en : 1;
	u8 meteridx : 6;
};

static int rtl8365mb_vlan_4k_read(struct realtek_priv *priv, u16 vid,
				  struct rtl8365mb_vlan4k *vlan4k)
{
	u16 data[RTL8365MB_CVLAN_ENTRY_SIZE];
	int val;
	int ret;

	ret = rtl8365mb_table_query(priv, RTL8365MB_TABLE_CVLAN,
				    RTL8365MB_TABLE_OP_READ, &vid, 0, 0,
				    data, ARRAY_SIZE(data));
	if (ret)
		return ret;

	/* Unpack table entry */
	memset(vlan4k, 0, sizeof(*vlan4k));
	vlan4k->vid = vid;

	val = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D0_MBR_MASK, data[0]);
	vlan4k->member = FIELD_PREP(RTL8365MB_CVLAN_MBR_LO_MASK, val);
	val = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D2_MBR_EXT_MASK, data[2]);
	vlan4k->member |= FIELD_PREP(RTL8365MB_CVLAN_MBR_HI_MASK, val);

	val = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D0_UNTAG_MASK, data[0]);
	vlan4k->untag = FIELD_PREP(RTL8365MB_CVLAN_UNTAG_LO_MASK, val);
	val = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D2_UNTAG_EXT_MASK, data[2]);
	vlan4k->untag |= FIELD_PREP(RTL8365MB_CVLAN_UNTAG_HI_MASK, val);

	vlan4k->fid = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D1_FID_MASK, data[1]);
	vlan4k->priority_en =
		FIELD_GET(RTL8365MB_CVLAN_ENTRY_D1_VBPEN_MASK, data[1]);
	vlan4k->priority =
		FIELD_GET(RTL8365MB_CVLAN_ENTRY_D1_VBPRI_MASK, data[1]);
	vlan4k->policing_en =
		FIELD_GET(RTL8365MB_CVLAN_ENTRY_D1_ENVLANPOL_MASK, data[1]);

	val = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D1_METERIDX_MASK, data[1]);
	val = FIELD_PREP(RTL8365MB_CVLAN_METERIDX_LO_MASK, val);
	vlan4k->meteridx = val;
	val = FIELD_GET(RTL8365MB_CVLAN_ENTRY_D2_METERIDX_EXT_MASK, data[2]);
	val = FIELD_PREP(RTL8365MB_CVLAN_METERIDX_HI_MASK, val);
	vlan4k->meteridx |= val;

	vlan4k->ivl_en =
		FIELD_GET(RTL8365MB_CVLAN_ENTRY_D1_IVL_SVL_MASK, data[1]);

	return 0;
}

static int rtl8365mb_vlan_4k_write(struct realtek_priv *priv,
				   const struct rtl8365mb_vlan4k *vlan4k)
{
	u16 data[RTL8365MB_CVLAN_ENTRY_SIZE] = { 0 };
	u16 vid;
	int val;

	/* Pack table entry value */
	val = FIELD_GET(RTL8365MB_CVLAN_MBR_LO_MASK, vlan4k->member);
	data[0] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D0_MBR_MASK, val);

	val = FIELD_GET(RTL8365MB_CVLAN_UNTAG_LO_MASK, vlan4k->untag);
	data[0] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D0_UNTAG_MASK, val);

	data[1] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D1_FID_MASK, vlan4k->fid);
	data[1] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D1_VBPEN_MASK,
			      vlan4k->priority_en);
	data[1] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D1_VBPRI_MASK,
			      vlan4k->priority);
	data[1] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D1_ENVLANPOL_MASK,
			      vlan4k->policing_en);

	/* FIELD_* does not play nice with struct bitfield. */
	val = vlan4k->meteridx;
	val = FIELD_GET(RTL8365MB_CVLAN_METERIDX_LO_MASK, val);
	data[1] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D1_METERIDX_MASK, val);

	data[1] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D1_IVL_SVL_MASK,
			      vlan4k->ivl_en);

	val = FIELD_GET(RTL8365MB_CVLAN_MBR_HI_MASK, vlan4k->member);
	data[2] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D2_MBR_EXT_MASK, val);

	val = FIELD_GET(RTL8365MB_CVLAN_UNTAG_HI_MASK, vlan4k->untag);
	data[2] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D2_UNTAG_EXT_MASK, val);

	val = vlan4k->meteridx;
	val = FIELD_GET(RTL8365MB_CVLAN_METERIDX_HI_MASK, val);
	data[2] |= FIELD_PREP(RTL8365MB_CVLAN_ENTRY_D2_METERIDX_EXT_MASK, val);

	vid = vlan4k->vid;
	return rtl8365mb_table_query(priv, RTL8365MB_TABLE_CVLAN,
				     RTL8365MB_TABLE_OP_WRITE, &vid, 0, 0,
				     data, ARRAY_SIZE(data));
}

static int
rtl8365mb_vlan_4k_port_set(struct dsa_switch *ds, int port,
			   const struct switchdev_obj_port_vlan *vlan,
			   struct netlink_ext_ack *extack,
			   bool include)
{
	struct realtek_priv *priv = ds->priv;
	struct rtl8365mb_vlan4k vlan4k = {0};
	int ret;

	dev_dbg(priv->dev, "%s VLAN %d 4K on port %d\n",
		include ? "add" : "del",
		vlan->vid, port);

	if (vlan->vid > RTL8365MB_MAX_4K_VID) {
		NL_SET_ERR_MSG_MOD(extack, "VLAN ID greater than "
				   __stringify(RTL8365MB_MAX_4K_VID));
		return -EINVAL;
	}

	ret = rtl8365mb_vlan_4k_read(priv, vlan->vid, &vlan4k);
	if (ret) {
		dev_err(priv->dev, "Failed to read VLAN 4k table\n");
		return ret;
	}

	if (include)
		vlan4k.member |= BIT(port);
	else
		vlan4k.member &= ~BIT(port);

	if (include && (vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED))
		vlan4k.untag |= BIT(port);
	else
		vlan4k.untag &= ~BIT(port);
	vlan4k.ivl_en = true; /* always use Independent VLAN Learning */

	ret = rtl8365mb_vlan_4k_write(priv, &vlan4k);
	if (ret) {
		dev_err(priv->dev, "Failed to write VLAN 4k table\n");
		return ret;
	}

	return 0;
}

/*
 * rtl8365mb_vlan_4k_port_add() - Add a port to a VLAN 4K table entry
 * @ds: dsa switch instance
 * @port: port index
 * @vlan: switchdev VLAN object containing the target VID and flags
 * @extack: netlink extended ACK for error reporting
 *
 * Adds the specified port to the hardware VLAN 4K membership table.
 *
 * Context: Can sleep. Must be called with &priv->vlan_lock held.
 * Takes and releases &priv->map_lock.
 * Return: 0 on success, or a negative error code on failure.
 */
int rtl8365mb_vlan_4k_port_add(struct dsa_switch *ds, int port,
			       const struct switchdev_obj_port_vlan *vlan,
			       struct netlink_ext_ack *extack)
{
	struct realtek_priv *priv = ds->priv;

	lockdep_assert_held(&priv->vlan_lock);

	return rtl8365mb_vlan_4k_port_set(ds, port, vlan, extack, true);
}

/*
 * rtl8365mb_vlan_4k_port_del() - Remove a port from a VLAN 4K table entry
 * @ds: dsa switch instance
 * @port: port index
 * @vlan: switchdev VLAN object containing the target VID
 *
 * Removes the specified port from the hardware VLAN 4K membership table.
 *
 * Context: Can sleep. Must be called with &priv->vlan_lock held.
 * Takes and releases &priv->map_lock.
 * Return: 0 on success, or a negative error code on failure.
 */
int rtl8365mb_vlan_4k_port_del(struct dsa_switch *ds, int port,
			       const struct switchdev_obj_port_vlan *vlan)
{
	struct realtek_priv *priv = ds->priv;

	lockdep_assert_held(&priv->vlan_lock);

	return rtl8365mb_vlan_4k_port_set(ds, port, vlan, NULL, false);
}

static int rtl8365mb_vlan_mc_read(struct realtek_priv *priv, u32 index,
				  struct rtl8365mb_vlanmc *vlanmc)
{
	u16 data[RTL8365MB_VLAN_MC_ENTRY_SIZE];
	int ret;

	ret = regmap_bulk_read(priv->map, RTL8365MB_VLAN_MC_REG(index), &data,
			       RTL8365MB_VLAN_MC_ENTRY_SIZE);
	if (ret)
		return ret;

	vlanmc->member = FIELD_GET(RTL8365MB_VLAN_MC_D0_MBR_MASK, data[0]);
	vlanmc->fid = FIELD_GET(RTL8365MB_VLAN_MC_D1_FID_MASK, data[1]);
	vlanmc->meteridx = FIELD_GET(RTL8365MB_VLAN_MC_D2_METERIDX_MASK,
				     data[2]);
	vlanmc->policing_en = FIELD_GET(RTL8365MB_VLAN_MC_D2_ENVLANPOL_MASK,
					data[2]);
	vlanmc->priority = FIELD_GET(RTL8365MB_VLAN_MC_D2_VBPRI_MASK, data[2]);
	vlanmc->priority_en = FIELD_GET(RTL8365MB_VLAN_MC_D2_VBPEN_MASK,
					data[2]);
	vlanmc->evid = FIELD_GET(RTL8365MB_VLAN_MC_D3_EVID_MASK, data[3]);

	return 0;
}

static int rtl8365mb_vlan_mc_write(struct realtek_priv *priv, u32 index,
				   const struct rtl8365mb_vlanmc *vlanmc)
{
	u16 data[RTL8365MB_VLAN_MC_ENTRY_SIZE] = { 0 };
	int ret;

	data[0] |= FIELD_PREP(RTL8365MB_VLAN_MC_D0_MBR_MASK, vlanmc->member);
	data[1] |= FIELD_PREP(RTL8365MB_VLAN_MC_D1_FID_MASK, vlanmc->fid);
	data[2] |= FIELD_PREP(RTL8365MB_VLAN_MC_D2_METERIDX_MASK,
			      vlanmc->meteridx);
	data[2] |= FIELD_PREP(RTL8365MB_VLAN_MC_D2_ENVLANPOL_MASK,
			      vlanmc->policing_en);
	data[2] |=
		FIELD_PREP(RTL8365MB_VLAN_MC_D2_VBPRI_MASK, vlanmc->priority);
	data[2] |= FIELD_PREP(RTL8365MB_VLAN_MC_D2_VBPEN_MASK,
			      vlanmc->priority_en);
	data[3] |= FIELD_PREP(RTL8365MB_VLAN_MC_D3_EVID_MASK, vlanmc->evid);

	ret = regmap_bulk_write(priv->map, RTL8365MB_VLAN_MC_REG(index), &data,
				RTL8365MB_VLAN_MC_ENTRY_SIZE);

	return ret;
}

static int rtl8365mb_vlan_mc_erase(struct realtek_priv *priv, u32 index)
{
	u16 data[RTL8365MB_VLAN_MC_ENTRY_SIZE] = { 0 };
	int ret;

	ret = regmap_bulk_write(priv->map, RTL8365MB_VLAN_MC_REG(index), &data,
				RTL8365MB_VLAN_MC_ENTRY_SIZE);

	return ret;
}

/*
 * rtl8365mb_vlan_mc_find() - find VLANMC index by VID or the first free index
 *
 * @priv: realtek_priv pointer
 * @vid: VLAN ID
 * @index: found index
 * @first_free: found free index
 *
 * If a VLAN MC entry using @vid was found, @index will return the matched index
 * and @first_free is undefined. If not found, @index will return 0 and
 * @first_free will return the first found free index in VLAN MC or 0 if the
 * table is full.
 *
 * Although 0 is a valid VLAN MC index, it is reserved for ports without PVID,
 * including standalone, non-member ports. It uses VID == 0.
 *
 * Both @index and @first_free will be in the * 1..@RTL8365MB_VLAN_MCMAX range.
 *
 * Return: Returns 0 on success, a negative error on failure.
 */
static int rtl8365mb_vlan_mc_find(struct realtek_priv *priv, u16 vid,
				  u8 *index, u8 *first_free)
{
	u32 vlan_entry_d3;
	u8 vlanmc_idx;
	u16 evid;
	int ret;

	*index = 0;
	*first_free = 0;

	/* look for existing entry or an empty one */
	/* By design, VlanMC[0] is reserved as a neutral PVID value for
	 * standalone ports. It always has EVID == 0. That way, we assume that
	 * all entries after index 0 with VID == 0 are empty.
	 **/
	for (vlanmc_idx = 1; vlanmc_idx <= RTL8365MB_VLAN_MCMAX; vlanmc_idx++) {
		/* just read the 4th word, where the evid is */
		ret = regmap_read(priv->map,
				  RTL8365MB_VLAN_MC_REG(vlanmc_idx) + 3,
				  &vlan_entry_d3);
		if (ret)
			return ret;

		evid = FIELD_GET(RTL8365MB_VLAN_MC_D3_EVID_MASK, vlan_entry_d3);

		if (evid == vid) {
			*index = vlanmc_idx;
			return 0;
		}

		if (evid == 0x0 && *first_free < 1)
			*first_free = vlanmc_idx;
	}
	return 0;
}

static int rtl8365mb_vlan_port_get_pvid_idx(struct realtek_priv *priv,
					    int port, u8 *vlanmc_idx)
{
	u32 data;
	int ret;

	ret = regmap_read(priv->map, RTL8365MB_VLAN_PVID_CTRL_REG(port), &data);
	if (ret)
		return ret;

	*vlanmc_idx = (data & RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_MASK(port))
		      >> RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_OFFSET(port);

	return 0;
}

/*
 * rtl8365mb_vlan_mc_port_set() - include or exclude a port from VlanMC
 * @ds: dsa switch
 * @port: the port number
 * @vid: the vlan VID to include/exclude @port
 * @pvid: inform if vid is used as pvid in @port
 * @extack: optional extack to return errors
 * @include: whether to include or exclude @port
 *
 * This function is used to include/exclude ports to the VlanMC table.
 *
 * VlanMC stands for VLAN membership config and it is used exclusively for
 * PVID. If @vlan members are not using PVID, this function will either
 * remove or not create a new VlanMC entry.
 *
 * VlanMC members are used as a reference port map, cleaning the entry once
 * no port is using it.
 *
 * Port PVID and accepted frame type are updated as well.
 *
 * Context: Can sleep. Must be called with &priv->vlan_lock held.
 * Takes and releases &priv->map_lock.
 * Return: Returns 0 on success, a negative error on failure.
 */
static
int rtl8365mb_vlan_mc_port_set(struct dsa_switch *ds, int port,
			       u16 vid, bool pvid,
			       struct netlink_ext_ack *extack,
			       bool include)
{
	struct realtek_priv *priv = ds->priv;
	struct rtl8365mb_vlanmc vlanmc = {0};
	u8 first_unused = 0;
	u8 vlanmc_idx = 0;
	int ret;

	dev_dbg(priv->dev, "%s VLAN %d MC on port %d\n",
		include ? "add" : "del",
		vid, port);

	if (vid > RTL8365MB_MAX_MC_VID) {
		NL_SET_ERR_MSG_MOD(extack, "VLAN ID greater than "
				   __stringify(RTL8365MB_MAX_MC_VID));
		return -EINVAL;
	}

	/* look for existing entry or an empty slot */
	ret = rtl8365mb_vlan_mc_find(priv, vid, &vlanmc_idx,
				     &first_unused);
	if (ret) {
		dev_err(priv->dev, "Failed to find a VLAN MC table index\n");
		return ret;
	}

	if (vlanmc_idx) {
		ret = rtl8365mb_vlan_mc_read(priv, vlanmc_idx, &vlanmc);
		if (ret) {
			dev_err(priv->dev, "Failed to read VLAN MC table\n");
			return ret;
		}
	} else if (include) {
		/* for now, vlan_mc is only required for PVID. Defer allocation
		 * until at least one port uses PVID.
		 */
		if (!pvid) {
			dev_dbg(priv->dev,
				"Not creating VlanMC for vlan %d until a port uses PVID (%d does not)\n",
				vid, port);
			return 0;
		}

		if (!first_unused) {
			NL_SET_ERR_MSG_MOD(extack, "All VLAN MC entries (0.."
					   __stringify(RTL8365MB_VLAN_MCMAX)
					   ") are in use.");
			return -ENOSPC;
		}

		vlanmc_idx = first_unused;
		vlanmc.evid = vid;

	} else /* excluding and VLANMC not found */ {
		return 0;
	}

	dev_dbg(priv->dev,
		"VLAN %d (idx: %d) PVID curr members: %08x\n",
		vid, vlanmc_idx, vlanmc.member);

	/* here we either have an existing VLANMC (with PVID members) or the
	 * added port is using this VLAN as PVID
	 */
	if (include)
		vlanmc.member |= BIT(port);
	else
		vlanmc.member &= ~BIT(port);

	/* just like we don't need to create a VLAN_MC when there is no port
	 * using it as PVID, we can erase it when there is no more port using
	 * it as PVID.
	 */
	if (!vlanmc.member) {
		dev_dbg(priv->dev,
			"Clearing VlanMC index %d previously used by VID %d\n",
			vlanmc_idx, vid);
		ret = rtl8365mb_vlan_mc_erase(priv, vlanmc_idx);
	} else {
		dev_dbg(priv->dev,
			"Saving VlanMC index %d with VID %d\n",
			vlanmc_idx, vid);
		ret = rtl8365mb_vlan_mc_write(priv, vlanmc_idx, &vlanmc);
	}
	if (ret) {
		dev_err(priv->dev, "Failed to write vlan MC entry\n");
		return ret;
	}

	return 0;
}

static int rtl8365mb_vlan_port_set_pvid(struct realtek_priv *priv,
					int port, u16 vlanmc_idx)
{
	int ret;
	u32 val;

	dev_dbg(priv->dev, "set PVID IDX %d on port %d\n", vlanmc_idx, port);

	val = vlanmc_idx << RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_OFFSET(port);
	ret = regmap_update_bits(priv->map,
				 RTL8365MB_VLAN_PVID_CTRL_REG(port),
				 RTL8365MB_VLAN_PVID_CTRL_PORT_MCIDX_MASK(port),
				 val);
	if (ret)
		return ret;

	return 0;
}

static int rtl8365mb_vlan_get_pvid_mc(struct realtek_priv *priv,
				      int port, u8 *vlanmc_idx,
				      struct rtl8365mb_vlanmc *vlanmc)
{
	int ret;

	ret = rtl8365mb_vlan_port_get_pvid_idx(priv, port, vlanmc_idx);
	if (ret)
		return ret;

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

	if (!*vlanmc_idx)
		return 0;

	ret = rtl8365mb_vlan_mc_read(priv, *vlanmc_idx, vlanmc);
	if (ret)
		return ret;

	return 0;
}

/*
 * rtl8365mb_vlan_port_get_pvid - Retrieve the port PVID
 * @priv: realtek switch private structure
 * @port: port index
 * @pvid: pointer to store the retrieved VLAN ID
 *
 * Returns the port PVID if defined or 0 if not.
 *
 * Context: Can sleep. Takes and releases &priv->map_lock.
 * Return: 0 on success or a negative error code on failure.
 */
int rtl8365mb_vlan_port_get_pvid(struct realtek_priv *priv, int port, u16 *pvid)
{
	struct rtl8365mb_vlanmc vlanmc;
	u8 vlanmc_idx;
	int ret;

	ret = rtl8365mb_vlan_get_pvid_mc(priv, port, &vlanmc_idx, &vlanmc);
	if (ret)
		return ret;

	*pvid = vlanmc.evid;
	return 0;
}

/*
 * rtl8365mb_vlan_port_get_framefilter() - Get the ingress frame filtering mode
 * for a port
 * @priv: realtek switch private structure
 * @port: port index
 * @frame_type: pointer to store the retrieved ingress frame filter type
 *
 * Context: Can sleep. Takes and releases &priv->map_lock.
 * Return: 0 on success, or a negative error code on failure.
 */
int
rtl8365mb_vlan_port_get_framefilter(struct realtek_priv *priv,
				    int port,
				    enum rtl8365mb_frame_ingress *frame_type)
{
	u32 val;
	int ret;

	/* Even if ACCEPT_FRAME_TYPE_ANY, the switch will still check if the
	 * port is a member of vlan PVID
	 */

	ret = regmap_read(priv->map, RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_REG(port),
			  &val);
	if (ret)
		return ret;

	*frame_type = field_get(RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_MASK(port),
				val);

	return 0;
}

/*
 * rtl8365mb_vlan_port_set_framefilter() - Set the ingress frame filtering mode
 * for a port
 * @priv: realtek switch private structure
 * @port: port index
 * @frame_type: the ingress frame filter type to configure
 *
 * Context: Can sleep. Takes and releases &priv->map_lock.
 * Return: 0 on success, or a negative error code on failure.
 */
int
rtl8365mb_vlan_port_set_framefilter(struct realtek_priv *priv,
				    int port,
				    enum rtl8365mb_frame_ingress frame_type)
{
	u32 val;

	/* Even if ACCEPT_FRAME_TYPE_ANY, the switch will still check if the
	 * port is a member of vlan PVID
	 */
	val = frame_type << RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_OFFSET(port);

	return regmap_update_bits(priv->map,
				  RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_REG(port),
				  RTL8365MB_VLAN_ACCEPT_FRAME_TYPE_MASK(port),
				  val);
}

/*
 * rtl8365mb_vlan_pvid_port_set() - Configure a port's PVID and associated
 * VLANMC entry
 * @ds: dsa switch instance
 * @port: port index
 * @vid: target VID
 * @extack: netlink extended ACK for error reporting
 *
 * Allocates or reuses a hardware VLANMC entry to map the given port to its new
 * PVID. Gracefully unwinds and restores previous configuration if a hardware
 * write operation fails during execution.
 *
 * Context: Can sleep. Must be called with &priv->vlan_lock held.
 * Takes and releases &priv->map_lock.
 * Return: 0 on success, or a negative error code on failure.
 */
int rtl8365mb_vlan_pvid_port_set(struct dsa_switch *ds, int port, u16 vid,
				 struct netlink_ext_ack *extack)
{
	enum rtl8365mb_frame_ingress accepted_frame, prev_accepted_frame;
	struct realtek_priv *priv = ds->priv;
	struct rtl8365mb_vlanmc prev_vlanmc = {0};
	u8 _unused_first_free_idx;
	u8 prev_vlanmc_idx;
	u8 vlanmc_idx;
	int ret;

	lockdep_assert_held(&priv->vlan_lock);

	/* Read the old PVID exclusively to undo in case of error */
	ret = rtl8365mb_vlan_get_pvid_mc(priv, port, &prev_vlanmc_idx,
					 &prev_vlanmc);
	if (ret) {
		dev_err(priv->dev, "Failed to read current VLAN MC\n");
		return ret;
	}

	ret = rtl8365mb_vlan_port_get_framefilter(priv, port,
						  &prev_accepted_frame);
	if (ret) {
		dev_err(priv->dev, "Failed to get current framefilter\n");
		return ret;
	}

	/* Find or allocate a new vlan MC and add port to members,
	 * although members are not checked by the HW in vlan MC.
	 */
	ret = rtl8365mb_vlan_mc_port_set(ds, port, vid, true, extack, true);
	if (ret)
		return ret;

	/* look for existing entry */
	ret = rtl8365mb_vlan_mc_find(priv, vid, &vlanmc_idx,
				     &_unused_first_free_idx);
	if (ret) {
		dev_err(priv->dev, "Failed to find a VLAN MC table index\n");
		goto undo_vlan_mc_port_set;
	}

	if (!vlanmc_idx) {
		dev_err(priv->dev, "VLAN should already exist in VLAN MC\n");
		ret = -ENOENT;
		goto undo_vlan_mc_port_set;
	}

	ret = rtl8365mb_vlan_port_set_pvid(priv, port, vlanmc_idx);
	if (ret) {
		dev_err(priv->dev, "Failed to set port PVID\n");
		goto undo_vlan_mc_port_set;
	}

	/* Changing accept frame is what enables PVID (if not enabled before) */
	accepted_frame = RTL8365MB_FRAME_TYPE_ANY_FRAME;
	ret = rtl8365mb_vlan_port_set_framefilter(priv, port, accepted_frame);
	if (ret) {
		dev_err(priv->dev, "Failed to set port frame filter\n");
		goto undo_vlan_port_set_pvid;
	}

	/* A VLAN can be added with PVID without removing from the old
	 * PVID VLAN. Clear PVID from the old VLAN MC (if needed).
	 */
	if (prev_vlanmc_idx && (prev_vlanmc.evid != vid)) {
		ret = rtl8365mb_vlan_mc_port_set(ds, port, prev_vlanmc.evid,
						 false, NULL, false);
		if (ret) {
			dev_err(priv->dev, "Failed to clear old VLAN MC\n");
			goto undo_set_framefilter;
		}
	}

	return 0;

undo_set_framefilter:
	(void)rtl8365mb_vlan_port_set_framefilter(priv, port,
						  prev_accepted_frame);

undo_vlan_port_set_pvid:
	(void)rtl8365mb_vlan_port_set_pvid(priv, port, prev_vlanmc_idx);

undo_vlan_mc_port_set:
	if (prev_vlanmc.evid != vid)
		(void)rtl8365mb_vlan_mc_port_set(ds, port, vid, false, NULL,
						 false);

	return ret;
}

/*
 * rtl8365mb_vlan_pvid_port_clear() - Remove a port's PVID configuration
 * @ds: dsa switch instance
 * @port: port index
 * @vid:  VLAN VID for PVID
 *
 * Resets the target port's hardware PVID allocation to 0. Cleans up and frees
 * the associated VLANMC entry if no other ports are referencing it.
 *
 * Context: Can sleep. Must be called with &priv->vlan_lock held.
 * Takes and releases &priv->map_lock.
 * Return: 0 on success, or a negative error code on failure.
 */
int rtl8365mb_vlan_pvid_port_clear(struct dsa_switch *ds, int port, u16 vid)
{
	enum rtl8365mb_frame_ingress accepted_frame, prev_accepted_frame;
	struct realtek_priv *priv = ds->priv;
	struct rtl8365mb_vlanmc vlanmc = {0};
	u8 vlanmc_idx;
	int ret;

	lockdep_assert_held(&priv->vlan_lock);

	ret = rtl8365mb_vlan_get_pvid_mc(priv, port, &vlanmc_idx,
					 &vlanmc);
	if (ret) {
		dev_err(priv->dev, "Failed to read current VLAN MC\n");
		return ret;
	}

	/* Port is not using PVID. Nothing to remove. */
	if (!vlanmc_idx)
		return 0;

	/* We are leaving a non PVID vlan, Nothing to remove. */
	if (vlanmc.evid != vid)
		return 0;

	ret = rtl8365mb_vlan_port_get_framefilter(priv, port,
						  &prev_accepted_frame);
	if (ret) {
		dev_err(priv->dev, "Failed to get current framefilter\n");
		return ret;
	}

	/* Changing accept frame is what really removes PVID. But only do
	 * that if we are filtering vlan
	 */
	if (dsa_port_is_vlan_filtering(dsa_to_port(ds, port))) {
		accepted_frame = RTL8365MB_FRAME_TYPE_TAGGED_ONLY;

		ret = rtl8365mb_vlan_port_set_framefilter(priv, port,
							  accepted_frame);
		if (ret) {
			dev_err(priv->dev, "Failed to set port frame filter\n");
			return ret;
		}
	} else {
		/* skip undo_set_framefilter */
		accepted_frame = prev_accepted_frame;
	}

	ret = rtl8365mb_vlan_port_set_pvid(priv, port, 0);
	if (ret) {
		dev_err(priv->dev, "Failed to set port PVID to 0\n");
		goto undo_set_framefilter;
	}

	/* Clears the VLAN MC membership and maybe VLAN MC entry if empty */
	ret = rtl8365mb_vlan_mc_port_set(ds, port, vlanmc.evid,
					 false, NULL, false);
	if (ret)
		goto undo_port_set_pvid;

	return 0;

undo_port_set_pvid:
	(void)rtl8365mb_vlan_port_set_pvid(priv, port, vlanmc_idx);

undo_set_framefilter:
	if (prev_accepted_frame != accepted_frame)
		(void)rtl8365mb_vlan_port_set_framefilter(priv, port,
							  prev_accepted_frame);

	return ret;
}