[PATCH net-next 4/6] net: dsa: motorcomm: Add TC prio support

[David Yang <mmyangfl@gmail.com>]

The HW supports fewer ACL priorities than TC does. Notice the default
priority for flower is 49152 and catchall filter has priority 65535, so
scale the priority accordingly and reject the ones that do not map onto
a hardware priority. Also adjust the register field name.

Signed-off-by: David Yang <mmyangfl@gmail.com>
---
 drivers/net/dsa/motorcomm/tc.c | 21 ++++++++++++++++++---
 drivers/net/dsa/motorcomm/tc.h |  5 +++--
 2 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/drivers/net/dsa/motorcomm/tc.c b/drivers/net/dsa/motorcomm/tc.c
index 0f6d7a4675fc..d919743d89c1 100644
--- a/drivers/net/dsa/motorcomm/tc.c
+++ b/drivers/net/dsa/motorcomm/tc.c
@@ -535,14 +535,14 @@ yt921x_acl_entries_find(struct yt921x_acl_entry *entries, unsigned int *sizep,
 }
 
 static void
-yt921x_acl_rule_set_ports(struct yt921x_acl_rule *aclrule, u16 ord,
+yt921x_acl_rule_set_ports(struct yt921x_acl_rule *aclrule, u16 prio,
 			  u16 ports_mask)
 {
 	struct yt921x_acl_entry *entries = aclrule->entries;
 
 	for (unsigned int i = 0; i < hweight8(aclrule->mask); i++) {
 		entries[i].key[1] |= YT921X_ACL_KEYb_SPORTS(ports_mask) |
-				     YT921X_ACL_KEYb_ORD(ord);
+				     YT921X_ACL_KEYb_PRIO(prio);
 	}
 }
 
@@ -1032,7 +1032,9 @@ yt921x_acl_rule_ext_parse_flow(struct yt921x_acl_rule_ext *ruleext, int port,
 			       const struct flow_cls_offload *cls, bool ingress,
 			       struct yt921x_priv *priv)
 {
+	const unsigned int scale = (U16_MAX + 1) / YT921X_ACL_PRIO_NUM;
 	struct netlink_ext_ack *extack = cls->common.extack;
+	u32 prio = cls->common.prio;
 	int res;
 
 	if (!ingress) {
@@ -1045,6 +1047,15 @@ yt921x_acl_rule_ext_parse_flow(struct yt921x_acl_rule_ext *ruleext, int port,
 		return -EOPNOTSUPP;
 	}
 
+	if (prio == U16_MAX) {
+		prio++;
+	} else if (prio % scale) {
+		NL_SET_ERR_MSG_FMT_MOD(extack,
+				       "Invalid priority %u, must be a multiple of %u",
+				       prio, scale);
+		return -EOPNOTSUPP;
+	}
+
 	res = yt921x_acl_rule_ext_parse_flow_action(ruleext, cls, priv, port);
 	if (res)
 		return res;
@@ -1052,7 +1063,11 @@ yt921x_acl_rule_ext_parse_flow(struct yt921x_acl_rule_ext *ruleext, int port,
 	if (res)
 		return res;
 
-	yt921x_acl_rule_set_ports(&ruleext->r, 0, BIT(port));
+	/* TC prio can't be 0 & TC lowest first -> HW highest first
+	 * map 1...ACL_PRIO_NUM to (ACL_PRIO_NUM - 1)...0
+	 */
+	prio = YT921X_ACL_PRIO_NUM - prio / scale;
+	yt921x_acl_rule_set_ports(&ruleext->r, prio, BIT(port));
 	ruleext->r.tag = cls->cookie;
 	ruleext->r.type = TC_SETUP_CLSFLOWER;
 	return 0;
diff --git a/drivers/net/dsa/motorcomm/tc.h b/drivers/net/dsa/motorcomm/tc.h
index 3d4a8a24f8a8..0eee2e3808c5 100644
--- a/drivers/net/dsa/motorcomm/tc.h
+++ b/drivers/net/dsa/motorcomm/tc.h
@@ -88,8 +88,8 @@
 #define   YT921X_ACL_ENTRY_GRPIDm(bin, x)		((x) << (4 * (bin) + 1))
 #define  YT921X_ACL_ENTRY_ENm(bin)		BIT(4 * (bin))
 #define YT921X_ACLn_KEYm(blk, bin)	(0x204000 + 0x200 * (bin) + 8 * (blk))
-#define  YT921X_ACL_KEYb_ORD_M			GENMASK(29, 21)
-#define   YT921X_ACL_KEYb_ORD(x)			FIELD_PREP(YT921X_ACL_KEYb_ORD_M, (x))
+#define  YT921X_ACL_KEYb_PRIO_M			GENMASK(29, 21)	/* highest first */
+#define   YT921X_ACL_KEYb_PRIO(x)			FIELD_PREP(YT921X_ACL_KEYb_PRIO_M, (x))
 #define  YT921X_ACL_KEYb_SPORTS_M		GENMASK(20, 10)
 #define   YT921X_ACL_KEYb_SPORTS(x)			FIELD_PREP(YT921X_ACL_KEYb_SPORTS_M, (x))
 #define  YT921X_ACL_KEYb_SPORTn(port)		BIT((port) + 10)
@@ -330,6 +330,7 @@ enum yt921x_l4_type {
 #define YT921X_SHAPE_CIR_MAX	((1 << 18) - 1)
 #define YT921X_SHAPE_CBS_MAX	((1 << 14) - 1)
 
+#define YT921X_ACL_PRIO_NUM	512
 #define YT921X_UDF_NUM		8
 
 struct yt921x_acl_entry {
-- 
2.53.0