From d84d7cf2a2b00203ac6e2261fd3474e6c0c954ca Mon Sep 17 00:00:00 2001
From: Tobias Waldekranz <tobias@waldekranz.com>
Date: Thu, 21 Mar 2024 19:12:15 +0100
Subject: [PATCH 21/33] net: dsa: Support EtherType based priority overrides
Organization: Wires

---
 include/net/dsa.h |  4 ++++
 net/dsa/user.c    | 56 +++++++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/include/net/dsa.h b/include/net/dsa.h
index 5cb456bf4639..c31d4e910f07 100644
--- a/include/net/dsa.h
+++ b/include/net/dsa.h
@@ -958,6 +958,10 @@ struct dsa_switch_ops {
 				     const u8 *sel, int nsel);
 	int	(*port_get_apptrust)(struct dsa_switch *ds, int port, u8 *sel,
 				     int *nsel);
+	int	(*port_add_etype_prio)(struct dsa_switch *ds, int port, u16 etype,
+				       u8 prio);
+	int	(*port_del_etype_prio)(struct dsa_switch *ds, int port, u16 etype,
+				       u8 prio);
 
 	/*
 	 * Suspend and resume
diff --git a/net/dsa/user.c b/net/dsa/user.c
index f59d66f0975d..58174ad612d2 100644
--- a/net/dsa/user.c
+++ b/net/dsa/user.c
@@ -2336,6 +2336,34 @@ dsa_user_dcbnl_add_dscp_prio(struct net_device *dev, struct dcb_app *app)
 	return 0;
 }
 
+static int __maybe_unused
+dsa_user_dcbnl_set_etype_prio(struct net_device *dev, struct dcb_app *app)
+{
+	struct dsa_port *dp = dsa_user_to_port(dev);
+	struct dsa_switch *ds = dp->ds;
+	unsigned long mask, new_prio;
+	int err;
+
+	if (!ds->ops->port_add_etype_prio)
+		return -EOPNOTSUPP;
+
+	err = dcb_ieee_setapp(dev, app);
+	if (err)
+		return err;
+
+	mask = dcb_ieee_getapp_mask(dev, app);
+	new_prio = __fls(mask);
+
+	err = ds->ops->port_add_etype_prio(ds, dp->index,
+					   app->protocol, new_prio);
+	if (err) {
+		dcb_ieee_delapp(dev, app);
+		return err;
+	}
+
+	return 0;
+}
+
 static int __maybe_unused dsa_user_dcbnl_ieee_setapp(struct net_device *dev,
 						     struct dcb_app *app)
 {
@@ -2345,7 +2373,7 @@ static int __maybe_unused dsa_user_dcbnl_ieee_setapp(struct net_device *dev,
 		case 0:
 			return dsa_user_dcbnl_set_default_prio(dev, app);
 		default:
-			return -EOPNOTSUPP;
+			return dsa_user_dcbnl_set_etype_prio(dev, app);
 		}
 		break;
 	case IEEE_8021QAZ_APP_SEL_DSCP:
@@ -2418,6 +2446,30 @@ dsa_user_dcbnl_del_dscp_prio(struct net_device *dev, struct dcb_app *app)
 	return 0;
 }
 
+static int __maybe_unused
+dsa_user_dcbnl_del_etype_prio(struct net_device *dev, struct dcb_app *app)
+{
+	struct dsa_port *dp = dsa_user_to_port(dev);
+	struct dsa_switch *ds = dp->ds;
+	int err;
+
+	if (!ds->ops->port_del_etype_prio)
+		return -EOPNOTSUPP;
+
+	err = dcb_ieee_delapp(dev, app);
+	if (err)
+		return err;
+
+	err = ds->ops->port_del_etype_prio(ds, dp->index,
+					   app->protocol, app->priority);
+	if (err) {
+		dcb_ieee_setapp(dev, app);
+		return err;
+	}
+
+	return 0;
+}
+
 static int __maybe_unused dsa_user_dcbnl_ieee_delapp(struct net_device *dev,
 						     struct dcb_app *app)
 {
@@ -2427,7 +2479,7 @@ static int __maybe_unused dsa_user_dcbnl_ieee_delapp(struct net_device *dev,
 		case 0:
 			return dsa_user_dcbnl_del_default_prio(dev, app);
 		default:
-			return -EOPNOTSUPP;
+			return dsa_user_dcbnl_del_etype_prio(dev, app);
 		}
 		break;
 	case IEEE_8021QAZ_APP_SEL_DSCP:
