[PATCH v1 net-next 5/5] net: dsa: felix: add police action for tc flower offload

[Xiaoliang Yang <xiaoliang.yang_1@nxp.com>]

This patch add police action to set flow meter table which is defined
in IEEE802.1Qci. Flow metering is two rates two buckets and three color
marker to policing the frames, we only enable one rate one bucket in
this patch.

Flow metering shares a same policer pool with VCAP policers, it calls
ocelot_vcap_policer_add() and ocelot_vcap_policer_del() to set flow
meter table.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/dsa/ocelot/felix_flower.c | 32 +++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/net/dsa/ocelot/felix_flower.c b/drivers/net/dsa/ocelot/felix_flower.c
index 71894dcc0af2..d58a2357bab1 100644
--- a/drivers/net/dsa/ocelot/felix_flower.c
+++ b/drivers/net/dsa/ocelot/felix_flower.c
@@ -363,6 +363,8 @@ static void felix_list_stream_filter_del(struct ocelot *ocelot, u32 index)
 		if (tmp->index == index) {
 			if (tmp->sg_valid)
 				felix_list_gate_del(ocelot, tmp->sgid);
+			if (tmp->fm_valid)
+				ocelot_vcap_policer_del(ocelot, tmp->fmid);
 
 			z = refcount_dec_and_test(&tmp->refcount);
 			if (z) {
@@ -466,6 +468,8 @@ static int felix_psfp_set(struct ocelot *ocelot,
 	if (ret) {
 		if (sfi->sg_valid)
 			felix_list_gate_del(ocelot, sfi->sgid);
+		if (sfi->fm_valid)
+			ocelot_vcap_policer_del(ocelot, sfi->fmid);
 		return ret;
 	}
 
@@ -559,7 +563,9 @@ int felix_flower_stream_replace(struct ocelot *ocelot, int port,
 	struct felix_streamid stream = {0};
 	struct felix_stream_gate_conf *sgi;
 	const struct flow_action_entry *a;
+	struct ocelot_policer pol;
 	int ret, size, i;
+	u64 rate, burst;
 	u32 index;
 
 	ret = felix_flower_parse_key(f, &stream);
@@ -595,6 +601,32 @@ int felix_flower_stream_replace(struct ocelot *ocelot, int port,
 			stream.sfid_valid = 1;
 			kfree(sgi);
 			break;
+		case FLOW_ACTION_POLICE:
+			if (f->common.chain_index != OCELOT_PSFP_CHAIN) {
+				NL_SET_ERR_MSG_MOD(extack,
+						   "Police action only be offloaded to PSFP chain");
+				return -EOPNOTSUPP;
+			}
+
+			index = a->police.index + FELIX_POLICER_PSFP_BASE;
+			if (index > FELIX_POLICER_PSFP_MAX)
+				return -EINVAL;
+
+			rate = a->police.rate_bytes_ps;
+			burst = rate * PSCHED_NS2TICKS(a->police.burst);
+			pol = (struct ocelot_policer) {
+				.burst = div_u64(burst, PSCHED_TICKS_PER_SEC),
+				.rate = div_u64(rate, 1000) * 8,
+			};
+			ret = ocelot_vcap_policer_add(ocelot, index, &pol);
+			if (ret)
+				return ret;
+
+			sfi.fm_valid = 1;
+			sfi.fmid = index;
+			sfi.maxsdu = a->police.mtu;
+			stream.sfid_valid = 1;
+			break;
 		default:
 			return -EOPNOTSUPP;
 		}
-- 
2.17.1