net: mscc: ocelot: add MAC Merge layer support for VSC9959

Felix (VSC9959) has a DEV_GMII:MM_CONFIG block composed of 2 registers
(ENABLE_CONFIG and VERIF_CONFIG). Because the MAC Merge statistics and
pMAC statistics are already in the Ocelot switch lib even if just Felix
supports them, I'm adding support for the whole MAC Merge layer in the
common Ocelot library too.

There is an interrupt (shared with the PTP interrupt) which signals
changes to the MM verification state. This is done because the
preemptible traffic classes should be committed to hardware only once
the verification procedure has declared the link partner of being
capable of receiving preemptible frames.

We implement ethtool getters and setters for the MAC Merge layer state.
The "TX enabled" and "verify status" are taken from the IRQ handler,
using a mutex to ensure serialized access.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

4 files changed, 231 insertions, 4 deletions

diff --git a/drivers/net/ethernet/mscc/Makefile b/drivers/net/ethernet/mscc/Makefile
index 5d435a565d4c..16987b72dfc0 100644
--- a/drivers/net/ethernet/mscc/Makefile
+++ b/drivers/net/ethernet/mscc/Makefile
@@ -5,6 +5,7 @@ mscc_ocelot_switch_lib-y := \
 	ocelot_devlink.o \
 	ocelot_flower.o \
 	ocelot_io.o \
+	ocelot_mm.o \
 	ocelot_police.o \
 	ocelot_ptp.o \
 	ocelot_stats.o \
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index da56f9bfeaf0..c060b03f7e27 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -2738,10 +2738,8 @@ int ocelot_init(struct ocelot *ocelot)
 		return -ENOMEM;
 
 	ret = ocelot_stats_init(ocelot);
-	if (ret) {
-		destroy_workqueue(ocelot->owq);
-		return ret;
-	}
+	if (ret)
+		goto err_stats_init;
 
 	INIT_LIST_HEAD(&ocelot->multicast);
 	INIT_LIST_HEAD(&ocelot->pgids);
@@ -2756,6 +2754,12 @@ int ocelot_init(struct ocelot *ocelot)
 	if (ocelot->ops->psfp_init)
 		ocelot->ops->psfp_init(ocelot);
 
+	if (ocelot->mm_supported) {
+		ret = ocelot_mm_init(ocelot);
+		if (ret)
+			goto err_mm_init;
+	}
+
 	for (port = 0; port < ocelot->num_phys_ports; port++) {
 		/* Clear all counters (5 groups) */
 		ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
@@ -2853,6 +2857,12 @@ int ocelot_init(struct ocelot *ocelot)
 				 ANA_CPUQ_8021_CFG, i);
 
 	return 0;
+
+err_mm_init:
+	ocelot_stats_deinit(ocelot);
+err_stats_init:
+	destroy_workqueue(ocelot->owq);
+	return ret;
 }
 EXPORT_SYMBOL(ocelot_init);
 
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index 70dbd9c4e512..e9a0179448bf 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -109,6 +109,8 @@ void ocelot_mirror_put(struct ocelot *ocelot);
 int ocelot_stats_init(struct ocelot *ocelot);
 void ocelot_stats_deinit(struct ocelot *ocelot);
 
+int ocelot_mm_init(struct ocelot *ocelot);
+
 extern struct notifier_block ocelot_netdevice_nb;
 extern struct notifier_block ocelot_switchdev_nb;
 extern struct notifier_block ocelot_switchdev_blocking_nb;
diff --git a/drivers/net/ethernet/mscc/ocelot_mm.c b/drivers/net/ethernet/mscc/ocelot_mm.c
new file mode 100644
index 000000000000..08820f2341a1
--- /dev/null
+++ b/drivers/net/ethernet/mscc/ocelot_mm.c
@@ -0,0 +1,214 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MIT)
+/*
+ * Hardware library for MAC Merge Layer and Frame Preemption on TSN-capable
+ * switches (VSC9959)
+ *
+ * Copyright 2022-2023 NXP
+ */
+#include <linux/ethtool.h>
+#include <soc/mscc/ocelot.h>
+#include <soc/mscc/ocelot_dev.h>
+#include <soc/mscc/ocelot_qsys.h>
+
+#include "ocelot.h"
+
+static const char *
+mm_verify_state_to_string(enum ethtool_mm_verify_status state)
+{
+	switch (state) {
+	case ETHTOOL_MM_VERIFY_STATUS_INITIAL:
+		return "INITIAL";
+	case ETHTOOL_MM_VERIFY_STATUS_VERIFYING:
+		return "VERIFYING";
+	case ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED:
+		return "SUCCEEDED";
+	case ETHTOOL_MM_VERIFY_STATUS_FAILED:
+		return "FAILED";
+	case ETHTOOL_MM_VERIFY_STATUS_DISABLED:
+		return "DISABLED";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+static enum ethtool_mm_verify_status ocelot_mm_verify_status(u32 val)
+{
+	switch (DEV_MM_STAT_MM_STATUS_PRMPT_VERIFY_STATE_X(val)) {
+	case 0:
+		return ETHTOOL_MM_VERIFY_STATUS_INITIAL;
+	case 1:
+		return ETHTOOL_MM_VERIFY_STATUS_VERIFYING;
+	case 2:
+		return ETHTOOL_MM_VERIFY_STATUS_SUCCEEDED;
+	case 3:
+		return ETHTOOL_MM_VERIFY_STATUS_FAILED;
+	case 4:
+		return ETHTOOL_MM_VERIFY_STATUS_DISABLED;
+	default:
+		return ETHTOOL_MM_VERIFY_STATUS_UNKNOWN;
+	}
+}
+
+void ocelot_port_mm_irq(struct ocelot *ocelot, int port)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	struct ocelot_mm_state *mm = &ocelot->mm[port];
+	enum ethtool_mm_verify_status verify_status;
+	u32 val;
+
+	mutex_lock(&mm->lock);
+
+	val = ocelot_port_readl(ocelot_port, DEV_MM_STATUS);
+
+	verify_status = ocelot_mm_verify_status(val);
+	if (mm->verify_status != verify_status) {
+		dev_dbg(ocelot->dev,
+			"Port %d MAC Merge verification state %s\n",
+			port, mm_verify_state_to_string(verify_status));
+		mm->verify_status = verify_status;
+	}
+
+	if (val & DEV_MM_STAT_MM_STATUS_PRMPT_ACTIVE_STICKY) {
+		mm->tx_active = !!(val & DEV_MM_STAT_MM_STATUS_PRMPT_ACTIVE_STATUS);
+
+		dev_dbg(ocelot->dev, "Port %d TX preemption %s\n",
+			port, mm->tx_active ? "active" : "inactive");
+	}
+
+	if (val & DEV_MM_STAT_MM_STATUS_UNEXP_RX_PFRM_STICKY) {
+		dev_err(ocelot->dev,
+			"Unexpected P-frame received on port %d while verification was unsuccessful or not yet verified\n",
+			port);
+	}
+
+	if (val & DEV_MM_STAT_MM_STATUS_UNEXP_TX_PFRM_STICKY) {
+		dev_err(ocelot->dev,
+			"Unexpected P-frame requested to be transmitted on port %d while verification was unsuccessful or not yet verified, or MM_TX_ENA=0\n",
+			port);
+	}
+
+	ocelot_port_writel(ocelot_port, val, DEV_MM_STATUS);
+
+	mutex_unlock(&mm->lock);
+}
+EXPORT_SYMBOL_GPL(ocelot_port_mm_irq);
+
+int ocelot_port_set_mm(struct ocelot *ocelot, int port,
+		       struct ethtool_mm_cfg *cfg,
+		       struct netlink_ext_ack *extack)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	u32 mm_enable = 0, verify_disable = 0, add_frag_size;
+	struct ocelot_mm_state *mm;
+	int err;
+
+	if (!ocelot->mm_supported)
+		return -EOPNOTSUPP;
+
+	mm = &ocelot->mm[port];
+
+	err = ethtool_mm_frag_size_min_to_add(cfg->tx_min_frag_size,
+					      &add_frag_size, extack);
+	if (err)
+		return err;
+
+	if (cfg->pmac_enabled)
+		mm_enable |= DEV_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA;
+
+	if (cfg->tx_enabled)
+		mm_enable |= DEV_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA;
+
+	if (!cfg->verify_enabled)
+		verify_disable = DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_DIS;
+
+	mutex_lock(&mm->lock);
+
+	ocelot_port_rmwl(ocelot_port, mm_enable,
+			 DEV_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA |
+			 DEV_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA,
+			 DEV_MM_ENABLE_CONFIG);
+
+	ocelot_port_rmwl(ocelot_port, verify_disable |
+			 DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME(cfg->verify_time),
+			 DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_DIS |
+			 DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_M,
+			 DEV_MM_VERIF_CONFIG);
+
+	ocelot_rmw_rix(ocelot,
+		       QSYS_PREEMPTION_CFG_MM_ADD_FRAG_SIZE(add_frag_size),
+		       QSYS_PREEMPTION_CFG_MM_ADD_FRAG_SIZE_M,
+		       QSYS_PREEMPTION_CFG,
+		       port);
+
+	mutex_unlock(&mm->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ocelot_port_set_mm);
+
+int ocelot_port_get_mm(struct ocelot *ocelot, int port,
+		       struct ethtool_mm_state *state)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	struct ocelot_mm_state *mm;
+	u32 val, add_frag_size;
+
+	if (!ocelot->mm_supported)
+		return -EOPNOTSUPP;
+
+	mm = &ocelot->mm[port];
+
+	mutex_lock(&mm->lock);
+
+	val = ocelot_port_readl(ocelot_port, DEV_MM_ENABLE_CONFIG);
+	state->pmac_enabled = !!(val & DEV_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA);
+	state->tx_enabled = !!(val & DEV_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA);
+
+	val = ocelot_port_readl(ocelot_port, DEV_MM_VERIF_CONFIG);
+	state->verify_time = DEV_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_X(val);
+	state->max_verify_time = 128;
+
+	val = ocelot_read_rix(ocelot, QSYS_PREEMPTION_CFG, port);
+	add_frag_size = QSYS_PREEMPTION_CFG_MM_ADD_FRAG_SIZE_X(val);
+	state->tx_min_frag_size = ethtool_mm_frag_size_add_to_min(add_frag_size);
+	state->rx_min_frag_size = ETH_ZLEN;
+
+	state->verify_status = mm->verify_status;
+	state->tx_active = mm->tx_active;
+
+	mutex_unlock(&mm->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ocelot_port_get_mm);
+
+int ocelot_mm_init(struct ocelot *ocelot)
+{
+	struct ocelot_port *ocelot_port;
+	struct ocelot_mm_state *mm;
+	int port;
+
+	if (!ocelot->mm_supported)
+		return 0;
+
+	ocelot->mm = devm_kcalloc(ocelot->dev, ocelot->num_phys_ports,
+				  sizeof(*ocelot->mm), GFP_KERNEL);
+	if (!ocelot->mm)
+		return -ENOMEM;
+
+	for (port = 0; port < ocelot->num_phys_ports; port++) {
+		u32 val;
+
+		mm = &ocelot->mm[port];
+		mutex_init(&mm->lock);
+		ocelot_port = ocelot->ports[port];
+
+		/* Update initial status variable for the
+		 * verification state machine
+		 */
+		val = ocelot_port_readl(ocelot_port, DEV_MM_STATUS);
+		mm->verify_status = ocelot_mm_verify_status(val);
+	}
+
+	return 0;
+}