[RFC PATCH net-next 1/2] net: dsa: allow setting port-based QoS priority using tc matchall skbedit

[Vladimir Oltean <olteanv@gmail.com>]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

In Time Sensitive Networking it is a common and simple use case to
configure switches to give all traffic from an attached station the same
priority, without requiring those stations to use VLAN PCP or IP DSCP to
signal the priority that they want. Many pieces of hardware support this
feature via a port-based default priority. We can model this in Linux
through a matchall action on the ingress qdisc of the port, plus a
skbedit priority action with the desired priority.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 include/net/dsa.h |  8 ++++++
 net/dsa/slave.c   | 72 +++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 80 insertions(+)

diff --git a/include/net/dsa.h b/include/net/dsa.h
index c9a3dd7588df..4b774287d255 100644
--- a/include/net/dsa.h
+++ b/include/net/dsa.h
@@ -155,6 +155,7 @@ struct dsa_switch_tree {
 enum dsa_port_mall_action_type {
 	DSA_PORT_MALL_MIRROR,
 	DSA_PORT_MALL_POLICER,
+	DSA_PORT_MALL_SKBEDIT,
 };
 
 /* TC mirroring entry */
@@ -169,6 +170,10 @@ struct dsa_mall_policer_tc_entry {
 	u64 rate_bytes_per_sec;
 };
 
+struct dsa_mall_skbedit_tc_entry {
+	int priority;
+};
+
 /* TC matchall entry */
 struct dsa_mall_tc_entry {
 	struct list_head list;
@@ -177,6 +182,7 @@ struct dsa_mall_tc_entry {
 	union {
 		struct dsa_mall_mirror_tc_entry mirror;
 		struct dsa_mall_policer_tc_entry policer;
+		struct dsa_mall_skbedit_tc_entry skbedit;
 	};
 };
 
@@ -612,6 +618,8 @@ struct dsa_switch_ops {
 	int	(*port_policer_add)(struct dsa_switch *ds, int port,
 				    struct dsa_mall_policer_tc_entry *policer);
 	void	(*port_policer_del)(struct dsa_switch *ds, int port);
+	int	(*port_priority_set)(struct dsa_switch *ds, int port,
+				     struct dsa_mall_skbedit_tc_entry *skbedit);
 	int	(*port_setup_tc)(struct dsa_switch *ds, int port,
 				 enum tc_setup_type type, void *type_data);
 
diff --git a/net/dsa/slave.c b/net/dsa/slave.c
index 5d7f6cada6a8..82cba26e2a8f 100644
--- a/net/dsa/slave.c
+++ b/net/dsa/slave.c
@@ -1018,6 +1018,66 @@ dsa_slave_add_cls_matchall_police(struct net_device *dev,
 	return err;
 }
 
+static int
+dsa_slave_add_cls_matchall_skbedit(struct net_device *dev,
+				   struct tc_cls_matchall_offload *cls,
+				   bool ingress)
+{
+	struct netlink_ext_ack *extack = cls->common.extack;
+	struct dsa_port *dp = dsa_slave_to_port(dev);
+	struct dsa_slave_priv *p = netdev_priv(dev);
+	struct dsa_mall_skbedit_tc_entry *skbedit;
+	struct dsa_mall_tc_entry *mall_tc_entry;
+	struct dsa_switch *ds = dp->ds;
+	struct flow_action_entry *act;
+	int err;
+
+	if (!ds->ops->port_priority_set) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Port priority not implemented");
+		return -EOPNOTSUPP;
+	}
+
+	if (!ingress) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Only supported on ingress qdisc");
+		return -EOPNOTSUPP;
+	}
+
+	if (!flow_action_basic_hw_stats_check(&cls->rule->action,
+					      cls->common.extack))
+		return -EOPNOTSUPP;
+
+	list_for_each_entry(mall_tc_entry, &p->mall_tc_list, list) {
+		if (mall_tc_entry->type == DSA_PORT_MALL_SKBEDIT) {
+			NL_SET_ERR_MSG_MOD(extack,
+					   "Only one port priority allowed");
+			return -EEXIST;
+		}
+	}
+
+	act = &cls->rule->action.entries[0];
+
+	mall_tc_entry = kzalloc(sizeof(*mall_tc_entry), GFP_KERNEL);
+	if (!mall_tc_entry)
+		return -ENOMEM;
+
+	mall_tc_entry->cookie = cls->cookie;
+	mall_tc_entry->type = DSA_PORT_MALL_SKBEDIT;
+	skbedit = &mall_tc_entry->skbedit;
+	skbedit->priority = act->priority;
+
+	err = ds->ops->port_priority_set(ds, dp->index, skbedit);
+	if (err) {
+		kfree(mall_tc_entry);
+		return err;
+	}
+
+	list_add_tail(&mall_tc_entry->list, &p->mall_tc_list);
+
+	return err;
+}
+
 static int dsa_slave_add_cls_matchall(struct net_device *dev,
 				      struct tc_cls_matchall_offload *cls,
 				      bool ingress)
@@ -1031,6 +1091,9 @@ static int dsa_slave_add_cls_matchall(struct net_device *dev,
 	else if (flow_offload_has_one_action(&cls->rule->action) &&
 		 cls->rule->action.entries[0].id == FLOW_ACTION_POLICE)
 		err = dsa_slave_add_cls_matchall_police(dev, cls, ingress);
+	else if (flow_offload_has_one_action(&cls->rule->action) &&
+		 cls->rule->action.entries[0].id == FLOW_ACTION_PRIORITY)
+		err = dsa_slave_add_cls_matchall_skbedit(dev, cls, ingress);
 
 	return err;
 }
@@ -1058,6 +1121,15 @@ static void dsa_slave_del_cls_matchall(struct net_device *dev,
 		if (ds->ops->port_policer_del)
 			ds->ops->port_policer_del(ds, dp->index);
 		break;
+	case DSA_PORT_MALL_SKBEDIT:
+		if (ds->ops->port_priority_set) {
+			struct dsa_mall_skbedit_tc_entry *skbedit;
+
+			skbedit = &mall_tc_entry->skbedit;
+			skbedit->priority = 0;
+			ds->ops->port_priority_set(ds, dp->index, skbedit);
+		}
+		break;
 	default:
 		WARN_ON(1);
 	}
-- 
2.25.1