[PATCH net-next 6/6] net: dsa: motorcomm: Add broadcast/multicast policers via tc police

[David Yang <mmyangfl@gmail.com>]

Recognize the following tc flower filters

    tc filter add dev lan1 ingress protocol all flower skip_sw \
        dst_mac ff:ff:ff:ff:ff:ff action police rate 8mbit burst 64k
    (and dst_mac 01:00:00:00:00:00/01:00:00:00:00:00)

and map it onto dedicated per-port storm control, following the sja1105
broadcast/multicast policer approach.

Signed-off-by: David Yang <mmyangfl@gmail.com>
---
 drivers/net/dsa/motorcomm/chip.c |  11 ++
 drivers/net/dsa/motorcomm/chip.h |   9 ++
 drivers/net/dsa/motorcomm/tc.c   | 175 ++++++++++++++++++++++++++++++-
 drivers/net/dsa/motorcomm/tc.h   |  17 +++
 4 files changed, 209 insertions(+), 3 deletions(-)

diff --git a/drivers/net/dsa/motorcomm/chip.c b/drivers/net/dsa/motorcomm/chip.c
index 0405f1441f2b..fb2514e01f91 100644
--- a/drivers/net/dsa/motorcomm/chip.c
+++ b/drivers/net/dsa/motorcomm/chip.c
@@ -2676,6 +2676,12 @@ static int yt921x_chip_setup_tc(struct yt921x_priv *priv)
 		return res;
 	priv->port_shape_slot_ns = ctrl * op_ns;
 
+	ctrl = YT921X_STORM_SLOT_DEF;
+	res = yt921x_reg_write(priv, YT921X_STORM_SLOT, ctrl);
+	if (res)
+		return res;
+	priv->storm_slot_ns = ctrl * 10 * NSEC_PER_USEC;
+
 	return 0;
 }
 
@@ -2781,6 +2787,11 @@ static int yt921x_chip_setup(struct yt921x_priv *priv)
 	if (res)
 		return res;
 
+	res = yt921x_reg_write(priv, YT921X_STORM_UNK_MCAST,
+			       YT921X_STORM_UNK_MCAST_PORTS_M);
+	if (res)
+		return res;
+
 	return 0;
 }
 
diff --git a/drivers/net/dsa/motorcomm/chip.h b/drivers/net/dsa/motorcomm/chip.h
index d7f2b22ae504..4b2a688663df 100644
--- a/drivers/net/dsa/motorcomm/chip.h
+++ b/drivers/net/dsa/motorcomm/chip.h
@@ -567,6 +567,12 @@ struct yt921x_mib {
 	u64 tx_oam;
 };
 
+enum yt921x_storm_type {
+	YT921X_STORM_BCAST,
+	YT921X_STORM_MCAST,
+	YT921X_STORM_NUM
+};
+
 struct yt921x_port {
 	unsigned char index;
 
@@ -586,6 +592,8 @@ struct yt921x_port {
 
 	unsigned short acl_cnt;
 
+	unsigned long storm_tags[YT921X_STORM_NUM];
+
 #if IS_ENABLED(CONFIG_NET_DSA_YT921X_LEDS)
 	unsigned char led_duty;
 	unsigned short led_cycle;
@@ -625,6 +633,7 @@ struct yt921x_priv {
 	const struct yt921x_info *info;
 	unsigned int meter_slot_ns;
 	unsigned int port_shape_slot_ns;
+	unsigned int storm_slot_ns;
 	/* cache of dsa_cpu_ports(ds) */
 	u16 cpu_ports_mask;
 	unsigned char cycle_ns;
diff --git a/drivers/net/dsa/motorcomm/tc.c b/drivers/net/dsa/motorcomm/tc.c
index 05d4a5957af9..3a0c6e0fbc52 100644
--- a/drivers/net/dsa/motorcomm/tc.c
+++ b/drivers/net/dsa/motorcomm/tc.c
@@ -182,6 +182,41 @@ yt921x_marker_tfm_shape(struct yt921x_marker *marker, u64 rate, u64 burst,
 				 priv, port, extack);
 }
 
+static int
+yt921x_marker_tfm_storm(struct yt921x_marker *marker,
+			const struct flow_action_police *police,
+			struct yt921x_priv *priv, int port)
+{
+	unsigned int slot_ns = priv->storm_slot_ns;
+	bool pkt_mode = !!police->rate_pkt_ps;
+	u64 burst;
+	u64 rate;
+	u32 cbs;
+	u32 cir;
+
+	rate = pkt_mode ? police->rate_pkt_ps : police->rate_bytes_ps;
+	burst = pkt_mode ? police->burst_pkt : police->burst;
+	if (pkt_mode) {
+		rate *= 1024;
+		burst *= 1024;
+	}
+
+	/* 1 token = 1 rate */
+	cir = div_u64(slot_ns * rate, NSEC_PER_SEC);
+	if (cir > YT921X_STORM_CIR_MAX)
+		return -ERANGE;
+	cbs = div_u64(slot_ns * burst, NSEC_PER_SEC);
+	if (cbs > YT921X_STORM_CBS_MAX)
+		return -ERANGE;
+
+	*marker = (typeof(*marker)){
+		.cir = cir,
+		.cbs = cbs,
+		.pkt_mode = pkt_mode,
+	};
+	return 0;
+}
+
 static int
 yt921x_police_validate(const struct flow_action_police *police,
 		       const struct flow_action *action,
@@ -550,6 +585,12 @@ struct yt921x_acl_rule_ext {
 	struct yt921x_acl_rule r;
 
 	struct yt921x_marker marker;
+	struct yt921x_marker marker_storm;
+
+	bool non_storm:1;
+	bool marker_storm_set:1;
+	bool storm_type_set:1;
+	unsigned char storm_type;
 };
 
 static int
@@ -597,6 +638,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 
 			entry->key[0] |= ntohl(match.key->dst);
 			entry->mask[0] |= ntohl(match.mask->dst);
+
+			ruleext->non_storm = true;
 		}
 
 		if (match.mask->src) {
@@ -607,6 +650,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 
 			entry->key[0] |= ntohl(match.key->src);
 			entry->mask[0] |= ntohl(match.mask->src);
+
+			ruleext->non_storm = true;
 		}
 	}
 
@@ -626,6 +671,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 
 			entry->key[0] |= ntohl(match.key->dst.s6_addr32[i]);
 			entry->mask[0] |= ntohl(match.mask->dst.s6_addr32[i]);
+
+			ruleext->non_storm = true;
 		}
 
 		for (unsigned int i = 0; i < 4; i++) {
@@ -639,6 +686,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 
 			entry->key[0] |= ntohl(match.key->src.s6_addr32[i]);
 			entry->mask[0] |= ntohl(match.mask->src.s6_addr32[i]);
+
+			ruleext->non_storm = true;
 		}
 	}
 
@@ -661,6 +710,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 				 ntohs(match.key->src);
 		entry->mask[0] |= (ntohs(match.mask->dst) << 16) |
 				  ntohs(match.mask->src);
+
+		ruleext->non_storm = true;
 	}
 
 	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_PORTS_RANGE)) {
@@ -684,21 +735,40 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 			entry->key[0] |= ntohs(match.key->tp_min.dst) << 16;
 			entry->key[1] |= YT921X_ACL_KEYb_L4_DPORT_RANGE_EN;
 			entry->mask[0] |= ntohs(match.key->tp_max.dst) << 16;
+
+			ruleext->non_storm = true;
 		}
 
 		if (match.mask->tp.src) {
 			entry->key[0] |= ntohs(match.key->tp_min.src);
 			entry->key[1] |= YT921X_ACL_KEYb_L4_SPORT_RANGE_EN;
 			entry->mask[0] |= ntohs(match.key->tp_max.src);
+
+			ruleext->non_storm = true;
 		}
 	}
 
 	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
+		static const u8 mcast_addr[ETH_ALEN] = {0x01};
 		struct flow_match_eth_addrs match;
 		u32 mask;
 
 		flow_rule_match_eth_addrs(rule, &match);
 
+		if (!ruleext->non_storm) {
+			if (is_broadcast_ether_addr(match.mask->dst) &&
+			    is_broadcast_ether_addr(match.key->dst)) {
+				ruleext->storm_type = YT921X_STORM_BCAST;
+				ruleext->storm_type_set = true;
+			} else if (ether_addr_equal(match.mask->dst, mcast_addr) &&
+				   is_multicast_ether_addr(match.key->dst)) {
+				ruleext->storm_type = YT921X_STORM_MCAST;
+				ruleext->storm_type_set = true;
+			} else {
+				ruleext->non_storm = true;
+			}
+		}
+
 		mask = ethaddr_hi4_to_u32(match.mask->dst);
 		if (mask) {
 			entry = yt921x_acl_entries_new(entries, &size,
@@ -781,6 +851,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 				entry->key[0] |= ntohs(match.key->n_proto);
 				entry->mask[0] |= ntohs(match.mask->n_proto);
 			}
+
+			ruleext->non_storm = true;
 		}
 
 		if (match.mask->ip_proto) {
@@ -820,6 +892,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 				entry->key[0] |= YT921X_ACL_BINa_MISC_IP_PROTO(match.key->ip_proto);
 				entry->mask[0] |= YT921X_ACL_BINa_MISC_IP_PROTO(match.mask->ip_proto);
 			}
+
+			ruleext->non_storm = true;
 		}
 	}
 
@@ -839,6 +913,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 								  set);
 			if (!size)
 				goto err;
+
+			ruleext->non_storm = true;
 		}
 		if (match.mask->flags & FLOW_DIS_FIRST_FRAG) {
 			bool set = match.key->flags & FLOW_DIS_FIRST_FRAG;
@@ -847,6 +923,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 								 set);
 			if (!size)
 				goto err;
+
+			ruleext->non_storm = true;
 		}
 	}
 
@@ -869,6 +947,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 
 			entry->key[0] |= YT921X_ACL_BINa_MISC_TOS(match.key->tos);
 			entry->mask[0] |= YT921X_ACL_BINa_MISC_TOS(match.mask->tos);
+
+			ruleext->non_storm = true;
 		}
 	}
 
@@ -889,6 +969,8 @@ yt921x_acl_rule_ext_parse_flow_entries(struct yt921x_acl_rule_ext *ruleext,
 
 			entry->key[0] |= YT921X_ACL_BINa_MISC_TCP_FLAGS(ntohs(match.key->flags));
 			entry->mask[0] |= YT921X_ACL_BINa_MISC_TCP_FLAGS(ntohs(match.mask->flags));
+
+			ruleext->non_storm = true;
 		}
 	}
 
@@ -923,7 +1005,10 @@ yt921x_acl_rule_ext_parse_flow_action(struct yt921x_acl_rule_ext *ruleext,
 	int res;
 
 	memset(action, 0, 3 * sizeof(*action));
-	flow_action_for_each(i, act, flow_action)
+	flow_action_for_each(i, act, flow_action) {
+		if (act->id != FLOW_ACTION_POLICE)
+			ruleext->non_storm = true;
+
 		switch (act->id) {
 		case FLOW_ACTION_ACCEPT:
 		case FLOW_ACTION_DROP:
@@ -996,6 +1081,16 @@ yt921x_acl_rule_ext_parse_flow_action(struct yt921x_acl_rule_ext *ruleext,
 			if (res)
 				return res;
 
+			if (!ruleext->non_storm) {
+				res = yt921x_marker_tfm_storm(&ruleext->marker_storm,
+							      police, priv,
+							      port);
+				if (!res)
+					ruleext->marker_storm_set = true;
+				else
+					ruleext->non_storm = true;
+			}
+
 			res = yt921x_marker_tfm_police(&ruleext->marker, police,
 						       0, priv, port, extack);
 			if (res)
@@ -1022,6 +1117,7 @@ yt921x_acl_rule_ext_parse_flow_action(struct yt921x_acl_rule_ext *ruleext,
 				     YT921X_ACL_ACTc_FWD_TRAP;
 			break;
 		}
+	}
 
 	ruleext->r.sw_assisted = !cls->common.skip_sw;
 	return 0;
@@ -1056,6 +1152,9 @@ yt921x_acl_rule_ext_parse_flow(struct yt921x_acl_rule_ext *ruleext, int port,
 		return -EOPNOTSUPP;
 	}
 
+	ruleext->non_storm = false;
+	ruleext->marker_storm_set = false;
+	ruleext->storm_type_set = false;
 	res = yt921x_acl_rule_ext_parse_flow_action(ruleext, cls, priv, port);
 	if (res)
 		return res;
@@ -1488,6 +1587,76 @@ yt921x_acl_add(struct yt921x_priv *priv,
 	return res;
 }
 
+static int
+yt921x_storm_del(struct yt921x_priv *priv, int port, unsigned int type)
+{
+	struct yt921x_port *pp = &priv->ports[port];
+	int res;
+
+	res = yt921x_reg_write(priv, YT921X_STORM_CTRLnm(type, port), 0);
+	/* the kernel never rolls back on failure */
+
+	pp->storm_tags[type] = 0;
+
+	return res;
+}
+
+static int
+yt921x_storm_add(struct yt921x_priv *priv, int port, unsigned long cookie,
+		 unsigned int type, const struct yt921x_marker *marker,
+		 struct netlink_ext_ack *extack)
+{
+	struct yt921x_port *pp = &priv->ports[port];
+	u32 ctrl;
+	int res;
+
+	if (pp->storm_tags[type])
+		return -ENOSPC;
+
+	ctrl = YT921X_STORM_CTRL_EN | YT921X_STORM_CTRL_CBS(marker->cbs) |
+	       YT921X_STORM_CTRL_CIR(marker->cir);
+	if (marker->pkt_mode)
+		ctrl |= YT921X_STORM_CTRL_PKT_MODE;
+	res = yt921x_reg_write(priv, YT921X_STORM_CTRLnm(type, port), ctrl);
+	if (res)
+		return res;
+
+	pp->storm_tags[type] = cookie;
+	return 0;
+}
+
+static int
+yt921x_flower_del(struct yt921x_priv *priv, int port, unsigned long cookie)
+{
+	struct yt921x_port *pp = &priv->ports[port];
+
+	for (unsigned int type = YT921X_STORM_BCAST; type < YT921X_STORM_NUM;
+	     type++)
+		if (pp->storm_tags[type] == cookie)
+			return yt921x_storm_del(priv, port, type);
+
+	return yt921x_acl_del(priv, TC_SETUP_CLSFLOWER, cookie);
+}
+
+static int
+yt921x_flower_add(struct yt921x_priv *priv, int port,
+		  const struct yt921x_acl_rule_ext *ruleext,
+		  struct netlink_ext_ack *extack)
+{
+	int res;
+
+	if (!ruleext->non_storm && ruleext->marker_storm_set &&
+	    ruleext->storm_type_set) {
+		res = yt921x_storm_add(priv, port, ruleext->r.tag,
+				       ruleext->storm_type,
+				       &ruleext->marker_storm, extack);
+		if (res != -ENOSPC)
+			return res;
+	}
+
+	return yt921x_acl_add(priv, ruleext, extack);
+}
+
 int
 yt921x_dsa_cls_flower_stats(struct dsa_switch *ds, int port,
 			    struct flow_cls_offload *cls, bool ingress)
@@ -1511,7 +1680,7 @@ yt921x_dsa_cls_flower_del(struct dsa_switch *ds, int port,
 	int res;
 
 	mutex_lock(&priv->reg_lock);
-	res = yt921x_acl_del(priv, TC_SETUP_CLSFLOWER, cls->cookie);
+	res = yt921x_flower_del(priv, port, cls->cookie);
 	mutex_unlock(&priv->reg_lock);
 
 	return res;
@@ -1532,7 +1701,7 @@ yt921x_dsa_cls_flower_add(struct dsa_switch *ds, int port,
 		return res;
 
 	mutex_lock(&priv->reg_lock);
-	res = yt921x_acl_add(priv, &ruleext, extack);
+	res = yt921x_flower_add(priv, port, &ruleext, extack);
 	mutex_unlock(&priv->reg_lock);
 
 	return res;
diff --git a/drivers/net/dsa/motorcomm/tc.h b/drivers/net/dsa/motorcomm/tc.h
index f2ea0ac32f7e..a9107f97a1b9 100644
--- a/drivers/net/dsa/motorcomm/tc.h
+++ b/drivers/net/dsa/motorcomm/tc.h
@@ -263,12 +263,26 @@ enum yt921x_l4_type {
 
 #define YT921X_PORTn_RATE(port)		(0x220000 + 4 * (port))
 #define  YT921X_PORT_RATE_GAP_VALUE		GENMASK(4, 0)	/* default 20 */
+#define YT921X_STORM_SLOT		0x220100
+#define  YT921X_STORM_SLOT_SLOT_M		GENMASK(11, 0)
 #define YT921X_METER_SLOT		0x220104
 #define  YT921X_METER_SLOT_SLOT_M		GENMASK(11, 0)
 #define YT921X_PORTn_METER(port)	(0x220108 + 4 * (port))
 #define  YT921X_PORT_METER_EN			BIT(4)
 #define  YT921X_PORT_METER_ID_M			GENMASK(3, 0)
 #define   YT921X_PORT_METER_ID(x)			FIELD_PREP(YT921X_PORT_METER_ID_M, (x))
+#define YT921X_STORM_UNK_MCAST		0x220140
+#define  YT921X_STORM_UNK_MCAST_PORTS_M		GENMASK(10, 0)
+#define  YT921X_STORM_UNK_MCAST_PORT(x)		BIT(x)
+#define YT921X_STORM_CTRLnm(type, port)	(0x220200 + 4 * (11 * (type) + (port)))
+#define  YT921X_STORM_CTRL_CIR_M		GENMASK(31, 13)
+#define   YT921X_STORM_CTRL_CIR(x)			FIELD_PREP(YT921X_STORM_CTRL_CIR_M, (x))
+#define  YT921X_STORM_CTRL_CBS_M		GENMASK(12, 3)
+#define   YT921X_STORM_CTRL_CBS(x)			FIELD_PREP(YT921X_STORM_CTRL_CBS_M, (x))
+#define  YT921X_STORM_CTRL_BYTE_MODE_INCLUDE_GAP	BIT(2)	/* +GAP_VALUE bytes each packet */
+#define  YT921X_STORM_CTRL_PKT_MODE		BIT(1)	/* 0: byte rate mode */
+#define  YT921X_STORM_CTRL_EN			BIT(0)
+#define YT921X_STORM_STATnm(type, port)	(0x220400 + 4 * (11 * (type) + (port)))
 #define YT921X_METERn_CTRL(x)		(0x220800 + 0x10 * (x))
 #define  YT921X_METER_CTRLc_METER_EN		BIT(14)
 #define  YT921X_METER_CTRLc_TOKEN_OVERFLOW_EN	BIT(13)	/* RFC4115: yellow use unused green bw */
@@ -339,6 +353,9 @@ enum yt921x_l4_type {
 #define YT921X_SHAPE_UNIT_MAX	((1 << 3) - 1)
 #define YT921X_SHAPE_CIR_MAX	((1 << 18) - 1)
 #define YT921X_SHAPE_CBS_MAX	((1 << 14) - 1)
+#define YT921X_STORM_SLOT_DEF	100
+#define YT921X_STORM_CIR_MAX	((1 << 19) - 1)
+#define YT921X_STORM_CBS_MAX	((1 << 10) - 1)
 
 #define YT921X_ACL_PRIO_NUM	512
 #define YT921X_UDF_NUM		8
-- 
2.53.0