From ea2d865a2101317e22c60143e891d643feac8e8c Mon Sep 17 00:00:00 2001
From: Tobias Waldekranz <tobias@waldekranz.com>
Date: Fri, 22 Mar 2024 16:15:43 +0100
Subject: [PATCH 10/27] net: dsa: mv88e6xxx: Support EtherType based priority
 overrides
Organization: Wires

---
 drivers/net/dsa/mv88e6xxx/chip.c    | 64 +++++++++++++++++++++++++++++
 drivers/net/dsa/mv88e6xxx/chip.h    | 21 ++++++++++
 drivers/net/dsa/mv88e6xxx/global2.c | 56 ++++++++++++++++++++++++-
 drivers/net/dsa/mv88e6xxx/global2.h |  4 ++
 drivers/net/dsa/mv88e6xxx/port.c    | 46 +++++++++++++++++++++
 drivers/net/dsa/mv88e6xxx/port.h    | 20 +++++++--
 6 files changed, 207 insertions(+), 4 deletions(-)

diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c
index c833b7e44d8b..c48369663425 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.c
+++ b/drivers/net/dsa/mv88e6xxx/chip.c
@@ -1715,6 +1715,11 @@ static int mv88e6xxx_rmu_setup(struct mv88e6xxx_chip *chip)
 
 static int mv88e6xxx_pot_setup(struct mv88e6xxx_chip *chip)
 {
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(chip->qpri_po); i++)
+		refcount_set(&chip->qpri_po[i].refcnt, 0);
+
 	if (chip->info->ops->pot_clear)
 		return chip->info->ops->pot_clear(chip);
 
@@ -3427,6 +3432,7 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 
 	chip->ports[port].chip = chip;
 	chip->ports[port].port = port;
+	refcount_set(&chip->ports[port].etype.refcnt, 0);
 
 	err = mv88e6xxx_port_setup_mac(chip, port, LINK_UNFORCED,
 				       SPEED_UNFORCED, DUPLEX_UNFORCED,
@@ -6599,6 +6605,7 @@ static struct mv88e6xxx_chip *mv88e6xxx_alloc_chip(struct device *dev)
 	chip->dev = dev;
 
 	mutex_init(&chip->reg_lock);
+	mutex_init(&chip->arb_lock);
 	INIT_LIST_HEAD(&chip->mdios);
 	idr_init(&chip->policies);
 	INIT_LIST_HEAD(&chip->msts);
@@ -7138,6 +7145,61 @@ static int mv88e6xxx_crosschip_lag_leave(struct dsa_switch *ds, int sw_index,
 	return err_sync ? : err_pvt;
 }
 
+static int mv88e6xxx_port_add_etype_prio(struct dsa_switch *ds, int port,
+					 u16 etype, u8 prio)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mv88e6xxx_reg_lock(chip);
+	err = mv88e6xxx_port_claim_ether_type(chip, port, etype);
+	if (err)
+		goto err;
+
+	err = mv88e6xxx_g2_claim_qpri_po(chip,
+					 MV88E6XXX_G2_PRIO_OVERRIDE_PTR_ETYPE,
+					 prio);
+	if (err)
+		goto err_relinquish_etype;
+
+	mv88e6xxx_reg_unlock(chip);
+	return 0;
+
+err_relinquish_etype:
+	mv88e6xxx_port_relinquish_ether_type(chip, port);
+err:
+	mv88e6xxx_reg_unlock(chip);
+	return err;
+}
+
+static int mv88e6xxx_port_del_etype_prio(struct dsa_switch *ds, int port,
+					 u16 etype, u8 prio)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mv88e6xxx_reg_lock(chip);
+	err = mv88e6xxx_g2_relinquish_qpri_po(chip,
+					      MV88E6XXX_G2_PRIO_OVERRIDE_PTR_ETYPE);
+	if (err)
+		goto err;
+
+	err = mv88e6xxx_port_relinquish_ether_type(chip, port);
+	if (err)
+		goto err_claim_po;
+
+	mv88e6xxx_reg_unlock(chip);
+	return 0;
+
+err_claim_po:
+	mv88e6xxx_g2_claim_qpri_po(chip,
+				   MV88E6XXX_G2_PRIO_OVERRIDE_PTR_ETYPE,
+				   prio);
+err:
+	mv88e6xxx_reg_unlock(chip);
+	return err;
+}
+
 static const struct phylink_mac_ops mv88e6xxx_phylink_mac_ops = {
 	.mac_select_pcs		= mv88e6xxx_mac_select_pcs,
 	.mac_prepare		= mv88e6xxx_mac_prepare,
@@ -7207,6 +7269,8 @@ static const struct dsa_switch_ops mv88e6xxx_switch_ops = {
 	.crosschip_lag_change	= mv88e6xxx_crosschip_lag_change,
 	.crosschip_lag_join	= mv88e6xxx_crosschip_lag_join,
 	.crosschip_lag_leave	= mv88e6xxx_crosschip_lag_leave,
+	.port_add_etype_prio	= mv88e6xxx_port_add_etype_prio,
+	.port_del_etype_prio	= mv88e6xxx_port_del_etype_prio,
 };
 
 static int mv88e6xxx_register_switch(struct mv88e6xxx_chip *chip)
diff --git a/drivers/net/dsa/mv88e6xxx/chip.h b/drivers/net/dsa/mv88e6xxx/chip.h
index 32c87ab754d1..25edc2534979 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.h
+++ b/drivers/net/dsa/mv88e6xxx/chip.h
@@ -293,6 +293,11 @@ struct mv88e6xxx_port {
 
 	/* MacAuth Bypass control flag */
 	bool mab;
+
+	struct {
+		refcount_t refcnt;
+		u16 proto;
+	} etype;
 };
 
 enum mv88e6xxx_region_id {
@@ -331,6 +336,11 @@ struct mv88e6xxx_hw_stat {
 	int type;
 };
 
+struct mv88e6xxx_po {
+	refcount_t refcnt;
+	u8 pri;
+};
+
 struct mv88e6xxx_led;
 
 struct mv88e6xxx_chip {
@@ -348,6 +358,14 @@ struct mv88e6xxx_chip {
 	/* This mutex protects the access to the switch registers */
 	struct mutex reg_lock;
 
+	/* This mutex protects arbitration of hardware resources which
+	 * may be allocated by multiple kernel consumers. As an
+	 * example, the dcb(8) facility manages priority overrides
+	 * independently per interface, whereas the hardware only
+	 * supports a single priority per chip.
+	 */
+	struct mutex arb_lock;
+
 	/* The MII bus and the address on the bus that is used to
 	 * communication with the switch
 	 */
@@ -437,6 +455,9 @@ struct mv88e6xxx_chip {
 
 	/* Bridge MST to SID mappings */
 	struct list_head msts;
+
+	/* Queue priority overrides */
+	struct mv88e6xxx_po qpri_po[16];
 };
 
 struct mv88e6xxx_bus_ops {
diff --git a/drivers/net/dsa/mv88e6xxx/global2.c b/drivers/net/dsa/mv88e6xxx/global2.c
index b2b5f6ba438f..2c52903e8992 100644
--- a/drivers/net/dsa/mv88e6xxx/global2.c
+++ b/drivers/net/dsa/mv88e6xxx/global2.c
@@ -315,7 +315,7 @@ int mv88e6xxx_g2_atu_stats_get(struct mv88e6xxx_chip *chip, u16 *stats)
 static int mv88e6xxx_g2_pot_write(struct mv88e6xxx_chip *chip, int pointer,
 				  u8 data)
 {
-	u16 val = (pointer << 8) | (data & 0x7);
+	u16 val = (pointer << 8) | (data & 0xf);
 
 	return mv88e6xxx_g2_write(chip, MV88E6XXX_G2_PRIO_OVERRIDE,
 				  MV88E6XXX_G2_PRIO_OVERRIDE_UPDATE | val);
@@ -335,6 +335,60 @@ int mv88e6xxx_g2_pot_clear(struct mv88e6xxx_chip *chip)
 	return err;
 }
 
+int mv88e6xxx_g2_claim_qpri_po(struct mv88e6xxx_chip *chip, u8 pointer,
+			       u8 qpri)
+{
+	struct mv88e6xxx_po *po;
+	int err = 0;
+
+	if (pointer > ARRAY_SIZE(chip->qpri_po))
+		return -EINVAL;
+
+	if (qpri > 7)
+		return -ERANGE;
+
+	po = &chip->qpri_po[pointer];
+
+	mutex_lock(&chip->arb_lock);
+	if (refcount_read(&po->refcnt)) {
+		if (qpri == po->pri)
+			refcount_inc(&po->refcnt);
+		else
+			err = -EBUSY;
+	} else {
+		err = mv88e6xxx_g2_pot_write(chip, pointer,
+					     MV88E6XXX_G2_PRIO_OVERRIDE_QFPRIEN | qpri);
+		if (!err) {
+			refcount_set(&po->refcnt, 1);
+			po->pri = qpri;
+		}
+	}
+	mutex_unlock(&chip->arb_lock);
+
+	return err;
+}
+
+int mv88e6xxx_g2_relinquish_qpri_po(struct mv88e6xxx_chip *chip, u8 pointer)
+{
+	struct mv88e6xxx_po *po;
+	int err = 0;
+
+	if (pointer > ARRAY_SIZE(chip->qpri_po))
+		return -EINVAL;
+
+	po = &chip->qpri_po[pointer];
+
+	mutex_lock(&chip->arb_lock);
+	if (refcount_dec_and_test(&po->refcnt)) {
+		err = mv88e6xxx_g2_pot_write(chip, pointer, 0);
+		if (err)
+			refcount_set(&po->refcnt, 1);
+	}
+	mutex_unlock(&chip->arb_lock);
+
+	return err;
+}
+
 /* Offset 0x14: EEPROM Command
  * Offset 0x15: EEPROM Data (for 16-bit data access)
  * Offset 0x15: EEPROM Addr (for 8-bit data access)
diff --git a/drivers/net/dsa/mv88e6xxx/global2.h b/drivers/net/dsa/mv88e6xxx/global2.h
index 3663645621c9..496269c26c50 100644
--- a/drivers/net/dsa/mv88e6xxx/global2.h
+++ b/drivers/net/dsa/mv88e6xxx/global2.h
@@ -138,6 +138,7 @@
 #define MV88E6XXX_G2_PRIO_OVERRIDE_UPDATE	0x8000
 #define MV88E6XXX_G2_PRIO_OVERRIDE_FPRISET	0x1000
 #define MV88E6XXX_G2_PRIO_OVERRIDE_PTR_MASK	0x0f00
+#define MV88E6XXX_G2_PRIO_OVERRIDE_PTR_ETYPE	0xc
 #define MV88E6352_G2_PRIO_OVERRIDE_QPRIAVBEN	0x0080
 #define MV88E6352_G2_PRIO_OVERRIDE_DATAAVB_MASK	0x0030
 #define MV88E6XXX_G2_PRIO_OVERRIDE_QFPRIEN	0x0008
@@ -359,6 +360,9 @@ int mv88e6185_g2_mgmt_rsvd2cpu(struct mv88e6xxx_chip *chip);
 int mv88e6352_g2_mgmt_rsvd2cpu(struct mv88e6xxx_chip *chip);
 
 int mv88e6xxx_g2_pot_clear(struct mv88e6xxx_chip *chip);
+int mv88e6xxx_g2_claim_qpri_po(struct mv88e6xxx_chip *chip, u8 pointer,
+			       u8 qpri);
+int mv88e6xxx_g2_relinquish_qpri_po(struct mv88e6xxx_chip *chip, u8 pointer);
 
 int mv88e6xxx_g2_trunk_mask_write(struct mv88e6xxx_chip *chip, int num,
 				  bool hash, u16 mask);
diff --git a/drivers/net/dsa/mv88e6xxx/port.c b/drivers/net/dsa/mv88e6xxx/port.c
index aa5d312d8aa5..84900814c456 100644
--- a/drivers/net/dsa/mv88e6xxx/port.c
+++ b/drivers/net/dsa/mv88e6xxx/port.c
@@ -1558,6 +1558,52 @@ int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
 	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_ETH_TYPE, etype);
 }
 
+int mv88e6xxx_port_claim_ether_type(struct mv88e6xxx_chip *chip, int port,
+				    u16 etype)
+{
+	struct mv88e6xxx_port *p = &chip->ports[port];
+	int err = 0;
+
+	if (!chip->info->ops->port_set_ether_type)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&chip->arb_lock);
+	if (refcount_read(&p->etype.refcnt)) {
+		if (etype == p->etype.proto)
+			refcount_inc(&p->etype.refcnt);
+		else
+			err = -EBUSY;
+	} else {
+		err = chip->info->ops->port_set_ether_type(chip, port, etype);
+		if (!err) {
+			refcount_set(&p->etype.refcnt, 1);
+			p->etype.proto = etype;
+		}
+	}
+	mutex_unlock(&chip->arb_lock);
+
+	return err;
+}
+
+int mv88e6xxx_port_relinquish_ether_type(struct mv88e6xxx_chip *chip, int port)
+{
+	struct mv88e6xxx_port *p = &chip->ports[port];
+	int err = 0;
+
+	if (!chip->info->ops->port_set_ether_type)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&chip->arb_lock);
+	if (refcount_dec_and_test(&p->etype.refcnt)) {
+		err = chip->info->ops->port_set_ether_type(chip, port,
+							   MV88E6XXX_PORT_ETH_TYPE_DEFAULT);
+		if (err)
+			refcount_set(&p->etype.refcnt, 1);
+	}
+	mutex_unlock(&chip->arb_lock);
+	return err;
+}
+
 /* Offset 0x16: LED Control Register */
 
 int mv88e6393x_port_led_write(struct mv88e6xxx_chip *chip, int port,
diff --git a/drivers/net/dsa/mv88e6xxx/port.h b/drivers/net/dsa/mv88e6xxx/port.h
index 7157864dc20f..7edb04c41ac8 100644
--- a/drivers/net/dsa/mv88e6xxx/port.h
+++ b/drivers/net/dsa/mv88e6xxx/port.h
@@ -286,7 +286,18 @@
 
 /* Offset 0x0F: Port Special Ether Type */
 #define MV88E6XXX_PORT_ETH_TYPE		0x0f
-#define MV88E6XXX_PORT_ETH_TYPE_DEFAULT	0x9100
+
+/* When MV88E6XXX_PORT_ETH_TYPE is used to override the queue priority
+ * of matching packets, the override applies to all the chip's
+ * ports. However, if the packet also matches a traffic class that the
+ * hardware classifies as being of higher priority, then that class's
+ * override configuration is used instead, even if that override is
+ * disabled. The class which is one level higher than EType is PPPoE,
+ * so by using that as the default value when no other value has been
+ * requested, we effectively disable the EType class for that port.
+ */
+#define MV88E6XXX_PORT_ETH_TYPE_DEFAULT	ETH_P_PPP_DISC
+
 
 /* Offset 0x10: InDiscards Low Counter */
 #define MV88E6XXX_PORT_IN_DISCARD_LO	0x10
@@ -426,8 +437,13 @@ int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port,
 int mv88e6393x_port_set_policy(struct mv88e6xxx_chip *chip, int port,
 			       enum mv88e6xxx_policy_mapping mapping,
 			       enum mv88e6xxx_policy_action action);
+int mv88e6393x_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
+				   u16 etype);
 int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
 				  u16 etype);
+int mv88e6xxx_port_claim_ether_type(struct mv88e6xxx_chip *chip, int port,
+				    u16 etype);
+int mv88e6xxx_port_relinquish_ether_type(struct mv88e6xxx_chip *chip, int port);
 int mv88e6393x_port_led_write(struct mv88e6xxx_chip *chip, int port,
 			      unsigned int pointer, u16 data);
 int mv88e6393x_port_led_read(struct mv88e6xxx_chip *chip, int port,
@@ -438,8 +454,6 @@ int mv88e6393x_set_egress_port(struct mv88e6xxx_chip *chip,
 int mv88e6393x_port_set_upstream_port(struct mv88e6xxx_chip *chip, int port,
 				      int upstream_port);
 int mv88e6393x_port_mgmt_rsvd2cpu(struct mv88e6xxx_chip *chip);
-int mv88e6393x_port_set_ether_type(struct mv88e6xxx_chip *chip, int port,
-				   u16 etype);
 int mv88e6xxx_port_set_message_port(struct mv88e6xxx_chip *chip, int port,
 				    bool message_port);
 int mv88e6xxx_port_set_trunk(struct mv88e6xxx_chip *chip, int port,
