[PATCH v4 net-next 0/8] net: dsa: felix: psfp support on vsc9959

[PATCH v4 net-next 0/8] net: dsa: felix: psfp support on vsc9959

[Xiaoliang Yang]

VSC9959 hardware supports Per-Stream Filtering and Policing(PSFP).
This patch series add PSFP support on tc flower offload of ocelot
driver. Use chain 30000 to distinguish PSFP from VCAP blocks. Add gate
and police set to support PSFP in VSC9959 driver.

v3->v4 changes:
 - Introduce vsc9959_psfp_sfi_table_get() function in patch where it is
   used to fix compile warning.
 - Store MAC entry type before FRER set, and recover it after FRER
   disabled.

v2->v3 changes:
 - Reorder first two patches. Export struct ocelot_mact_entry, then add
   ocelot_mact_lookup() and ocelot_mact_write() functions.
 - Add PSFP list to struct ocelot, and init it by using
   ocelot->ops->psfp_init().

v1->v2 changes:
 - Use tc flower offload of ocelot driver to support PSFP add and delete.
 - Add PSFP tables add/del functions in felix_vsc9959.c.
 - Use list_for_each_entry to simplify the code.

Vladimir Oltean (2):
  net: mscc: ocelot: export struct ocelot_mact_entry
  net: mscc: ocelot: add MAC table write and lookup operations

Xiaoliang Yang (6):
  net: mscc: ocelot: set vcap IS2 chain to goto PSFP chain
  net: mscc: ocelot: add gate and police action offload to PSFP
  net: dsa: felix: support psfp filter on vsc9959
  net: dsa: felix: add stream gate settings for psfp
  net: mscc: ocelot: use index to set vcap policer
  net: dsa: felix: use vcap policer to set flow meter for psfp

 drivers/net/dsa/ocelot/felix.c             |   2 +
 drivers/net/dsa/ocelot/felix.h             |   2 +
 drivers/net/dsa/ocelot/felix_vsc9959.c     | 697 ++++++++++++++++++++-
 drivers/net/ethernet/mscc/ocelot.c         |  56 +-
 drivers/net/ethernet/mscc/ocelot.h         |  13 -
 drivers/net/ethernet/mscc/ocelot_flower.c  |  74 ++-
 drivers/net/ethernet/mscc/ocelot_vcap.c    | 103 +--
 drivers/net/ethernet/mscc/ocelot_vsc7514.c |   7 +
 include/soc/mscc/ocelot.h                  |  49 +-
 include/soc/mscc/ocelot_ana.h              |  10 +
 include/soc/mscc/ocelot_vcap.h             |   1 +
 11 files changed, 942 insertions(+), 72 deletions(-)

-- 
2.17.1

[PATCH v4 net-next 1/8] net: mscc: ocelot: export struct ocelot_mact_entry

[Xiaoliang Yang]

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

Felix DSA needs to use this struct to export MAC table write and lookup
operations as well, for its stream identification functions, so export
them in preparation of that.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c |  6 ------
 drivers/net/ethernet/mscc/ocelot.h | 13 -------------
 include/soc/mscc/ocelot.h          | 19 +++++++++++++++++++
 3 files changed, 19 insertions(+), 19 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index c581b955efb3..39a5cee81677 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -14,12 +14,6 @@
 #define TABLE_UPDATE_SLEEP_US 10
 #define TABLE_UPDATE_TIMEOUT_US 100000
 
-struct ocelot_mact_entry {
-	u8 mac[ETH_ALEN];
-	u16 vid;
-	enum macaccess_entry_type type;
-};
-
 static inline u32 ocelot_mact_read_macaccess(struct ocelot *ocelot)
 {
 	return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index 1952d6a1b98a..a77050b13d18 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -54,19 +54,6 @@ struct ocelot_dump_ctx {
 	int idx;
 };
 
-/* MAC table entry types.
- * ENTRYTYPE_NORMAL is subject to aging.
- * ENTRYTYPE_LOCKED is not subject to aging.
- * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
- * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
- */
-enum macaccess_entry_type {
-	ENTRYTYPE_NORMAL = 0,
-	ENTRYTYPE_LOCKED,
-	ENTRYTYPE_MACv4,
-	ENTRYTYPE_MACv6,
-};
-
 /* A (PGID) port mask structure, encoding the 2^ocelot->num_phys_ports
  * possibilities of egress port masks for L2 multicast traffic.
  * For a switch with 9 user ports, there are 512 possible port masks, but the
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index 06706a9fd5b1..32b3c60d6046 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -698,6 +698,25 @@ struct ocelot_skb_cb {
 	u8 ts_id;
 };
 
+/* MAC table entry types.
+ * ENTRYTYPE_NORMAL is subject to aging.
+ * ENTRYTYPE_LOCKED is not subject to aging.
+ * ENTRYTYPE_MACv4 is not subject to aging. For IPv4 multicast.
+ * ENTRYTYPE_MACv6 is not subject to aging. For IPv6 multicast.
+ */
+enum macaccess_entry_type {
+	ENTRYTYPE_NORMAL = 0,
+	ENTRYTYPE_LOCKED,
+	ENTRYTYPE_MACv4,
+	ENTRYTYPE_MACv6,
+};
+
+struct ocelot_mact_entry {
+	u8 mac[ETH_ALEN];
+	u16 vid;
+	enum macaccess_entry_type type;
+};
+
 #define OCELOT_SKB_CB(skb) \
 	((struct ocelot_skb_cb *)((skb)->cb))
 
-- 
2.17.1

Re: [PATCH v4 net-next 1/8] net: mscc: ocelot: export struct ocelot_mact_entry

[Florian Fainelli]

On 9/22/2021 3:51 AM, Xiaoliang Yang wrote:
> From: Vladimir Oltean <vladimir.oltean@nxp.com>
> 
> Felix DSA needs to use this struct to export MAC table write and lookup
> operations as well, for its stream identification functions, so export
> them in preparation of that.
> 
> Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
> Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>

Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
-- 
Florian

[PATCH v4 net-next 2/8] net: mscc: ocelot: add MAC table write and lookup operations

[Xiaoliang Yang]

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

ocelot_mact_write() can be used for directly modifying an FDB entry
situated at a given row and column, as opposed to the current
ocelot_mact_learn() which calculates the row and column indices
automatically (based on a 11-bit hash derived from the {DMAC, VID} key).

ocelot_mact_lookup() can be used to retrieve the row and column at which
an FDB entry with the given {DMAC, VID} key is found.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c | 47 ++++++++++++++++++++++++++++++
 include/soc/mscc/ocelot.h          |  6 ++++
 2 files changed, 53 insertions(+)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 39a5cee81677..689c800caa54 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -96,6 +96,53 @@ int ocelot_mact_forget(struct ocelot *ocelot,
 }
 EXPORT_SYMBOL(ocelot_mact_forget);
 
+int ocelot_mact_lookup(struct ocelot *ocelot, const unsigned char mac[ETH_ALEN],
+		       unsigned int vid, int *row, int *col)
+{
+	int val;
+
+	ocelot_mact_select(ocelot, mac, vid);
+
+	/* Issue a read command with MACACCESS_VALID=1. */
+	ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
+		     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
+		     ANA_TABLES_MACACCESS);
+
+	if (ocelot_mact_wait_for_completion(ocelot))
+		return -ETIMEDOUT;
+
+	/* Read back the entry flags */
+	val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
+	if (!(val & ANA_TABLES_MACACCESS_VALID))
+		return -ENOENT;
+
+	ocelot_field_read(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
+	ocelot_field_read(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
+
+	return 0;
+}
+EXPORT_SYMBOL(ocelot_mact_lookup);
+
+/* Like ocelot_mact_learn, except at a specific row and col. */
+void ocelot_mact_write(struct ocelot *ocelot, int port,
+		       const struct ocelot_mact_entry *entry,
+		       int row, int col)
+{
+	ocelot_mact_select(ocelot, entry->mac, entry->vid);
+
+	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_M_INDEX, row);
+	ocelot_field_write(ocelot, ANA_TABLES_MACTINDX_BUCKET, col);
+
+	ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
+		     ANA_TABLES_MACACCESS_ENTRYTYPE(entry->type) |
+		     ANA_TABLES_MACACCESS_DEST_IDX(port) |
+		     ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
+		     ANA_TABLES_MACACCESS);
+
+	ocelot_mact_wait_for_completion(ocelot);
+}
+EXPORT_SYMBOL(ocelot_mact_write);
+
 static void ocelot_mact_init(struct ocelot *ocelot)
 {
 	/* Configure the learning mode entries attributes:
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index 32b3c60d6046..babaa5b0c026 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -923,6 +923,12 @@ void ocelot_phylink_mac_link_up(struct ocelot *ocelot, int port,
 				bool tx_pause, bool rx_pause,
 				unsigned long quirks);
 
+int ocelot_mact_lookup(struct ocelot *ocelot, const unsigned char mac[ETH_ALEN],
+		       unsigned int vid, int *row, int *col);
+void ocelot_mact_write(struct ocelot *ocelot, int port,
+		       const struct ocelot_mact_entry *entry,
+		       int row, int col);
+
 #if IS_ENABLED(CONFIG_BRIDGE_MRP)
 int ocelot_mrp_add(struct ocelot *ocelot, int port,
 		   const struct switchdev_obj_mrp *mrp);
-- 
2.17.1

Re: [PATCH v4 net-next 2/8] net: mscc: ocelot: add MAC table write and lookup operations

[Florian Fainelli]

On 9/22/2021 3:51 AM, Xiaoliang Yang wrote:
> From: Vladimir Oltean <vladimir.oltean@nxp.com>
> 
> ocelot_mact_write() can be used for directly modifying an FDB entry
> situated at a given row and column, as opposed to the current
> ocelot_mact_learn() which calculates the row and column indices
> automatically (based on a 11-bit hash derived from the {DMAC, VID} key).
> 
> ocelot_mact_lookup() can be used to retrieve the row and column at which
> an FDB entry with the given {DMAC, VID} key is found.
> 
> Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
> Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>

Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
-- 
Florian

[PATCH v4 net-next 3/8] net: mscc: ocelot: set vcap IS2 chain to goto PSFP chain

[Xiaoliang Yang]

Some chips in the ocelot series such as VSC9959 support Per-Stream
Filtering and Policing(PSFP), which is processing after VCAP blocks.
We set this block on chain 30000 and set vcap IS2 chain to goto PSFP
chain if hardware support.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot_flower.c | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot_flower.c b/drivers/net/ethernet/mscc/ocelot_flower.c
index 8b843d3c9189..ce812194e44c 100644
--- a/drivers/net/ethernet/mscc/ocelot_flower.c
+++ b/drivers/net/ethernet/mscc/ocelot_flower.c
@@ -20,6 +20,9 @@
 	(1 * VCAP_BLOCK + (lookup) * VCAP_LOOKUP)
 #define VCAP_IS2_CHAIN(lookup, pag)	\
 	(2 * VCAP_BLOCK + (lookup) * VCAP_LOOKUP + (pag))
+/* PSFP chain and block ID */
+#define PSFP_BLOCK_ID			OCELOT_NUM_VCAP_BLOCKS
+#define OCELOT_PSFP_CHAIN		(3 * VCAP_BLOCK)
 
 static int ocelot_chain_to_block(int chain, bool ingress)
 {
@@ -46,6 +49,9 @@ static int ocelot_chain_to_block(int chain, bool ingress)
 			if (chain == VCAP_IS2_CHAIN(lookup, pag))
 				return VCAP_IS2;
 
+	if (chain == OCELOT_PSFP_CHAIN)
+		return PSFP_BLOCK_ID;
+
 	return -EOPNOTSUPP;
 }
 
@@ -84,7 +90,8 @@ static bool ocelot_is_goto_target_valid(int goto_target, int chain,
 			goto_target == VCAP_IS1_CHAIN(1) ||
 			goto_target == VCAP_IS1_CHAIN(2) ||
 			goto_target == VCAP_IS2_CHAIN(0, 0) ||
-			goto_target == VCAP_IS2_CHAIN(1, 0));
+			goto_target == VCAP_IS2_CHAIN(1, 0) ||
+			goto_target == OCELOT_PSFP_CHAIN);
 
 	if (chain == VCAP_IS1_CHAIN(0))
 		return (goto_target == VCAP_IS1_CHAIN(1));
@@ -111,7 +118,11 @@ static bool ocelot_is_goto_target_valid(int goto_target, int chain,
 		if (chain == VCAP_IS2_CHAIN(0, pag))
 			return (goto_target == VCAP_IS2_CHAIN(1, pag));
 
-	/* VCAP IS2 lookup 1 cannot jump anywhere */
+	/* VCAP IS2 lookup 1 can goto to PSFP block if hardware support */
+	for (pag = 0; pag < VCAP_IS2_NUM_PAG; pag++)
+		if (chain == VCAP_IS2_CHAIN(1, pag))
+			return (goto_target == OCELOT_PSFP_CHAIN);
+
 	return false;
 }
 
@@ -353,7 +364,7 @@ static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 
 	if (filter->goto_target == -1) {
 		if ((filter->block_id == VCAP_IS2 && filter->lookup == 1) ||
-		    chain == 0) {
+		    chain == 0 || filter->block_id == PSFP_BLOCK_ID) {
 			allow_missing_goto_target = true;
 		} else {
 			NL_SET_ERR_MSG_MOD(extack, "Missing GOTO action");
-- 
2.17.1

[PATCH v4 net-next 4/8] net: mscc: ocelot: add gate and police action offload to PSFP

[Xiaoliang Yang]

PSFP support gate and police action. This patch add the gate and police
action to flower parse action, check chain ID to determine which block
to offload. Adding psfp callback functions to add, delete and update gate
and police in PSFP table if hardware supports it.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c        |  3 ++
 drivers/net/ethernet/mscc/ocelot_flower.c | 52 ++++++++++++++++++++++-
 include/soc/mscc/ocelot.h                 |  5 +++
 include/soc/mscc/ocelot_vcap.h            |  1 +
 4 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 689c800caa54..565dc5cc42c3 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -2147,6 +2147,9 @@ int ocelot_init(struct ocelot *ocelot)
 	ocelot_vcap_init(ocelot);
 	ocelot_cpu_port_init(ocelot);
 
+	if (ocelot->ops->psfp_init)
+		ocelot->ops->psfp_init(ocelot);
+
 	for (port = 0; port < ocelot->num_phys_ports; port++) {
 		/* Clear all counters (5 groups) */
 		ocelot_write(ocelot, SYS_STAT_CFG_STAT_VIEW(port) |
diff --git a/drivers/net/ethernet/mscc/ocelot_flower.c b/drivers/net/ethernet/mscc/ocelot_flower.c
index ce812194e44c..daeaee99933d 100644
--- a/drivers/net/ethernet/mscc/ocelot_flower.c
+++ b/drivers/net/ethernet/mscc/ocelot_flower.c
@@ -220,10 +220,14 @@ static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 			filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
 			break;
 		case FLOW_ACTION_POLICE:
+			if (filter->block_id == PSFP_BLOCK_ID) {
+				filter->type = OCELOT_PSFP_FILTER_OFFLOAD;
+				break;
+			}
 			if (filter->block_id != VCAP_IS2 ||
 			    filter->lookup != 0) {
 				NL_SET_ERR_MSG_MOD(extack,
-						   "Police action can only be offloaded to VCAP IS2 lookup 0");
+						   "Police action can only be offloaded to VCAP IS2 lookup 0 or PSFP");
 				return -EOPNOTSUPP;
 			}
 			if (filter->goto_target != -1) {
@@ -356,6 +360,14 @@ static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 			filter->action.pcp_a_val = a->vlan.prio;
 			filter->type = OCELOT_VCAP_FILTER_OFFLOAD;
 			break;
+		case FLOW_ACTION_GATE:
+			if (filter->block_id != PSFP_BLOCK_ID) {
+				NL_SET_ERR_MSG_MOD(extack,
+						   "Gate action can only be offloaded to PSFP chain");
+				return -EOPNOTSUPP;
+			}
+			filter->type = OCELOT_PSFP_FILTER_OFFLOAD;
+			break;
 		default:
 			NL_SET_ERR_MSG_MOD(extack, "Cannot offload action");
 			return -EOPNOTSUPP;
@@ -646,6 +658,10 @@ static int ocelot_flower_parse(struct ocelot *ocelot, int port, bool ingress,
 	if (ret)
 		return ret;
 
+	/* PSFP filter need to parse key by stream identification function. */
+	if (filter->type == OCELOT_PSFP_FILTER_OFFLOAD)
+		return 0;
+
 	return ocelot_flower_parse_key(ocelot, port, ingress, f, filter);
 }
 
@@ -718,6 +734,15 @@ int ocelot_cls_flower_replace(struct ocelot *ocelot, int port,
 	if (filter->type == OCELOT_VCAP_FILTER_DUMMY)
 		return ocelot_vcap_dummy_filter_add(ocelot, filter);
 
+	if (filter->type == OCELOT_PSFP_FILTER_OFFLOAD) {
+		kfree(filter);
+		if (ocelot->ops->psfp_filter_add)
+			return ocelot->ops->psfp_filter_add(ocelot, f);
+
+		NL_SET_ERR_MSG_MOD(extack, "PSFP chain is not supported in HW");
+		return -EOPNOTSUPP;
+	}
+
 	return ocelot_vcap_filter_add(ocelot, filter, f->common.extack);
 }
 EXPORT_SYMBOL_GPL(ocelot_cls_flower_replace);
@@ -733,6 +758,13 @@ int ocelot_cls_flower_destroy(struct ocelot *ocelot, int port,
 	if (block_id < 0)
 		return 0;
 
+	if (block_id == PSFP_BLOCK_ID) {
+		if (ocelot->ops->psfp_filter_del)
+			return ocelot->ops->psfp_filter_del(ocelot, f);
+
+		return -EOPNOTSUPP;
+	}
+
 	block = &ocelot->block[block_id];
 
 	filter = ocelot_vcap_block_find_filter_by_id(block, f->cookie, true);
@@ -751,12 +783,25 @@ int ocelot_cls_flower_stats(struct ocelot *ocelot, int port,
 {
 	struct ocelot_vcap_filter *filter;
 	struct ocelot_vcap_block *block;
+	struct flow_stats stats;
 	int block_id, ret;
 
 	block_id = ocelot_chain_to_block(f->common.chain_index, ingress);
 	if (block_id < 0)
 		return 0;
 
+	if (block_id == PSFP_BLOCK_ID) {
+		if (ocelot->ops->psfp_stats_get) {
+			ret = ocelot->ops->psfp_stats_get(ocelot, f, &stats);
+			if (ret)
+				return ret;
+
+			goto stats_update;
+		}
+
+		return -EOPNOTSUPP;
+	}
+
 	block = &ocelot->block[block_id];
 
 	filter = ocelot_vcap_block_find_filter_by_id(block, f->cookie, true);
@@ -767,7 +812,10 @@ int ocelot_cls_flower_stats(struct ocelot *ocelot, int port,
 	if (ret)
 		return ret;
 
-	flow_stats_update(&f->stats, 0x0, filter->stats.pkts, 0, 0x0,
+	stats.pkts = filter->stats.pkts;
+
+stats_update:
+	flow_stats_update(&f->stats, 0x0, stats.pkts, 0, 0x0,
 			  FLOW_ACTION_HW_STATS_IMMEDIATE);
 	return 0;
 }
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index babaa5b0c026..096c38c65157 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -564,6 +564,11 @@ struct ocelot_ops {
 	u16 (*wm_enc)(u16 value);
 	u16 (*wm_dec)(u16 value);
 	void (*wm_stat)(u32 val, u32 *inuse, u32 *maxuse);
+	void (*psfp_init)(struct ocelot *ocelot);
+	int (*psfp_filter_add)(struct ocelot *ocelot, struct flow_cls_offload *f);
+	int (*psfp_filter_del)(struct ocelot *ocelot, struct flow_cls_offload *f);
+	int (*psfp_stats_get)(struct ocelot *ocelot, struct flow_cls_offload *f,
+			      struct flow_stats *stats);
 };
 
 struct ocelot_vcap_block {
diff --git a/include/soc/mscc/ocelot_vcap.h b/include/soc/mscc/ocelot_vcap.h
index 25fd525aaf92..24b495ce140c 100644
--- a/include/soc/mscc/ocelot_vcap.h
+++ b/include/soc/mscc/ocelot_vcap.h
@@ -646,6 +646,7 @@ enum ocelot_vcap_filter_type {
 	OCELOT_VCAP_FILTER_DUMMY,
 	OCELOT_VCAP_FILTER_PAG,
 	OCELOT_VCAP_FILTER_OFFLOAD,
+	OCELOT_PSFP_FILTER_OFFLOAD,
 };
 
 struct ocelot_vcap_id {
-- 
2.17.1

[PATCH v4 net-next 5/8] net: dsa: felix: support psfp filter on vsc9959

[Xiaoliang Yang]

VSC9959 supports Per-Stream Filtering and Policing(PSFP) that complies
with the IEEE 802.1Qci standard. The stream is identified by Null stream
identification(DMAC and VLAN ID) defined in IEEE802.1CB.

For PSFP, four tables need to be set up: stream table, stream filter
table, stream gate table, and flow meter table. Identify the stream by
parsing the tc flower keys and add it to the stream table. The stream
filter table is automatically maintained, and its index is determined by
SGID(flow gate index) and FMID(flow meter index).

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/dsa/ocelot/felix_vsc9959.c | 454 ++++++++++++++++++++++++-
 include/soc/mscc/ocelot.h              |   8 +
 include/soc/mscc/ocelot_ana.h          |  10 +
 3 files changed, 462 insertions(+), 10 deletions(-)

diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index f966a253d1c7..52d10aab93b2 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -5,6 +5,7 @@
 #include <linux/fsl/enetc_mdio.h>
 #include <soc/mscc/ocelot_qsys.h>
 #include <soc/mscc/ocelot_vcap.h>
+#include <soc/mscc/ocelot_ana.h>
 #include <soc/mscc/ocelot_ptp.h>
 #include <soc/mscc/ocelot_sys.h>
 #include <soc/mscc/ocelot.h>
@@ -292,7 +293,7 @@ static const u32 vsc9959_sys_regmap[] = {
 	REG_RESERVED(SYS_MMGT_FAST),
 	REG_RESERVED(SYS_EVENTS_DIF),
 	REG_RESERVED(SYS_EVENTS_CORE),
-	REG_RESERVED(SYS_CNT),
+	REG(SYS_CNT,				0x000000),
 	REG(SYS_PTP_STATUS,			0x000f14),
 	REG(SYS_PTP_TXSTAMP,			0x000f18),
 	REG(SYS_PTP_NXT,			0x000f1c),
@@ -1022,15 +1023,6 @@ static void vsc9959_wm_stat(u32 val, u32 *inuse, u32 *maxuse)
 	*maxuse = val & GENMASK(11, 0);
 }
 
-static const struct ocelot_ops vsc9959_ops = {
-	.reset			= vsc9959_reset,
-	.wm_enc			= vsc9959_wm_enc,
-	.wm_dec			= vsc9959_wm_dec,
-	.wm_stat		= vsc9959_wm_stat,
-	.port_to_netdev		= felix_port_to_netdev,
-	.netdev_to_port		= felix_netdev_to_port,
-};
-
 static int vsc9959_mdio_bus_alloc(struct ocelot *ocelot)
 {
 	struct felix *felix = ocelot_to_felix(ocelot);
@@ -1346,6 +1338,448 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 	}
 }
 
+#define VSC9959_PSFP_SFID_MAX			175
+#define VSC9959_PSFP_GATE_ID_MAX		183
+#define VSC9959_PSFP_POLICER_MAX		383
+
+struct felix_stream {
+	struct list_head list;
+	unsigned long id;
+	u8 dmac[ETH_ALEN];
+	u16 vid;
+	s8 prio;
+	u8 sfid_valid;
+	u32 sfid;
+	u8 rsv_type;
+};
+
+struct felix_stream_filter {
+	struct list_head list;
+	refcount_t refcount;
+	u32 index;
+	u8 enable;
+	u8 sg_valid;
+	u32 sgid;
+	u8 fm_valid;
+	u32 fmid;
+	u8 prio_valid;
+	u8 prio;
+	u32 maxsdu;
+};
+
+struct felix_stream_filter_counters {
+	u32 match;
+	u32 not_pass_gate;
+	u32 not_pass_sdu;
+	u32 red;
+};
+
+static int vsc9959_stream_identify(struct flow_cls_offload *f,
+				   struct felix_stream *stream)
+{
+	struct flow_rule *rule = flow_cls_offload_flow_rule(f);
+	struct flow_dissector *dissector = rule->match.dissector;
+
+	if (dissector->used_keys &
+	    ~(BIT(FLOW_DISSECTOR_KEY_CONTROL) |
+	      BIT(FLOW_DISSECTOR_KEY_BASIC) |
+	      BIT(FLOW_DISSECTOR_KEY_VLAN) |
+	      BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS)))
+		return -EOPNOTSUPP;
+
+	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
+		struct flow_match_eth_addrs match;
+
+		flow_rule_match_eth_addrs(rule, &match);
+		ether_addr_copy(stream->dmac, match.key->dst);
+		if (!is_zero_ether_addr(match.mask->src))
+			return -EOPNOTSUPP;
+	} else {
+		return -EOPNOTSUPP;
+	}
+
+	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
+		struct flow_match_vlan match;
+
+		flow_rule_match_vlan(rule, &match);
+		if (match.mask->vlan_priority)
+			stream->prio = match.key->vlan_priority;
+		else
+			stream->prio = -1;
+
+		if (!match.mask->vlan_id)
+			return -EOPNOTSUPP;
+		stream->vid = match.key->vlan_id;
+	} else {
+		return -EOPNOTSUPP;
+	}
+
+	stream->id = f->cookie;
+
+	return 0;
+}
+
+static int vsc9959_mact_stream_set(struct ocelot *ocelot,
+				   struct felix_stream *stream,
+				   struct netlink_ext_ack *extack)
+{
+	struct ocelot_mact_entry entry;
+	u32 row, col, reg, dst_idx;
+	u8 type;
+	int ret;
+
+	/* Stream identification desn't support to add a stream with non
+	 * existent MAC (The MAC entry has not been learned in MAC table).
+	 */
+	ret = ocelot_mact_lookup(ocelot, stream->dmac, stream->vid, &row, &col);
+	if (ret) {
+		if (extack)
+			NL_SET_ERR_MSG_MOD(extack, "Stream is not learned in MAC table");
+		return -EOPNOTSUPP;
+	}
+
+	ocelot_rmw(ocelot,
+		   (stream->sfid_valid ? ANA_TABLES_STREAMDATA_SFID_VALID : 0) |
+		   ANA_TABLES_STREAMDATA_SFID(stream->sfid),
+		   ANA_TABLES_STREAMDATA_SFID_VALID |
+		   ANA_TABLES_STREAMDATA_SFID_M,
+		   ANA_TABLES_STREAMDATA);
+
+	reg = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
+	dst_idx = (reg & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
+	type = ANA_TABLES_MACACCESS_ENTRYTYPE_X(reg);
+
+	reg = ocelot_read(ocelot, ANA_TABLES_STREAMDATA);
+	if ((ANA_TABLES_STREAMDATA_SFID_VALID |
+	     ANA_TABLES_STREAMDATA_SSID_VALID) & reg) {
+		entry.type = (type ? type : ENTRYTYPE_LOCKED);
+		stream->rsv_type = type;
+	} else {
+		entry.type = stream->rsv_type;
+	}
+
+	ether_addr_copy(entry.mac, stream->dmac);
+	entry.vid = stream->vid;
+
+	ocelot_mact_write(ocelot, dst_idx, &entry, row, col);
+
+	return 0;
+}
+
+static struct felix_stream *
+vsc9959_stream_table_lookup(struct list_head *stream_list,
+			    struct felix_stream *stream)
+{
+	struct felix_stream *tmp;
+
+	list_for_each_entry(tmp, stream_list, list)
+		if (ether_addr_equal(tmp->dmac, stream->dmac) &&
+		    tmp->vid == stream->vid)
+			return tmp;
+
+	return NULL;
+}
+
+static int vsc9959_stream_table_add(struct ocelot *ocelot,
+				    struct list_head *stream_list,
+				    struct felix_stream *stream,
+				    struct netlink_ext_ack *extack)
+{
+	struct felix_stream *stream_entry;
+	int ret;
+
+	stream_entry = kzalloc(sizeof(*stream_entry), GFP_KERNEL);
+	if (!stream_entry)
+		return -ENOMEM;
+
+	memcpy(stream_entry, stream, sizeof(*stream_entry));
+
+	ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack);
+	if (ret) {
+		kfree(stream_entry);
+		return ret;
+	}
+
+	list_add_tail(&stream_entry->list, stream_list);
+
+	return 0;
+}
+
+static struct felix_stream *
+vsc9959_stream_table_get(struct list_head *stream_list, unsigned long id)
+{
+	struct felix_stream *tmp;
+
+	list_for_each_entry(tmp, stream_list, list)
+		if (tmp->id == id)
+			return tmp;
+
+	return NULL;
+}
+
+static void vsc9959_stream_table_del(struct ocelot *ocelot,
+				     struct felix_stream *stream)
+{
+	vsc9959_mact_stream_set(ocelot, stream, NULL);
+
+	list_del(&stream->list);
+	kfree(stream);
+}
+
+static u32 vsc9959_sfi_access_status(struct ocelot *ocelot)
+{
+	return ocelot_read(ocelot, ANA_TABLES_SFIDACCESS);
+}
+
+static int vsc9959_psfp_sfi_set(struct ocelot *ocelot,
+				struct felix_stream_filter *sfi)
+{
+	u32 val;
+
+	if (sfi->index > VSC9959_PSFP_SFID_MAX)
+		return -EINVAL;
+
+	if (!sfi->enable) {
+		ocelot_write(ocelot, ANA_TABLES_SFIDTIDX_SFID_INDEX(sfi->index),
+			     ANA_TABLES_SFIDTIDX);
+
+		val = ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE);
+		ocelot_write(ocelot, val, ANA_TABLES_SFIDACCESS);
+
+		return readx_poll_timeout(vsc9959_sfi_access_status, ocelot, val,
+					  (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
+					  10, 100000);
+	}
+
+	if (sfi->sgid > VSC9959_PSFP_GATE_ID_MAX ||
+	    sfi->fmid > VSC9959_PSFP_POLICER_MAX)
+		return -EINVAL;
+
+	ocelot_write(ocelot,
+		     (sfi->sg_valid ? ANA_TABLES_SFIDTIDX_SGID_VALID : 0) |
+		     ANA_TABLES_SFIDTIDX_SGID(sfi->sgid) |
+		     (sfi->fm_valid ? ANA_TABLES_SFIDTIDX_POL_ENA : 0) |
+		     ANA_TABLES_SFIDTIDX_POL_IDX(sfi->fmid) |
+		     ANA_TABLES_SFIDTIDX_SFID_INDEX(sfi->index),
+		     ANA_TABLES_SFIDTIDX);
+
+	ocelot_write(ocelot,
+		     (sfi->prio_valid ? ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA : 0) |
+		     ANA_TABLES_SFIDACCESS_IGR_PRIO(sfi->prio) |
+		     ANA_TABLES_SFIDACCESS_MAX_SDU_LEN(sfi->maxsdu) |
+		     ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE),
+		     ANA_TABLES_SFIDACCESS);
+
+	return readx_poll_timeout(vsc9959_sfi_access_status, ocelot, val,
+				  (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
+				  10, 100000);
+}
+
+static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot,
+				      struct felix_stream_filter *sfi)
+{
+	struct felix_stream_filter *sfi_entry, *tmp;
+	struct list_head *pos, *q, *last;
+	struct ocelot_psfp_list *psfp;
+	u32 insert = 0;
+	int ret;
+
+	psfp = &ocelot->psfp;
+	last = &psfp->sfi_list;
+
+	list_for_each_safe(pos, q, &psfp->sfi_list) {
+		tmp = list_entry(pos, struct felix_stream_filter, list);
+		if (sfi->sg_valid == tmp->sg_valid &&
+		    sfi->fm_valid == tmp->fm_valid &&
+		    tmp->sgid == sfi->sgid &&
+		    tmp->fmid == sfi->fmid) {
+			sfi->index = tmp->index;
+			refcount_inc(&tmp->refcount);
+			return 0;
+		}
+		/* Make sure that the index is increasing in order. */
+		if (tmp->index == insert) {
+			last = pos;
+			insert++;
+		}
+	}
+	sfi->index = insert;
+
+	sfi_entry = kzalloc(sizeof(*sfi_entry), GFP_KERNEL);
+	if (!sfi_entry)
+		return -ENOMEM;
+
+	memcpy(sfi_entry, sfi, sizeof(*sfi_entry));
+	refcount_set(&sfi_entry->refcount, 1);
+
+	ret = vsc9959_psfp_sfi_set(ocelot, sfi_entry);
+	if (ret) {
+		kfree(sfi_entry);
+		return ret;
+	}
+
+	list_add(&sfi_entry->list, last);
+
+	return 0;
+}
+
+static void vsc9959_psfp_sfi_table_del(struct ocelot *ocelot, u32 index)
+{
+	struct felix_stream_filter *tmp, *n;
+	struct ocelot_psfp_list *psfp;
+	u8 z;
+
+	psfp = &ocelot->psfp;
+
+	list_for_each_entry_safe(tmp, n, &psfp->sfi_list, list)
+		if (tmp->index == index) {
+			z = refcount_dec_and_test(&tmp->refcount);
+			if (z) {
+				tmp->enable = 0;
+				vsc9959_psfp_sfi_set(ocelot, tmp);
+				list_del(&tmp->list);
+				kfree(tmp);
+			}
+			break;
+		}
+}
+
+static void vsc9959_psfp_counters_get(struct ocelot *ocelot, u32 index,
+				      struct felix_stream_filter_counters *counters)
+{
+	ocelot_rmw(ocelot, SYS_STAT_CFG_STAT_VIEW(index),
+		   SYS_STAT_CFG_STAT_VIEW_M,
+		   SYS_STAT_CFG);
+
+	counters->match = ocelot_read_gix(ocelot, SYS_CNT, 0x200);
+	counters->not_pass_gate = ocelot_read_gix(ocelot, SYS_CNT, 0x201);
+	counters->not_pass_sdu = ocelot_read_gix(ocelot, SYS_CNT, 0x202);
+	counters->red = ocelot_read_gix(ocelot, SYS_CNT, 0x203);
+
+	/* Clear the PSFP counter. */
+	ocelot_write(ocelot,
+		     SYS_STAT_CFG_STAT_VIEW(index) |
+		     SYS_STAT_CFG_STAT_CLEAR_SHOT(0x10),
+		     SYS_STAT_CFG);
+}
+
+static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
+				   struct flow_cls_offload *f)
+{
+	struct netlink_ext_ack *extack = f->common.extack;
+	struct felix_stream_filter sfi = {0};
+	const struct flow_action_entry *a;
+	struct felix_stream *stream_entry;
+	struct felix_stream stream = {0};
+	struct ocelot_psfp_list *psfp;
+	int ret, i;
+
+	psfp = &ocelot->psfp;
+
+	ret = vsc9959_stream_identify(f, &stream);
+	if (ret) {
+		NL_SET_ERR_MSG_MOD(extack, "Only can match on VID, PCP, and dest MAC");
+		return ret;
+	}
+
+	flow_action_for_each(i, a, &f->rule->action) {
+		switch (a->id) {
+		case FLOW_ACTION_GATE:
+		case FLOW_ACTION_POLICE:
+		default:
+			return -EOPNOTSUPP;
+		}
+	}
+
+	/* Check if stream is set. */
+	stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream);
+	if (stream_entry) {
+		NL_SET_ERR_MSG_MOD(extack, "This stream is already added");
+		return -EEXIST;
+	}
+
+	sfi.prio_valid = (stream.prio < 0 ? 0 : 1);
+	sfi.prio = (sfi.prio_valid ? stream.prio : 0);
+	sfi.enable = 1;
+
+	ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi);
+	if (ret)
+		return ret;
+
+	stream.sfid = sfi.index;
+	stream.sfid_valid = 1;
+	ret = vsc9959_stream_table_add(ocelot, &psfp->stream_list,
+				       &stream, extack);
+	if (ret)
+		vsc9959_psfp_sfi_table_del(ocelot, stream.sfid);
+
+	return ret;
+}
+
+static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
+				   struct flow_cls_offload *f)
+{
+	struct ocelot_psfp_list *psfp;
+	struct felix_stream *stream;
+
+	psfp = &ocelot->psfp;
+
+	stream = vsc9959_stream_table_get(&psfp->stream_list, f->cookie);
+	if (!stream)
+		return -ENOMEM;
+
+	vsc9959_psfp_sfi_table_del(ocelot, stream->sfid);
+
+	stream->sfid_valid = 0;
+	vsc9959_stream_table_del(ocelot, stream);
+
+	return 0;
+}
+
+static int vsc9959_psfp_stats_get(struct ocelot *ocelot,
+				  struct flow_cls_offload *f,
+				  struct flow_stats *stats)
+{
+	struct felix_stream_filter_counters counters;
+	struct ocelot_psfp_list *psfp;
+	struct felix_stream *stream;
+
+	psfp = &ocelot->psfp;
+	stream = vsc9959_stream_table_get(&psfp->stream_list, f->cookie);
+	if (!stream)
+		return -ENOMEM;
+
+	vsc9959_psfp_counters_get(ocelot, stream->sfid, &counters);
+
+	stats->pkts = counters.match;
+	stats->drops = counters.not_pass_gate + counters.not_pass_sdu +
+		       counters.red;
+
+	return 0;
+}
+
+static void vsc9959_psfp_init(struct ocelot *ocelot)
+{
+	struct ocelot_psfp_list *psfp = &ocelot->psfp;
+
+	INIT_LIST_HEAD(&psfp->stream_list);
+	INIT_LIST_HEAD(&psfp->sfi_list);
+	INIT_LIST_HEAD(&psfp->sgi_list);
+}
+
+static const struct ocelot_ops vsc9959_ops = {
+	.reset			= vsc9959_reset,
+	.wm_enc			= vsc9959_wm_enc,
+	.wm_dec			= vsc9959_wm_dec,
+	.wm_stat		= vsc9959_wm_stat,
+	.port_to_netdev		= felix_port_to_netdev,
+	.netdev_to_port		= felix_netdev_to_port,
+	.psfp_init		= vsc9959_psfp_init,
+	.psfp_filter_add	= vsc9959_psfp_filter_add,
+	.psfp_filter_del	= vsc9959_psfp_filter_del,
+	.psfp_stats_get		= vsc9959_psfp_stats_get,
+};
+
 static const struct felix_info felix_info_vsc9959 = {
 	.target_io_res		= vsc9959_target_io_res,
 	.port_io_res		= vsc9959_port_io_res,
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index 096c38c65157..a611f9cd5935 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -582,6 +582,12 @@ struct ocelot_vlan {
 	u16 vid;
 };
 
+struct ocelot_psfp_list {
+	struct list_head stream_list;
+	struct list_head sfi_list;
+	struct list_head sgi_list;
+};
+
 enum ocelot_sb {
 	OCELOT_SB_BUF,
 	OCELOT_SB_REF,
@@ -673,6 +679,8 @@ struct ocelot {
 	struct ocelot_vcap_block	block[3];
 	struct vcap_props		*vcap;
 
+	struct ocelot_psfp_list		psfp;
+
 	/* Workqueue to check statistics for overflow with its lock */
 	struct mutex			stats_lock;
 	u64				*stats;
diff --git a/include/soc/mscc/ocelot_ana.h b/include/soc/mscc/ocelot_ana.h
index 1669481d9779..67e0ae05a5ab 100644
--- a/include/soc/mscc/ocelot_ana.h
+++ b/include/soc/mscc/ocelot_ana.h
@@ -227,6 +227,11 @@
 #define ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(x)             ((x) & GENMASK(1, 0))
 #define ANA_TABLES_SFIDACCESS_SFID_TBL_CMD_M              GENMASK(1, 0)
 
+#define SFIDACCESS_CMD_IDLE                               0
+#define SFIDACCESS_CMD_READ                               1
+#define SFIDACCESS_CMD_WRITE                              2
+#define SFIDACCESS_CMD_INIT                               3
+
 #define ANA_TABLES_SFIDTIDX_SGID_VALID                    BIT(26)
 #define ANA_TABLES_SFIDTIDX_SGID(x)                       (((x) << 18) & GENMASK(25, 18))
 #define ANA_TABLES_SFIDTIDX_SGID_M                        GENMASK(25, 18)
@@ -255,6 +260,11 @@
 #define ANA_SG_CONFIG_REG_3_INIT_IPS(x)                   (((x) << 21) & GENMASK(24, 21))
 #define ANA_SG_CONFIG_REG_3_INIT_IPS_M                    GENMASK(24, 21)
 #define ANA_SG_CONFIG_REG_3_INIT_IPS_X(x)                 (((x) & GENMASK(24, 21)) >> 21)
+#define ANA_SG_CONFIG_REG_3_IPV_VALID                     BIT(24)
+#define ANA_SG_CONFIG_REG_3_IPV_INVALID(x)                (((x) << 24) & GENMASK(24, 24))
+#define ANA_SG_CONFIG_REG_3_INIT_IPV(x)                   (((x) << 21) & GENMASK(23, 21))
+#define ANA_SG_CONFIG_REG_3_INIT_IPV_M                    GENMASK(23, 21)
+#define ANA_SG_CONFIG_REG_3_INIT_IPV_X(x)                 (((x) & GENMASK(23, 21)) >> 21)
 #define ANA_SG_CONFIG_REG_3_INIT_GATE_STATE               BIT(25)
 
 #define ANA_SG_GCL_GS_CONFIG_RSZ                          0x4
-- 
2.17.1

Re: [PATCH v4 net-next 5/8] net: dsa: felix: support psfp filter on vsc9959

[Vladimir Oltean]

Hello Xiaoliang,

On Wed, Sep 22, 2021 at 06:51:59PM +0800, Xiaoliang Yang wrote:
> +static int vsc9959_mact_stream_set(struct ocelot *ocelot,
> +				   struct felix_stream *stream,
> +				   struct netlink_ext_ack *extack)
> +{
> +	struct ocelot_mact_entry entry;
> +	u32 row, col, reg, dst_idx;
> +	u8 type;
> +	int ret;
> +
> +	/* Stream identification desn't support to add a stream with non
> +	 * existent MAC (The MAC entry has not been learned in MAC table).
> +	 */
> +	ret = ocelot_mact_lookup(ocelot, stream->dmac, stream->vid, &row, &col);
> +	if (ret) {
> +		if (extack)
> +			NL_SET_ERR_MSG_MOD(extack, "Stream is not learned in MAC table");
> +		return -EOPNOTSUPP;
> +	}
> +
> +	ocelot_rmw(ocelot,
> +		   (stream->sfid_valid ? ANA_TABLES_STREAMDATA_SFID_VALID : 0) |
> +		   ANA_TABLES_STREAMDATA_SFID(stream->sfid),
> +		   ANA_TABLES_STREAMDATA_SFID_VALID |
> +		   ANA_TABLES_STREAMDATA_SFID_M,
> +		   ANA_TABLES_STREAMDATA);
> +
> +	reg = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
> +	dst_idx = (reg & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
> +	type = ANA_TABLES_MACACCESS_ENTRYTYPE_X(reg);
> +
> +	reg = ocelot_read(ocelot, ANA_TABLES_STREAMDATA);
> +	if ((ANA_TABLES_STREAMDATA_SFID_VALID |
> +	     ANA_TABLES_STREAMDATA_SSID_VALID) & reg) {
> +		entry.type = (type ? type : ENTRYTYPE_LOCKED);
> +		stream->rsv_type = type;
> +	} else {
> +		entry.type = stream->rsv_type;
> +	}
> +
> +	ether_addr_copy(entry.mac, stream->dmac);
> +	entry.vid = stream->vid;
> +
> +	ocelot_mact_write(ocelot, dst_idx, &entry, row, col);
> +
> +	return 0;
> +}

> +static int vsc9959_stream_table_add(struct ocelot *ocelot,
> +				    struct list_head *stream_list,
> +				    struct felix_stream *stream,
> +				    struct netlink_ext_ack *extack)
> +{
> +	struct felix_stream *stream_entry;
> +	int ret;
> +
> +	stream_entry = kzalloc(sizeof(*stream_entry), GFP_KERNEL);
> +	if (!stream_entry)
> +		return -ENOMEM;
> +
> +	memcpy(stream_entry, stream, sizeof(*stream_entry));
> +
> +	ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack);
> +	if (ret) {
> +		kfree(stream_entry);
> +		return ret;
> +	}
> +
> +	list_add_tail(&stream_entry->list, stream_list);
> +
> +	return 0;
> +}

Remember this discussion we had a while ago?

| Let's take the function below.
| 
| static void ocelot_prove_mac_table_entries_can_move(struct ocelot *ocelot)
| {
| 	unsigned char mac1[ETH_ALEN] = {0x00, 0x04, 0x9f, 0x63, 0x35, 0xea};
| 	unsigned char mac2[ETH_ALEN] = {0x00, 0x04, 0x9f, 0x63, 0x35, 0xeb};
| 	int row, bucket, arbitrary_pgid = 4;
| 	int vid1 = 102;
| 	int vid2 = 103;
| 	int err;
| 
| 	err = ocelot_mact_learn(ocelot, arbitrary_pgid, mac1, vid1,
| 				ENTRYTYPE_LOCKED);
| 	if (err)
| 		return;
| 
| 	err = ocelot_mact_lookup(ocelot, mac1, vid1, &row, &bucket);
| 	if (err)
| 		return;
| 
| 	dev_info(ocelot->dev,
| 		 "Address 1 (mac %pM vid %d) is in MAC table row %d bucket %d\n",
| 		 mac1, vid1, row, bucket);
| 
| 	err = ocelot_mact_learn(ocelot, arbitrary_pgid, mac2, vid2,
| 				ENTRYTYPE_LOCKED);
| 	if (err)
| 		return;
| 
| 	err = ocelot_mact_lookup(ocelot, mac2, vid2, &row, &bucket);
| 	if (err)
| 		return;
| 
| 	dev_info(ocelot->dev,
| 		 "Address 2 (mac %pM vid %d) is in MAC table row %d bucket %d\n",
| 		 mac2, vid2, row, bucket);
| 
| 	err = ocelot_mact_lookup(ocelot, mac1, vid1, &row, &bucket);
| 	if (err)
| 		return;
| 
| 	dev_info(ocelot->dev,
| 		 "Address 1 (mac %pM vid %d) is in MAC table row %d bucket %d\n",
| 		 mac1, vid1, row, bucket);
| }
| 
| What will it print?
| 
| Address 1 (mac 00:04:9f:63:35:ea vid 102) is in MAC table row 917 bucket 0
| Address 2 (mac 00:04:9f:63:35:eb vid 103) is in MAC table row 917 bucket 0
| Address 1 (mac 00:04:9f:63:35:ea vid 102) is in MAC table row 917 bucket 1
| 
| What does this mean?
| 
| The ROW portion of a FDB entry's position within the MAC table is
| statically determined using an 11-bit hash derived from the {DMAC, VID}
| key. Within a row, there can be up to 4 buckets, each bucket holding 1
| MAC table entry.
| 
| But when the hashes of 2 addresses collide and they end up in the same
| row (as in the above example, with address 1 = "mac 00:04:9f:63:35:ea
| vid 102" and address 2 = "mac 00:04:9f:63:35:eb vid 103"), things don't
| happen quite as you might expect. Namely, the second address appears to
| be installed by the switch at the same row and bucket as the first
| address. So is the first address overwritten? No, it has been moved by
| the switch, automatically, to bucket 1.

So if the autonomous and concurrent learning of one MAC address might
move existing MAC table entries from a row to the right, then who
guarantees exactly that the {row, col} for which you are setting up the
SFID is the {row, col} that belongs to the {stream->dmac, stream->vid}
you have searched for?

Microchip people, do we need to temporarily disable hardware address
learning on all ports, and take a lock with the FDB add and delete
operations to ensure they are serialized?

RE: [PATCH v4 net-next 5/8] net: dsa: felix: support psfp filter on vsc9959

[Xiaoliang Yang]

Hi Vladimir,

On Wed, Sep 22, 2021 at 12:47:59 +0000, Vladimir Oltean wrote:
> > +static int vsc9959_mact_stream_set(struct ocelot *ocelot,
> > +				   struct felix_stream *stream,
> > +				   struct netlink_ext_ack *extack) {
> > +	struct ocelot_mact_entry entry;
> > +	u32 row, col, reg, dst_idx;
> > +	u8 type;
> > +	int ret;
> > +
> > +	/* Stream identification desn't support to add a stream with non
> > +	 * existent MAC (The MAC entry has not been learned in MAC table).
> > +	 */
> > +	ret = ocelot_mact_lookup(ocelot, stream->dmac, stream->vid, &row,
> &col);
> > +	if (ret) {
> > +		if (extack)
> > +			NL_SET_ERR_MSG_MOD(extack, "Stream is not learned in MAC
> table");
> > +		return -EOPNOTSUPP;
> > +	}
> > +
> > +	ocelot_rmw(ocelot,
> > +		   (stream->sfid_valid ? ANA_TABLES_STREAMDATA_SFID_VALID : 0)
> |
> > +		   ANA_TABLES_STREAMDATA_SFID(stream->sfid),
> > +		   ANA_TABLES_STREAMDATA_SFID_VALID |
> > +		   ANA_TABLES_STREAMDATA_SFID_M,
> > +		   ANA_TABLES_STREAMDATA);
> > +
> > +	reg = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
> > +	dst_idx = (reg & ANA_TABLES_MACACCESS_DEST_IDX_M) >> 3;
> > +	type = ANA_TABLES_MACACCESS_ENTRYTYPE_X(reg);
> > +
> > +	reg = ocelot_read(ocelot, ANA_TABLES_STREAMDATA);
> > +	if ((ANA_TABLES_STREAMDATA_SFID_VALID |
> > +	     ANA_TABLES_STREAMDATA_SSID_VALID) & reg) {
> > +		entry.type = (type ? type : ENTRYTYPE_LOCKED);
> > +		stream->rsv_type = type;
> > +	} else {
> > +		entry.type = stream->rsv_type;
> > +	}
> > +
> > +	ether_addr_copy(entry.mac, stream->dmac);
> > +	entry.vid = stream->vid;
> > +
> > +	ocelot_mact_write(ocelot, dst_idx, &entry, row, col);
> > +
> > +	return 0;
> > +}
> 
> > +static int vsc9959_stream_table_add(struct ocelot *ocelot,
> > +				    struct list_head *stream_list,
> > +				    struct felix_stream *stream,
> > +				    struct netlink_ext_ack *extack) {
> > +	struct felix_stream *stream_entry;
> > +	int ret;
> > +
> > +	stream_entry = kzalloc(sizeof(*stream_entry), GFP_KERNEL);
> > +	if (!stream_entry)
> > +		return -ENOMEM;
> > +
> > +	memcpy(stream_entry, stream, sizeof(*stream_entry));
> > +
> > +	ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack);
> > +	if (ret) {
> > +		kfree(stream_entry);
> > +		return ret;
> > +	}
> > +
> > +	list_add_tail(&stream_entry->list, stream_list);
> > +
> > +	return 0;
> > +}
> 
> Remember this discussion we had a while ago?
> 
> | Let's take the function below.
> |
> | static void ocelot_prove_mac_table_entries_can_move(struct ocelot
> | *ocelot) {
> | 	unsigned char mac1[ETH_ALEN] = {0x00, 0x04, 0x9f, 0x63, 0x35, 0xea};
> | 	unsigned char mac2[ETH_ALEN] = {0x00, 0x04, 0x9f, 0x63, 0x35, 0xeb};
> | 	int row, bucket, arbitrary_pgid = 4;
> | 	int vid1 = 102;
> | 	int vid2 = 103;
> | 	int err;
> |
> | 	err = ocelot_mact_learn(ocelot, arbitrary_pgid, mac1, vid1,
> | 				ENTRYTYPE_LOCKED);
> | 	if (err)
> | 		return;
> |
> | 	err = ocelot_mact_lookup(ocelot, mac1, vid1, &row, &bucket);
> | 	if (err)
> | 		return;
> |
> | 	dev_info(ocelot->dev,
> | 		 "Address 1 (mac %pM vid %d) is in MAC table row %d
> bucket %d\n",
> | 		 mac1, vid1, row, bucket);
> |
> | 	err = ocelot_mact_learn(ocelot, arbitrary_pgid, mac2, vid2,
> | 				ENTRYTYPE_LOCKED);
> | 	if (err)
> | 		return;
> |
> | 	err = ocelot_mact_lookup(ocelot, mac2, vid2, &row, &bucket);
> | 	if (err)
> | 		return;
> |
> | 	dev_info(ocelot->dev,
> | 		 "Address 2 (mac %pM vid %d) is in MAC table row %d
> bucket %d\n",
> | 		 mac2, vid2, row, bucket);
> |
> | 	err = ocelot_mact_lookup(ocelot, mac1, vid1, &row, &bucket);
> | 	if (err)
> | 		return;
> |
> | 	dev_info(ocelot->dev,
> | 		 "Address 1 (mac %pM vid %d) is in MAC table row %d
> bucket %d\n",
> | 		 mac1, vid1, row, bucket);
> | }
> |
> | What will it print?
> |
> | Address 1 (mac 00:04:9f:63:35:ea vid 102) is in MAC table row 917
> | bucket 0 Address 2 (mac 00:04:9f:63:35:eb vid 103) is in MAC table row
> | 917 bucket 0 Address 1 (mac 00:04:9f:63:35:ea vid 102) is in MAC table
> | row 917 bucket 1
> |
> | What does this mean?
> |
> | The ROW portion of a FDB entry's position within the MAC table is
> | statically determined using an 11-bit hash derived from the {DMAC,
> | VID} key. Within a row, there can be up to 4 buckets, each bucket
> | holding 1 MAC table entry.
> |
> | But when the hashes of 2 addresses collide and they end up in the same
> | row (as in the above example, with address 1 = "mac 00:04:9f:63:35:ea
> | vid 102" and address 2 = "mac 00:04:9f:63:35:eb vid 103"), things
> | don't happen quite as you might expect. Namely, the second address
> | appears to be installed by the switch at the same row and bucket as
> | the first address. So is the first address overwritten? No, it has
> | been moved by the switch, automatically, to bucket 1.
> 
> So if the autonomous and concurrent learning of one MAC address might
> move existing MAC table entries from a row to the right, then who guarantees
> exactly that the {row, col} for which you are setting up the SFID is the {row, col}
> that belongs to the {stream->dmac, stream->vid} you have searched for?
> 
> Microchip people, do we need to temporarily disable hardware address
> learning on all ports, and take a lock with the FDB add and delete operations
> to ensure they are serialized?

Maybe we need to use ocelot_mact_learn() instead of ocelot_mact_write() after setting SFID in StreamData. I think this can avoid writing a wrong entry.

Regards,
Xiaoliang

Re: [PATCH v4 net-next 5/8] net: dsa: felix: support psfp filter on vsc9959

[Vladimir Oltean]

On Thu, Sep 23, 2021 at 02:30:16AM +0000, Xiaoliang Yang wrote:
> Maybe we need to use ocelot_mact_learn() instead of
> ocelot_mact_write() after setting SFID in StreamData. I think this can
> avoid writing a wrong entry.

So you're thinking of introducing a new ocelot_mact_learn_with_streamdata(),
that writes the SFID and SSID of the STREAMDATA too, instead of editing
them in-place for an existing MAC table entry, and then issuing a LEARN
MAC Table command which would hopefully transfer the entire data
structure to the MAC table?

Have you tried that?

In the documentation for the LEARN MAC Table command, I see:

Purpose: Insert/learn new entry in MAC table.  Position given by (MAC, VID)

Use: Configure MAC and VID of the new entry in MACHDATA and MACLDATA.
Configure remaining entry fields in MACACCESS.  The location in the MAC
table is calculated based on (MAC, VID).

I just hope it will transfer the STREAMDATA too, it doesn't explicitly
say that it will...

And assuming it does, will the LEARN command overwrite an existing
static FDB entry which has the same MAC DA and VLAN ID, but not SFID?
I haven't tried that either.

RE: [PATCH v4 net-next 5/8] net: dsa: felix: support psfp filter on vsc9959

[Xiaoliang Yang]

Hi Vladimir,

On Thu, Sep 23, 2021 at 15:45:16 +0000, Vladimir Oltean wrote:
> > Maybe we need to use ocelot_mact_learn() instead of
> > ocelot_mact_write() after setting SFID in StreamData. I think this can
> > avoid writing a wrong entry.
> 
> So you're thinking of introducing a new ocelot_mact_learn_with_streamdata(),
> that writes the SFID and SSID of the STREAMDATA too, instead of editing them
> in-place for an existing MAC table entry, and then issuing a LEARN MAC Table
> command which would hopefully transfer the entire data structure to the MAC
> table?
> 
> Have you tried that?

Yes, I have tried. I mean writes SFID of STREAMDATA in vsc9959_mact_stream_set() first, then calls ocelot_mact_learn() function to write VID, mac and STREAMDATA in MAC table. We don't need to introduce a new function. Once we call ocelot_mact_learn() function, STREAMDATA will be stored in the learned entry.

> 
> In the documentation for the LEARN MAC Table command, I see:
> 
> Purpose: Insert/learn new entry in MAC table.  Position given by (MAC, VID)
> 
> Use: Configure MAC and VID of the new entry in MACHDATA and MACLDATA.
> Configure remaining entry fields in MACACCESS.  The location in the MAC
> table is calculated based on (MAC, VID).
> 
> I just hope it will transfer the STREAMDATA too, it doesn't explicitly say that it
> will...
> 
> And assuming it does, will the LEARN command overwrite an existing static
> FDB entry which has the same MAC DA and VLAN ID, but not SFID?
> I haven't tried that either.

I tried the case that when MAC table index has changed, STREAMDATA will keep move with VID and MAC. The entry { STREAMDATA , VID, MAC} also can overwrite a static exist entry. I think we can do like this.

Re: [PATCH v4 net-next 5/8] net: dsa: felix: support psfp filter on vsc9959

[Vladimir Oltean]

On Thu, Sep 23, 2021 at 11:23:45AM +0000, Xiaoliang Yang wrote:
> Hi Vladimir,
>
> On Thu, Sep 23, 2021 at 15:45:16 +0000, Vladimir Oltean wrote:
> > > Maybe we need to use ocelot_mact_learn() instead of
> > > ocelot_mact_write() after setting SFID in StreamData. I think this can
> > > avoid writing a wrong entry.
> >
> > So you're thinking of introducing a new ocelot_mact_learn_with_streamdata(),
> > that writes the SFID and SSID of the STREAMDATA too, instead of editing them
> > in-place for an existing MAC table entry, and then issuing a LEARN MAC Table
> > command which would hopefully transfer the entire data structure to the MAC
> > table?
> >
> > Have you tried that?
>
> Yes, I have tried. I mean writes SFID of STREAMDATA in
> vsc9959_mact_stream_set() first, then calls ocelot_mact_learn()
> function to write VID, mac and STREAMDATA in MAC table. We don't need
> to introduce a new function. Once we call ocelot_mact_learn()
> function, STREAMDATA will be stored in the learned entry.
>
> >
> > In the documentation for the LEARN MAC Table command, I see:
> >
> > Purpose: Insert/learn new entry in MAC table.  Position given by (MAC, VID)
> >
> > Use: Configure MAC and VID of the new entry in MACHDATA and MACLDATA.
> > Configure remaining entry fields in MACACCESS.  The location in the MAC
> > table is calculated based on (MAC, VID).
> >
> > I just hope it will transfer the STREAMDATA too, it doesn't explicitly say that it
> > will...
> >
> > And assuming it does, will the LEARN command overwrite an existing static
> > FDB entry which has the same MAC DA and VLAN ID, but not SFID?
> > I haven't tried that either.
>
> I tried the case that when MAC table index has changed, STREAMDATA
> will keep move with VID and MAC. The entry { STREAMDATA , VID, MAC}
> also can overwrite a static exist entry. I think we can do like this.

Ok, so maybe we should do that?

Even though I must say I don't really like the idea of partially writing
MAC table entry data from the vsc9959 driver, and partially from
ocelot_mact_learn. I also have this patch pending:
https://patchwork.kernel.org/project/netdevbpf/patch/20210824114049.3814660-4-vladimir.oltean@nxp.com/
and concurrency will be an absolute mess. The ocelot->mact_lock will
need to be taken _before_ we start writing the STREAMDATA, so this
variant of ocelot_mact_learn will still have to stay somewhere in the
ocelot library, and be organized something like this:

__ocelot_mact_learn()
{
	do what ocelot_mact_learn() currently does
}

ocelot_mact_learn()
{
	mutex_lock(&ocelot->mact_lock);
	__ocelot_mact_learn();
	mutex_unlock(&ocelot->mact_lock);
}

ocelot_mact_learn_streamdata()
{
	mutex_lock(&ocelot->mact_lock);
	write_streamdata();
	__ocelot_mact_learn();
	mutex_unlock(&ocelot->mact_lock);
}

otherwise I would need to introduce avoidable refactoring in the driver.
In fact, could you please pick up that mact_lock patch? Even if the
rtnl_mutex was not dropped yet, the extra lock should not hurt anyone.

[PATCH v4 net-next 6/8] net: dsa: felix: add stream gate settings for psfp

[Xiaoliang Yang]

This patch adds stream gate settings for PSFP. Use SGI table to store
stream gate entries. Disable the gate entry when it is not used by any
stream.

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

diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index 52d10aab93b2..9264934631d7 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -8,6 +8,7 @@
 #include <soc/mscc/ocelot_ana.h>
 #include <soc/mscc/ocelot_ptp.h>
 #include <soc/mscc/ocelot_sys.h>
+#include <net/tc_act/tc_gate.h>
 #include <soc/mscc/ocelot.h>
 #include <linux/dsa/ocelot.h>
 #include <linux/pcs-lynx.h>
@@ -1341,6 +1342,8 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 #define VSC9959_PSFP_SFID_MAX			175
 #define VSC9959_PSFP_GATE_ID_MAX		183
 #define VSC9959_PSFP_POLICER_MAX		383
+#define VSC9959_PSFP_GATE_LIST_NUM		4
+#define VSC9959_PSFP_GATE_CYCLETIME_MIN		5000
 
 struct felix_stream {
 	struct list_head list;
@@ -1374,6 +1377,24 @@ struct felix_stream_filter_counters {
 	u32 red;
 };
 
+struct felix_stream_gate {
+	u32 index;
+	u8 enable;
+	u8 ipv_valid;
+	u8 init_ipv;
+	u64 basetime;
+	u64 cycletime;
+	u64 cycletime_ext;
+	u32 num_entries;
+	struct action_gate_entry entries[0];
+};
+
+struct felix_stream_gate_entry {
+	struct list_head list;
+	refcount_t refcount;
+	u32 index;
+};
+
 static int vsc9959_stream_identify(struct flow_cls_offload *f,
 				   struct felix_stream *stream)
 {
@@ -1623,6 +1644,18 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot,
 	return 0;
 }
 
+static struct felix_stream_filter *
+vsc9959_psfp_sfi_table_get(struct list_head *sfi_list, u32 index)
+{
+	struct felix_stream_filter *tmp;
+
+	list_for_each_entry(tmp, sfi_list, list)
+		if (tmp->index == index)
+			return tmp;
+
+	return NULL;
+}
+
 static void vsc9959_psfp_sfi_table_del(struct ocelot *ocelot, u32 index)
 {
 	struct felix_stream_filter *tmp, *n;
@@ -1644,6 +1677,152 @@ static void vsc9959_psfp_sfi_table_del(struct ocelot *ocelot, u32 index)
 		}
 }
 
+static void vsc9959_psfp_parse_gate(const struct flow_action_entry *entry,
+				    struct felix_stream_gate *sgi)
+{
+	sgi->index = entry->gate.index;
+	sgi->ipv_valid = (entry->gate.prio < 0) ? 0 : 1;
+	sgi->init_ipv = (sgi->ipv_valid) ? entry->gate.prio : 0;
+	sgi->basetime = entry->gate.basetime;
+	sgi->cycletime = entry->gate.cycletime;
+	sgi->num_entries = entry->gate.num_entries;
+	sgi->enable = 1;
+
+	memcpy(sgi->entries, entry->gate.entries,
+	       entry->gate.num_entries * sizeof(struct action_gate_entry));
+}
+
+static u32 vsc9959_sgi_cfg_status(struct ocelot *ocelot)
+{
+	return ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
+}
+
+static int vsc9959_psfp_sgi_set(struct ocelot *ocelot,
+				struct felix_stream_gate *sgi)
+{
+	struct action_gate_entry *e;
+	struct timespec64 base_ts;
+	u32 interval_sum = 0;
+	u32 val;
+	int i;
+
+	if (sgi->index > VSC9959_PSFP_GATE_ID_MAX)
+		return -EINVAL;
+
+	ocelot_write(ocelot, ANA_SG_ACCESS_CTRL_SGID(sgi->index),
+		     ANA_SG_ACCESS_CTRL);
+
+	if (!sgi->enable) {
+		ocelot_rmw(ocelot, ANA_SG_CONFIG_REG_3_INIT_GATE_STATE,
+			   ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
+			   ANA_SG_CONFIG_REG_3_GATE_ENABLE,
+			   ANA_SG_CONFIG_REG_3);
+
+		return 0;
+	}
+
+	if (sgi->cycletime < VSC9959_PSFP_GATE_CYCLETIME_MIN ||
+	    sgi->cycletime > NSEC_PER_SEC)
+		return -EINVAL;
+
+	if (sgi->num_entries > VSC9959_PSFP_GATE_LIST_NUM)
+		return -EINVAL;
+
+	vsc9959_new_base_time(ocelot, sgi->basetime, sgi->cycletime, &base_ts);
+	ocelot_write(ocelot, base_ts.tv_nsec, ANA_SG_CONFIG_REG_1);
+	val = lower_32_bits(base_ts.tv_sec);
+	ocelot_write(ocelot, val, ANA_SG_CONFIG_REG_2);
+
+	val = upper_32_bits(base_ts.tv_sec);
+	ocelot_write(ocelot,
+		     (sgi->ipv_valid ? ANA_SG_CONFIG_REG_3_IPV_VALID : 0) |
+		     ANA_SG_CONFIG_REG_3_INIT_IPV(sgi->init_ipv) |
+		     ANA_SG_CONFIG_REG_3_GATE_ENABLE |
+		     ANA_SG_CONFIG_REG_3_LIST_LENGTH(sgi->num_entries) |
+		     ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
+		     ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(val),
+		     ANA_SG_CONFIG_REG_3);
+
+	ocelot_write(ocelot, sgi->cycletime, ANA_SG_CONFIG_REG_4);
+
+	e = sgi->entries;
+	for (i = 0; i < sgi->num_entries; i++) {
+		u32 ips = (e[i].ipv < 0) ? 0 : (e[i].ipv + 8);
+
+		ocelot_write_rix(ocelot, ANA_SG_GCL_GS_CONFIG_IPS(ips) |
+				 (e[i].gate_state ?
+				  ANA_SG_GCL_GS_CONFIG_GATE_STATE : 0),
+				 ANA_SG_GCL_GS_CONFIG, i);
+
+		interval_sum += e[i].interval;
+		ocelot_write_rix(ocelot, interval_sum, ANA_SG_GCL_TI_CONFIG, i);
+	}
+
+	ocelot_rmw(ocelot, ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
+		   ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
+		   ANA_SG_ACCESS_CTRL);
+
+	return readx_poll_timeout(vsc9959_sgi_cfg_status, ocelot, val,
+				  (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
+				  10, 100000);
+}
+
+static int vsc9959_psfp_sgi_table_add(struct ocelot *ocelot,
+				      struct felix_stream_gate *sgi)
+{
+	struct felix_stream_gate_entry *tmp;
+	struct ocelot_psfp_list *psfp;
+	int ret;
+
+	psfp = &ocelot->psfp;
+
+	list_for_each_entry(tmp, &psfp->sgi_list, list)
+		if (tmp->index == sgi->index) {
+			refcount_inc(&tmp->refcount);
+			return 0;
+		}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	ret = vsc9959_psfp_sgi_set(ocelot, sgi);
+	if (ret) {
+		kfree(tmp);
+		return ret;
+	}
+
+	tmp->index = sgi->index;
+	refcount_set(&tmp->refcount, 1);
+	list_add_tail(&tmp->list, &psfp->sgi_list);
+
+	return 0;
+}
+
+static void vsc9959_psfp_sgi_table_del(struct ocelot *ocelot,
+				       u32 index)
+{
+	struct felix_stream_gate_entry *tmp, *n;
+	struct felix_stream_gate sgi = {0};
+	struct ocelot_psfp_list *psfp;
+	u8 z;
+
+	psfp = &ocelot->psfp;
+
+	list_for_each_entry_safe(tmp, n, &psfp->sgi_list, list)
+		if (tmp->index == index) {
+			z = refcount_dec_and_test(&tmp->refcount);
+			if (z) {
+				sgi.index = index;
+				sgi.enable = 0;
+				vsc9959_psfp_sgi_set(ocelot, &sgi);
+				list_del(&tmp->list);
+				kfree(tmp);
+			}
+			break;
+		}
+}
+
 static void vsc9959_psfp_counters_get(struct ocelot *ocelot, u32 index,
 				      struct felix_stream_filter_counters *counters)
 {
@@ -1671,8 +1850,9 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	const struct flow_action_entry *a;
 	struct felix_stream *stream_entry;
 	struct felix_stream stream = {0};
+	struct felix_stream_gate *sgi;
 	struct ocelot_psfp_list *psfp;
-	int ret, i;
+	int ret, i, size;
 
 	psfp = &ocelot->psfp;
 
@@ -1685,6 +1865,18 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	flow_action_for_each(i, a, &f->rule->action) {
 		switch (a->id) {
 		case FLOW_ACTION_GATE:
+			size = struct_size(sgi, entries, a->gate.num_entries);
+			sgi = kzalloc(size, GFP_KERNEL);
+			vsc9959_psfp_parse_gate(a, sgi);
+			ret = vsc9959_psfp_sgi_table_add(ocelot, sgi);
+			if (ret) {
+				kfree(sgi);
+				return ret;
+			}
+			sfi.sg_valid = 1;
+			sfi.sgid = sgi->index;
+			kfree(sgi);
+			break;
 		case FLOW_ACTION_POLICE:
 		default:
 			return -EOPNOTSUPP;
@@ -1695,7 +1887,8 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream);
 	if (stream_entry) {
 		NL_SET_ERR_MSG_MOD(extack, "This stream is already added");
-		return -EEXIST;
+		ret = -EEXIST;
+		goto err;
 	}
 
 	sfi.prio_valid = (stream.prio < 0 ? 0 : 1);
@@ -1704,14 +1897,22 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 
 	ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi);
 	if (ret)
-		return ret;
+		goto err;
 
 	stream.sfid = sfi.index;
 	stream.sfid_valid = 1;
 	ret = vsc9959_stream_table_add(ocelot, &psfp->stream_list,
 				       &stream, extack);
-	if (ret)
+	if (ret) {
 		vsc9959_psfp_sfi_table_del(ocelot, stream.sfid);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	if (sfi.sg_valid)
+		vsc9959_psfp_sgi_table_del(ocelot, sfi.sgid);
 
 	return ret;
 }
@@ -1719,6 +1920,7 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
 				   struct flow_cls_offload *f)
 {
+	static struct felix_stream_filter *sfi;
 	struct ocelot_psfp_list *psfp;
 	struct felix_stream *stream;
 
@@ -1728,6 +1930,13 @@ static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
 	if (!stream)
 		return -ENOMEM;
 
+	sfi = vsc9959_psfp_sfi_table_get(&psfp->sfi_list, stream->sfid);
+	if (!sfi)
+		return -ENOMEM;
+
+	if (sfi->sg_valid)
+		vsc9959_psfp_sgi_table_del(ocelot, sfi->sgid);
+
 	vsc9959_psfp_sfi_table_del(ocelot, stream->sfid);
 
 	stream->sfid_valid = 0;
-- 
2.17.1

[PATCH v4 net-next 7/8] net: mscc: ocelot: use index to set vcap policer

[Xiaoliang Yang]

Policer was previously automatically assigned from the highest index to
the lowest index from policer pool. But police action of tc flower now
uses index to set an police entry. This patch uses the police index to
set vcap policers, so that one policer can be shared by multiple rules.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
 drivers/net/dsa/ocelot/felix.c             |   2 +
 drivers/net/dsa/ocelot/felix.h             |   2 +
 drivers/net/dsa/ocelot/felix_vsc9959.c     |   4 +
 drivers/net/ethernet/mscc/ocelot_flower.c  |   5 +
 drivers/net/ethernet/mscc/ocelot_vcap.c    | 103 +++++++++++++--------
 drivers/net/ethernet/mscc/ocelot_vsc7514.c |   7 ++
 include/soc/mscc/ocelot.h                  |  11 ++-
 7 files changed, 96 insertions(+), 38 deletions(-)

diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index 3656e67af789..1505ef2016da 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -984,6 +984,8 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
 	ocelot->num_stats	= felix->info->num_stats;
 	ocelot->num_mact_rows	= felix->info->num_mact_rows;
 	ocelot->vcap		= felix->info->vcap;
+	ocelot->vcap_pol.base	= felix->info->vcap_pol_base;
+	ocelot->vcap_pol.max	= felix->info->vcap_pol_max;
 	ocelot->ops		= felix->info->ops;
 	ocelot->npi_inj_prefix	= OCELOT_TAG_PREFIX_SHORT;
 	ocelot->npi_xtr_prefix	= OCELOT_TAG_PREFIX_SHORT;
diff --git a/drivers/net/dsa/ocelot/felix.h b/drivers/net/dsa/ocelot/felix.h
index 5854bab43327..1a299717b8d1 100644
--- a/drivers/net/dsa/ocelot/felix.h
+++ b/drivers/net/dsa/ocelot/felix.h
@@ -21,6 +21,8 @@ struct felix_info {
 	int				num_ports;
 	int				num_tx_queues;
 	struct vcap_props		*vcap;
+	u16				vcap_pol_base;
+	u16				vcap_pol_max;
 	int				switch_pci_bar;
 	int				imdio_pci_bar;
 	const struct ptp_clock_info	*ptp_caps;
diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index 9264934631d7..1418d2a66bd6 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -19,6 +19,8 @@
 #include "felix.h"
 
 #define VSC9959_TAS_GCL_ENTRY_MAX	63
+#define VSC9959_VCAP_POLICER_BASE	63
+#define VSC9959_VCAP_POLICER_MAX	383
 
 static const u32 vsc9959_ana_regmap[] = {
 	REG(ANA_ADVLEARN,			0x0089a0),
@@ -1999,6 +2001,8 @@ static const struct felix_info felix_info_vsc9959 = {
 	.stats_layout		= vsc9959_stats_layout,
 	.num_stats		= ARRAY_SIZE(vsc9959_stats_layout),
 	.vcap			= vsc9959_vcap_props,
+	.vcap_pol_base		= VSC9959_VCAP_POLICER_BASE,
+	.vcap_pol_max		= VSC9959_VCAP_POLICER_MAX,
 	.num_mact_rows		= 2048,
 	.num_ports		= 6,
 	.num_tx_queues		= OCELOT_NUM_TC,
diff --git a/drivers/net/ethernet/mscc/ocelot_flower.c b/drivers/net/ethernet/mscc/ocelot_flower.c
index daeaee99933d..bc8a65c227ca 100644
--- a/drivers/net/ethernet/mscc/ocelot_flower.c
+++ b/drivers/net/ethernet/mscc/ocelot_flower.c
@@ -241,6 +241,11 @@ static int ocelot_flower_parse_action(struct ocelot *ocelot, int port,
 				return -EOPNOTSUPP;
 			}
 			filter->action.police_ena = true;
+			filter->action.pol_ix = a->police.index +
+						ocelot->vcap_pol.base;
+			if (filter->action.pol_ix > ocelot->vcap_pol.max)
+				return -EINVAL;
+
 			rate = a->police.rate_bytes_ps;
 			filter->action.pol.rate = div_u64(rate, 1000) * 8;
 			filter->action.pol.burst = a->police.burst;
diff --git a/drivers/net/ethernet/mscc/ocelot_vcap.c b/drivers/net/ethernet/mscc/ocelot_vcap.c
index 7945393a0655..1639c2780343 100644
--- a/drivers/net/ethernet/mscc/ocelot_vcap.c
+++ b/drivers/net/ethernet/mscc/ocelot_vcap.c
@@ -887,10 +887,18 @@ static void vcap_entry_set(struct ocelot *ocelot, int ix,
 		return es0_entry_set(ocelot, ix, filter);
 }
 
-static int ocelot_vcap_policer_add(struct ocelot *ocelot, u32 pol_ix,
-				   struct ocelot_policer *pol)
+struct vcap_policer_entry {
+	struct list_head list;
+	refcount_t refcount;
+	u32 pol_ix;
+};
+
+int ocelot_vcap_policer_add(struct ocelot *ocelot, u32 pol_ix,
+			    struct ocelot_policer *pol)
 {
 	struct qos_policer_conf pp = { 0 };
+	struct vcap_policer_entry *tmp;
+	int ret;
 
 	if (!pol)
 		return -EINVAL;
@@ -899,57 +907,74 @@ static int ocelot_vcap_policer_add(struct ocelot *ocelot, u32 pol_ix,
 	pp.pir = pol->rate;
 	pp.pbs = pol->burst;
 
-	return qos_policer_conf_set(ocelot, 0, pol_ix, &pp);
+	list_for_each_entry(tmp, &ocelot->vcap_pol.pol_list, list)
+		if (tmp->pol_ix == pol_ix) {
+			refcount_inc(&tmp->refcount);
+			return 0;
+		}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	ret = qos_policer_conf_set(ocelot, 0, pol_ix, &pp);
+	if (ret) {
+		kfree(tmp);
+		return ret;
+	}
+
+	tmp->pol_ix = pol_ix;
+	refcount_set(&tmp->refcount, 1);
+	list_add_tail(&tmp->list, &ocelot->vcap_pol.pol_list);
+
+	return 0;
 }
+EXPORT_SYMBOL(ocelot_vcap_policer_add);
 
-static void ocelot_vcap_policer_del(struct ocelot *ocelot,
-				    struct ocelot_vcap_block *block,
-				    u32 pol_ix)
+int ocelot_vcap_policer_del(struct ocelot *ocelot, u32 pol_ix)
 {
-	struct ocelot_vcap_filter *filter;
 	struct qos_policer_conf pp = {0};
-	int index = -1;
-
-	if (pol_ix < block->pol_lpr)
-		return;
-
-	list_for_each_entry(filter, &block->rules, list) {
-		index++;
-		if (filter->block_id == VCAP_IS2 &&
-		    filter->action.police_ena &&
-		    filter->action.pol_ix < pol_ix) {
-			filter->action.pol_ix += 1;
-			ocelot_vcap_policer_add(ocelot, filter->action.pol_ix,
-						&filter->action.pol);
-			is2_entry_set(ocelot, index, filter);
+	struct vcap_policer_entry *tmp, *n;
+	u8 z = 0;
+
+	list_for_each_entry_safe(tmp, n, &ocelot->vcap_pol.pol_list, list)
+		if (tmp->pol_ix == pol_ix) {
+			z = refcount_dec_and_test(&tmp->refcount);
+			if (z) {
+				list_del(&tmp->list);
+				kfree(tmp);
+			}
 		}
-	}
 
-	pp.mode = MSCC_QOS_RATE_MODE_DISABLED;
-	qos_policer_conf_set(ocelot, 0, pol_ix, &pp);
+	if (z) {
+		pp.mode = MSCC_QOS_RATE_MODE_DISABLED;
+		return qos_policer_conf_set(ocelot, 0, pol_ix, &pp);
+	}
 
-	block->pol_lpr++;
+	return 0;
 }
+EXPORT_SYMBOL(ocelot_vcap_policer_del);
 
-static void ocelot_vcap_filter_add_to_block(struct ocelot *ocelot,
-					    struct ocelot_vcap_block *block,
-					    struct ocelot_vcap_filter *filter)
+static int ocelot_vcap_filter_add_to_block(struct ocelot *ocelot,
+					   struct ocelot_vcap_block *block,
+					   struct ocelot_vcap_filter *filter)
 {
 	struct ocelot_vcap_filter *tmp;
 	struct list_head *pos, *n;
+	int ret;
 
 	if (filter->block_id == VCAP_IS2 && filter->action.police_ena) {
-		block->pol_lpr--;
-		filter->action.pol_ix = block->pol_lpr;
-		ocelot_vcap_policer_add(ocelot, filter->action.pol_ix,
-					&filter->action.pol);
+		ret = ocelot_vcap_policer_add(ocelot, filter->action.pol_ix,
+					      &filter->action.pol);
+		if (ret)
+			return ret;
 	}
 
 	block->count++;
 
 	if (list_empty(&block->rules)) {
 		list_add(&filter->list, &block->rules);
-		return;
+		return 0;
 	}
 
 	list_for_each_safe(pos, n, &block->rules) {
@@ -958,6 +983,8 @@ static void ocelot_vcap_filter_add_to_block(struct ocelot *ocelot,
 			break;
 	}
 	list_add(&filter->list, pos->prev);
+
+	return 0;
 }
 
 static bool ocelot_vcap_filter_equal(const struct ocelot_vcap_filter *a,
@@ -1132,7 +1159,7 @@ int ocelot_vcap_filter_add(struct ocelot *ocelot,
 			   struct netlink_ext_ack *extack)
 {
 	struct ocelot_vcap_block *block = &ocelot->block[filter->block_id];
-	int i, index;
+	int i, index, ret;
 
 	if (!ocelot_exclusive_mac_etype_filter_rules(ocelot, filter)) {
 		NL_SET_ERR_MSG_MOD(extack,
@@ -1141,7 +1168,9 @@ int ocelot_vcap_filter_add(struct ocelot *ocelot,
 	}
 
 	/* Add filter to the linked list */
-	ocelot_vcap_filter_add_to_block(ocelot, block, filter);
+	ret = ocelot_vcap_filter_add_to_block(ocelot, block, filter);
+	if (ret)
+		return ret;
 
 	/* Get the index of the inserted filter */
 	index = ocelot_vcap_block_get_filter_index(block, filter);
@@ -1174,7 +1203,7 @@ static void ocelot_vcap_block_remove_filter(struct ocelot *ocelot,
 		if (ocelot_vcap_filter_equal(filter, tmp)) {
 			if (tmp->block_id == VCAP_IS2 &&
 			    tmp->action.police_ena)
-				ocelot_vcap_policer_del(ocelot, block,
+				ocelot_vcap_policer_del(ocelot,
 							tmp->action.pol_ix);
 
 			list_del(pos);
@@ -1350,13 +1379,13 @@ int ocelot_vcap_init(struct ocelot *ocelot)
 		struct vcap_props *vcap = &ocelot->vcap[i];
 
 		INIT_LIST_HEAD(&block->rules);
-		block->pol_lpr = OCELOT_POLICER_DISCARD - 1;
 
 		ocelot_vcap_detect_constants(ocelot, vcap);
 		ocelot_vcap_init_one(ocelot, vcap);
 	}
 
 	INIT_LIST_HEAD(&ocelot->dummy_rules);
+	INIT_LIST_HEAD(&ocelot->vcap_pol.pol_list);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/mscc/ocelot_vsc7514.c b/drivers/net/ethernet/mscc/ocelot_vsc7514.c
index 291ae6817c26..403c47d05304 100644
--- a/drivers/net/ethernet/mscc/ocelot_vsc7514.c
+++ b/drivers/net/ethernet/mscc/ocelot_vsc7514.c
@@ -20,6 +20,9 @@
 #include <soc/mscc/ocelot_hsio.h>
 #include "ocelot.h"
 
+#define VSC7514_VCAP_POLICER_BASE			128
+#define VSC7514_VCAP_POLICER_MAX			191
+
 static const u32 ocelot_ana_regmap[] = {
 	REG(ANA_ADVLEARN,				0x009000),
 	REG(ANA_VLANMASK,				0x009004),
@@ -1128,6 +1131,10 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 	ocelot->num_flooding_pgids = 1;
 
 	ocelot->vcap = vsc7514_vcap_props;
+
+	ocelot->vcap_pol.base = VSC7514_VCAP_POLICER_BASE;
+	ocelot->vcap_pol.max = VSC7514_VCAP_POLICER_MAX;
+
 	ocelot->npi = -1;
 
 	err = ocelot_init(ocelot);
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index a611f9cd5935..fa006168e7fe 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -571,10 +571,15 @@ struct ocelot_ops {
 			      struct flow_stats *stats);
 };
 
+struct ocelot_vcap_policer {
+	struct list_head pol_list;
+	u16 base;
+	u16 max;
+};
+
 struct ocelot_vcap_block {
 	struct list_head rules;
 	int count;
-	int pol_lpr;
 };
 
 struct ocelot_vlan {
@@ -677,6 +682,7 @@ struct ocelot {
 
 	struct list_head		dummy_rules;
 	struct ocelot_vcap_block	block[3];
+	struct ocelot_vcap_policer	vcap_pol;
 	struct vcap_props		*vcap;
 
 	struct ocelot_psfp_list		psfp;
@@ -941,6 +947,9 @@ int ocelot_mact_lookup(struct ocelot *ocelot, const unsigned char mac[ETH_ALEN],
 void ocelot_mact_write(struct ocelot *ocelot, int port,
 		       const struct ocelot_mact_entry *entry,
 		       int row, int col);
+int ocelot_vcap_policer_add(struct ocelot *ocelot, u32 pol_ix,
+			    struct ocelot_policer *pol);
+int ocelot_vcap_policer_del(struct ocelot *ocelot, u32 pol_ix);
 
 #if IS_ENABLED(CONFIG_BRIDGE_MRP)
 int ocelot_mrp_add(struct ocelot *ocelot, int port,
-- 
2.17.1

Re: [PATCH v4 net-next 7/8] net: mscc: ocelot: use index to set vcap policer

[Vladimir Oltean]

On Wed, Sep 22, 2021 at 06:52:01PM +0800, Xiaoliang Yang wrote:
> Policer was previously automatically assigned from the highest index to
> the lowest index from policer pool. But police action of tc flower now
> uses index to set an police entry. This patch uses the police index to
> set vcap policers, so that one policer can be shared by multiple rules.
> 
> Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
> ---
> +#define VSC9959_VCAP_POLICER_BASE	63
> +#define VSC9959_VCAP_POLICER_MAX	383
>  

> +#define VSC7514_VCAP_POLICER_BASE			128
> +#define VSC7514_VCAP_POLICER_MAX			191

I think this deserves an explanation.

The VSC7514 driver uses the max number of policers as 383 (0x17f) ever
since commit b596229448dd ("net: mscc: ocelot: Add support for tcam"),
aka the very beginning.

Yet, the documentation at "3.10.1 Policer Allocation"
https://ww1.microchip.com/downloads/en/DeviceDoc/VMDS-10491.pdf
says very clearly that there are only 192 policers indeed.

What's going on?

Also, FWIW, Seville has this policer allocation:

      0 ----+----------------------+
            |  Port Policers (11)  |
     11 ----+----------------------+
            |  VCAP Policers (21)  |
     32 ----+----------------------+
            |   QoS Policers (88)  |
    120 ----+----------------------+
            |  VCAP Policers (43)  |
    162 ----+----------------------+

RE: [PATCH v4 net-next 7/8] net: mscc: ocelot: use index to set vcap policer

[Xiaoliang Yang]

On Wed, Sep 22, 2021 at 13:18:37 +0000, Vladimir Oltean wrote:
> > Policer was previously automatically assigned from the highest index
> > to the lowest index from policer pool. But police action of tc flower
> > now uses index to set an police entry. This patch uses the police
> > index to set vcap policers, so that one policer can be shared by multiple
> rules.
> >
> > Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
> > ---
> > +#define VSC9959_VCAP_POLICER_BASE	63
> > +#define VSC9959_VCAP_POLICER_MAX	383
> >
> 
> > +#define VSC7514_VCAP_POLICER_BASE			128
> > +#define VSC7514_VCAP_POLICER_MAX			191
> 
> I think this deserves an explanation.
> 
> The VSC7514 driver uses the max number of policers as 383 (0x17f) ever since
> commit b596229448dd ("net: mscc: ocelot: Add support for tcam"), aka the
> very beginning.
> 
> Yet, the documentation at "3.10.1 Policer Allocation"
> https://ww1.microchip.com/downloads/en/DeviceDoc/VMDS-10491.pdf
> says very clearly that there are only 192 policers indeed.
> 
> What's going on?

In commit commit b596229448dd ("net: mscc: ocelot: Add support for tcam"), Horatiu Vultur define the max number of policers as 383:
+#define OCELOT_POLICER_DISCARD 0x17f
VCAP IS2 use this policer to set drop action. I did not change this and set the VCAP policers with 128-191 according to the VSC7514 document.

I don't know why 383 was used as the maximum value of policer in the original code. Can Microchip people check the code or the documentation for errors?

> 
> Also, FWIW, Seville has this policer allocation:
> 
>       0 ----+----------------------+
>             |  Port Policers (11)  |
>      11 ----+----------------------+
>             |  VCAP Policers (21)  |
>      32 ----+----------------------+
>             |   QoS Policers (88)  |
>     120 ----+----------------------+
>             |  VCAP Policers (43)  |
>     162 ----+----------------------+

I didn't find Seville's document, if this allocation is right, I will add it in Seville driver.

Thanks,
Xiaoliang

Re: [PATCH v4 net-next 7/8] net: mscc: ocelot: use index to set vcap policer

[Horatiu Vultur]

The 09/23/2021 01:52, Xiaoliang Yang wrote:
> EXTERNAL EMAIL: Do not click links or open attachments unless you know the content is safe
> 
> On Wed, Sep 22, 2021 at 13:18:37 +0000, Vladimir Oltean wrote:
> > > Policer was previously automatically assigned from the highest index
> > > to the lowest index from policer pool. But police action of tc flower
> > > now uses index to set an police entry. This patch uses the police
> > > index to set vcap policers, so that one policer can be shared by multiple
> > rules.
> > >
> > > Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
> > > ---
> > > +#define VSC9959_VCAP_POLICER_BASE  63
> > > +#define VSC9959_VCAP_POLICER_MAX   383
> > >
> >
> > > +#define VSC7514_VCAP_POLICER_BASE                  128
> > > +#define VSC7514_VCAP_POLICER_MAX                   191
> >
> > I think this deserves an explanation.
> >
> > The VSC7514 driver uses the max number of policers as 383 (0x17f) ever since
> > commit b596229448dd ("net: mscc: ocelot: Add support for tcam"), aka the
> > very beginning.
> >
> > Yet, the documentation at "3.10.1 Policer Allocation"
> > https://ww1.microchip.com/downloads/en/DeviceDoc/VMDS-10491.pdf
> > says very clearly that there are only 192 policers indeed.
> >
> > What's going on?
> 
> In commit commit b596229448dd ("net: mscc: ocelot: Add support for tcam"), Horatiu Vultur define the max number of policers as 383:
> +#define OCELOT_POLICER_DISCARD 0x17f
> VCAP IS2 use this policer to set drop action. I did not change this and set the VCAP policers with 128-191 according to the VSC7514 document.
> 
> I don't know why 383 was used as the maximum value of policer in the original code. Can Microchip people check the code or the documentation for errors?

It was defined as 383 because the HW actually support this number of
policers. But for this SKU it is recomended to use 191, but no one will
stop you from using 383.

> 
> >
> > Also, FWIW, Seville has this policer allocation:
> >
> >       0 ----+----------------------+
> >             |  Port Policers (11)  |
> >      11 ----+----------------------+
> >             |  VCAP Policers (21)  |
> >      32 ----+----------------------+
> >             |   QoS Policers (88)  |
> >     120 ----+----------------------+
> >             |  VCAP Policers (43)  |
> >     162 ----+----------------------+
> 
> I didn't find Seville's document, if this allocation is right, I will add it in Seville driver.
> 
> Thanks,
> Xiaoliang

-- 
/Horatiu

Re: [PATCH v4 net-next 7/8] net: mscc: ocelot: use index to set vcap policer

[Vladimir Oltean]

On Thu, Sep 23, 2021 at 09:30:59AM +0200, Horatiu Vultur wrote:
> > In commit commit b596229448dd ("net: mscc: ocelot: Add support for tcam"), Horatiu Vultur define the max number of policers as 383:
> > +#define OCELOT_POLICER_DISCARD 0x17f
> > VCAP IS2 use this policer to set drop action. I did not change this and set the VCAP policers with 128-191 according to the VSC7514 document.
> >
> > I don't know why 383 was used as the maximum value of policer in the original code. Can Microchip people check the code or the documentation for errors?
>
> It was defined as 383 because the HW actually support this number of
> policers. But for this SKU it is recomended to use 191, but no one will
> stop you from using 383.

So if it is recommended to use 191, why did you use 383? Should Xiaoliang
change that to 191, or leave it alone?

> > > Also, FWIW, Seville has this policer allocation:
> > >
> > >       0 ----+----------------------+
> > >             |  Port Policers (11)  |
> > >      11 ----+----------------------+
> > >             |  VCAP Policers (21)  |
> > >      32 ----+----------------------+
> > >             |   QoS Policers (88)  |
> > >     120 ----+----------------------+
> > >             |  VCAP Policers (43)  |
> > >     162 ----+----------------------+
> >
> > I didn't find Seville's document, if this allocation is right, I will add it in Seville driver.

Strange enough, I don't remember having reports about the VCAP IS2
policers on Seville not working, and of course being in the common code,
we'd start with a count of 384 policers for that hardware too, and
counting from the end. I think I even tested the policers when adding
the VCAP IS2 constants, and they worked. Is there any sort of index
wraparound that takes place?

Re: [PATCH v4 net-next 7/8] net: mscc: ocelot: use index to set vcap policer

[Horatiu Vultur]

The 09/23/2021 09:22, Vladimir Oltean wrote:
> EXTERNAL EMAIL: Do not click links or open attachments unless you know the content is safe
> 
> On Thu, Sep 23, 2021 at 09:30:59AM +0200, Horatiu Vultur wrote:
> > > In commit commit b596229448dd ("net: mscc: ocelot: Add support for tcam"), Horatiu Vultur define the max number of policers as 383:
> > > +#define OCELOT_POLICER_DISCARD 0x17f
> > > VCAP IS2 use this policer to set drop action. I did not change this and set the VCAP policers with 128-191 according to the VSC7514 document.
> > >
> > > I don't know why 383 was used as the maximum value of policer in the original code. Can Microchip people check the code or the documentation for errors?
> >
> > It was defined as 383 because the HW actually support this number of
> > policers. But for this SKU it is recomended to use 191, but no one will
> > stop you from using 383.
> 
> So if it is recommended to use 191, why did you use 383? Should Xiaoliang
> change that to 191, or leave it alone?

I think is better to leave it alone. I am not aware of doing any hard if
the value is 383.

> 
> > > > Also, FWIW, Seville has this policer allocation:
> > > >
> > > >       0 ----+----------------------+
> > > >             |  Port Policers (11)  |
> > > >      11 ----+----------------------+
> > > >             |  VCAP Policers (21)  |
> > > >      32 ----+----------------------+
> > > >             |   QoS Policers (88)  |
> > > >     120 ----+----------------------+
> > > >             |  VCAP Policers (43)  |
> > > >     162 ----+----------------------+
> > >
> > > I didn't find Seville's document, if this allocation is right, I will add it in Seville driver.
> 
> Strange enough, I don't remember having reports about the VCAP IS2
> policers on Seville not working, and of course being in the common code,
> we'd start with a count of 384 policers for that hardware too, and
> counting from the end. I think I even tested the policers when adding
> the VCAP IS2 constants, and they worked. Is there any sort of index
> wraparound that takes place?

I don't think there is any wraparound.

-- 
/Horatiu

[PATCH v4 net-next 8/8] net: dsa: felix: use vcap policer to set flow meter for psfp

[Xiaoliang Yang]

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, so the PSFP
policer calls ocelot_vcap_policer_add() and ocelot_vcap_policer_del() to
set flow meter police.

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

diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index 1418d2a66bd6..1118101d0ee8 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -1343,6 +1343,7 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 
 #define VSC9959_PSFP_SFID_MAX			175
 #define VSC9959_PSFP_GATE_ID_MAX		183
+#define VSC9959_PSFP_POLICER_BASE		63
 #define VSC9959_PSFP_POLICER_MAX		383
 #define VSC9959_PSFP_GATE_LIST_NUM		4
 #define VSC9959_PSFP_GATE_CYCLETIME_MIN		5000
@@ -1854,7 +1855,10 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	struct felix_stream stream = {0};
 	struct felix_stream_gate *sgi;
 	struct ocelot_psfp_list *psfp;
+	struct ocelot_policer pol;
 	int ret, i, size;
+	u64 rate, burst;
+	u32 index;
 
 	psfp = &ocelot->psfp;
 
@@ -1873,13 +1877,33 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 			ret = vsc9959_psfp_sgi_table_add(ocelot, sgi);
 			if (ret) {
 				kfree(sgi);
-				return ret;
+				goto err;
 			}
 			sfi.sg_valid = 1;
 			sfi.sgid = sgi->index;
 			kfree(sgi);
 			break;
 		case FLOW_ACTION_POLICE:
+			index = a->police.index + VSC9959_PSFP_POLICER_BASE;
+			if (index > VSC9959_PSFP_POLICER_MAX) {
+				ret = -EINVAL;
+				goto err;
+			}
+
+			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)
+				goto err;
+
+			sfi.fm_valid = 1;
+			sfi.fmid = index;
+			sfi.maxsdu = a->police.mtu;
+			break;
 		default:
 			return -EOPNOTSUPP;
 		}
@@ -1916,6 +1940,9 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	if (sfi.sg_valid)
 		vsc9959_psfp_sgi_table_del(ocelot, sfi.sgid);
 
+	if (sfi.fm_valid)
+		ocelot_vcap_policer_del(ocelot, sfi.fmid);
+
 	return ret;
 }
 
@@ -1939,6 +1966,9 @@ static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
 	if (sfi->sg_valid)
 		vsc9959_psfp_sgi_table_del(ocelot, sfi->sgid);
 
+	if (sfi->fm_valid)
+		ocelot_vcap_policer_del(ocelot, sfi->fmid);
+
 	vsc9959_psfp_sfi_table_del(ocelot, stream->sfid);
 
 	stream->sfid_valid = 0;
-- 
2.17.1

[PATCH v4 net-next 0/8] net: dsa: felix: psfp support on vsc9959

[Xiaoliang Yang]

VSC9959 hardware supports Per-Stream Filtering and Policing(PSFP).
This patch series add PSFP support on tc flower offload of ocelot
driver. Use chain 30000 to distinguish PSFP from VCAP blocks. Add gate
and police set to support PSFP in VSC9959 driver.

v3->v4 changes:
 - Introduce vsc9959_psfp_sfi_table_get() function in patch where it is
   used to fix compile warning.
 - Store MAC entry type before FRER set, and recover it after FRER
   disabled.

v2->v3 changes:
 - Reorder first two patches. Export struct ocelot_mact_entry, then add
   ocelot_mact_lookup() and ocelot_mact_write() functions.
 - Add PSFP list to struct ocelot, and init it by using
   ocelot->ops->psfp_init().

v1->v2 changes:
 - Use tc flower offload of ocelot driver to support PSFP add and delete.
 - Add PSFP tables add/del functions in felix_vsc9959.c.
 - Use list_for_each_entry to simplify the code.

Vladimir Oltean (2):
  net: mscc: ocelot: export struct ocelot_mact_entry
  net: mscc: ocelot: add MAC table write and lookup operations

Xiaoliang Yang (6):
  net: mscc: ocelot: set vcap IS2 chain to goto PSFP chain
  net: mscc: ocelot: add gate and police action offload to PSFP
  net: dsa: felix: support psfp filter on vsc9959
  net: dsa: felix: add stream gate settings for psfp
  net: mscc: ocelot: use index to set vcap policer
  net: dsa: felix: use vcap policer to set flow meter for psfp

 drivers/net/dsa/ocelot/felix.c             |   2 +
 drivers/net/dsa/ocelot/felix.h             |   2 +
 drivers/net/dsa/ocelot/felix_vsc9959.c     | 697 ++++++++++++++++++++-
 drivers/net/ethernet/mscc/ocelot.c         |  56 +-
 drivers/net/ethernet/mscc/ocelot.h         |  13 -
 drivers/net/ethernet/mscc/ocelot_flower.c  |  74 ++-
 drivers/net/ethernet/mscc/ocelot_vcap.c    | 103 +--
 drivers/net/ethernet/mscc/ocelot_vsc7514.c |   7 +
 include/soc/mscc/ocelot.h                  |  49 +-
 include/soc/mscc/ocelot_ana.h              |  10 +
 include/soc/mscc/ocelot_vcap.h             |   1 +
 11 files changed, 942 insertions(+), 72 deletions(-)

-- 
2.17.1

[PATCH v4 net-next 6/8] net: dsa: felix: add stream gate settings for psfp

[Xiaoliang Yang]

This patch adds stream gate settings for PSFP. Use SGI table to store
stream gate entries. Disable the gate entry when it is not used by any
stream.

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

diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index 52d10aab93b2..9264934631d7 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -8,6 +8,7 @@
 #include <soc/mscc/ocelot_ana.h>
 #include <soc/mscc/ocelot_ptp.h>
 #include <soc/mscc/ocelot_sys.h>
+#include <net/tc_act/tc_gate.h>
 #include <soc/mscc/ocelot.h>
 #include <linux/dsa/ocelot.h>
 #include <linux/pcs-lynx.h>
@@ -1341,6 +1342,8 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 #define VSC9959_PSFP_SFID_MAX			175
 #define VSC9959_PSFP_GATE_ID_MAX		183
 #define VSC9959_PSFP_POLICER_MAX		383
+#define VSC9959_PSFP_GATE_LIST_NUM		4
+#define VSC9959_PSFP_GATE_CYCLETIME_MIN		5000
 
 struct felix_stream {
 	struct list_head list;
@@ -1374,6 +1377,24 @@ struct felix_stream_filter_counters {
 	u32 red;
 };
 
+struct felix_stream_gate {
+	u32 index;
+	u8 enable;
+	u8 ipv_valid;
+	u8 init_ipv;
+	u64 basetime;
+	u64 cycletime;
+	u64 cycletime_ext;
+	u32 num_entries;
+	struct action_gate_entry entries[0];
+};
+
+struct felix_stream_gate_entry {
+	struct list_head list;
+	refcount_t refcount;
+	u32 index;
+};
+
 static int vsc9959_stream_identify(struct flow_cls_offload *f,
 				   struct felix_stream *stream)
 {
@@ -1623,6 +1644,18 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot,
 	return 0;
 }
 
+static struct felix_stream_filter *
+vsc9959_psfp_sfi_table_get(struct list_head *sfi_list, u32 index)
+{
+	struct felix_stream_filter *tmp;
+
+	list_for_each_entry(tmp, sfi_list, list)
+		if (tmp->index == index)
+			return tmp;
+
+	return NULL;
+}
+
 static void vsc9959_psfp_sfi_table_del(struct ocelot *ocelot, u32 index)
 {
 	struct felix_stream_filter *tmp, *n;
@@ -1644,6 +1677,152 @@ static void vsc9959_psfp_sfi_table_del(struct ocelot *ocelot, u32 index)
 		}
 }
 
+static void vsc9959_psfp_parse_gate(const struct flow_action_entry *entry,
+				    struct felix_stream_gate *sgi)
+{
+	sgi->index = entry->gate.index;
+	sgi->ipv_valid = (entry->gate.prio < 0) ? 0 : 1;
+	sgi->init_ipv = (sgi->ipv_valid) ? entry->gate.prio : 0;
+	sgi->basetime = entry->gate.basetime;
+	sgi->cycletime = entry->gate.cycletime;
+	sgi->num_entries = entry->gate.num_entries;
+	sgi->enable = 1;
+
+	memcpy(sgi->entries, entry->gate.entries,
+	       entry->gate.num_entries * sizeof(struct action_gate_entry));
+}
+
+static u32 vsc9959_sgi_cfg_status(struct ocelot *ocelot)
+{
+	return ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
+}
+
+static int vsc9959_psfp_sgi_set(struct ocelot *ocelot,
+				struct felix_stream_gate *sgi)
+{
+	struct action_gate_entry *e;
+	struct timespec64 base_ts;
+	u32 interval_sum = 0;
+	u32 val;
+	int i;
+
+	if (sgi->index > VSC9959_PSFP_GATE_ID_MAX)
+		return -EINVAL;
+
+	ocelot_write(ocelot, ANA_SG_ACCESS_CTRL_SGID(sgi->index),
+		     ANA_SG_ACCESS_CTRL);
+
+	if (!sgi->enable) {
+		ocelot_rmw(ocelot, ANA_SG_CONFIG_REG_3_INIT_GATE_STATE,
+			   ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
+			   ANA_SG_CONFIG_REG_3_GATE_ENABLE,
+			   ANA_SG_CONFIG_REG_3);
+
+		return 0;
+	}
+
+	if (sgi->cycletime < VSC9959_PSFP_GATE_CYCLETIME_MIN ||
+	    sgi->cycletime > NSEC_PER_SEC)
+		return -EINVAL;
+
+	if (sgi->num_entries > VSC9959_PSFP_GATE_LIST_NUM)
+		return -EINVAL;
+
+	vsc9959_new_base_time(ocelot, sgi->basetime, sgi->cycletime, &base_ts);
+	ocelot_write(ocelot, base_ts.tv_nsec, ANA_SG_CONFIG_REG_1);
+	val = lower_32_bits(base_ts.tv_sec);
+	ocelot_write(ocelot, val, ANA_SG_CONFIG_REG_2);
+
+	val = upper_32_bits(base_ts.tv_sec);
+	ocelot_write(ocelot,
+		     (sgi->ipv_valid ? ANA_SG_CONFIG_REG_3_IPV_VALID : 0) |
+		     ANA_SG_CONFIG_REG_3_INIT_IPV(sgi->init_ipv) |
+		     ANA_SG_CONFIG_REG_3_GATE_ENABLE |
+		     ANA_SG_CONFIG_REG_3_LIST_LENGTH(sgi->num_entries) |
+		     ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
+		     ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(val),
+		     ANA_SG_CONFIG_REG_3);
+
+	ocelot_write(ocelot, sgi->cycletime, ANA_SG_CONFIG_REG_4);
+
+	e = sgi->entries;
+	for (i = 0; i < sgi->num_entries; i++) {
+		u32 ips = (e[i].ipv < 0) ? 0 : (e[i].ipv + 8);
+
+		ocelot_write_rix(ocelot, ANA_SG_GCL_GS_CONFIG_IPS(ips) |
+				 (e[i].gate_state ?
+				  ANA_SG_GCL_GS_CONFIG_GATE_STATE : 0),
+				 ANA_SG_GCL_GS_CONFIG, i);
+
+		interval_sum += e[i].interval;
+		ocelot_write_rix(ocelot, interval_sum, ANA_SG_GCL_TI_CONFIG, i);
+	}
+
+	ocelot_rmw(ocelot, ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
+		   ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
+		   ANA_SG_ACCESS_CTRL);
+
+	return readx_poll_timeout(vsc9959_sgi_cfg_status, ocelot, val,
+				  (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
+				  10, 100000);
+}
+
+static int vsc9959_psfp_sgi_table_add(struct ocelot *ocelot,
+				      struct felix_stream_gate *sgi)
+{
+	struct felix_stream_gate_entry *tmp;
+	struct ocelot_psfp_list *psfp;
+	int ret;
+
+	psfp = &ocelot->psfp;
+
+	list_for_each_entry(tmp, &psfp->sgi_list, list)
+		if (tmp->index == sgi->index) {
+			refcount_inc(&tmp->refcount);
+			return 0;
+		}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	ret = vsc9959_psfp_sgi_set(ocelot, sgi);
+	if (ret) {
+		kfree(tmp);
+		return ret;
+	}
+
+	tmp->index = sgi->index;
+	refcount_set(&tmp->refcount, 1);
+	list_add_tail(&tmp->list, &psfp->sgi_list);
+
+	return 0;
+}
+
+static void vsc9959_psfp_sgi_table_del(struct ocelot *ocelot,
+				       u32 index)
+{
+	struct felix_stream_gate_entry *tmp, *n;
+	struct felix_stream_gate sgi = {0};
+	struct ocelot_psfp_list *psfp;
+	u8 z;
+
+	psfp = &ocelot->psfp;
+
+	list_for_each_entry_safe(tmp, n, &psfp->sgi_list, list)
+		if (tmp->index == index) {
+			z = refcount_dec_and_test(&tmp->refcount);
+			if (z) {
+				sgi.index = index;
+				sgi.enable = 0;
+				vsc9959_psfp_sgi_set(ocelot, &sgi);
+				list_del(&tmp->list);
+				kfree(tmp);
+			}
+			break;
+		}
+}
+
 static void vsc9959_psfp_counters_get(struct ocelot *ocelot, u32 index,
 				      struct felix_stream_filter_counters *counters)
 {
@@ -1671,8 +1850,9 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	const struct flow_action_entry *a;
 	struct felix_stream *stream_entry;
 	struct felix_stream stream = {0};
+	struct felix_stream_gate *sgi;
 	struct ocelot_psfp_list *psfp;
-	int ret, i;
+	int ret, i, size;
 
 	psfp = &ocelot->psfp;
 
@@ -1685,6 +1865,18 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	flow_action_for_each(i, a, &f->rule->action) {
 		switch (a->id) {
 		case FLOW_ACTION_GATE:
+			size = struct_size(sgi, entries, a->gate.num_entries);
+			sgi = kzalloc(size, GFP_KERNEL);
+			vsc9959_psfp_parse_gate(a, sgi);
+			ret = vsc9959_psfp_sgi_table_add(ocelot, sgi);
+			if (ret) {
+				kfree(sgi);
+				return ret;
+			}
+			sfi.sg_valid = 1;
+			sfi.sgid = sgi->index;
+			kfree(sgi);
+			break;
 		case FLOW_ACTION_POLICE:
 		default:
 			return -EOPNOTSUPP;
@@ -1695,7 +1887,8 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 	stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream);
 	if (stream_entry) {
 		NL_SET_ERR_MSG_MOD(extack, "This stream is already added");
-		return -EEXIST;
+		ret = -EEXIST;
+		goto err;
 	}
 
 	sfi.prio_valid = (stream.prio < 0 ? 0 : 1);
@@ -1704,14 +1897,22 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 
 	ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi);
 	if (ret)
-		return ret;
+		goto err;
 
 	stream.sfid = sfi.index;
 	stream.sfid_valid = 1;
 	ret = vsc9959_stream_table_add(ocelot, &psfp->stream_list,
 				       &stream, extack);
-	if (ret)
+	if (ret) {
 		vsc9959_psfp_sfi_table_del(ocelot, stream.sfid);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	if (sfi.sg_valid)
+		vsc9959_psfp_sgi_table_del(ocelot, sfi.sgid);
 
 	return ret;
 }
@@ -1719,6 +1920,7 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
 static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
 				   struct flow_cls_offload *f)
 {
+	static struct felix_stream_filter *sfi;
 	struct ocelot_psfp_list *psfp;
 	struct felix_stream *stream;
 
@@ -1728,6 +1930,13 @@ static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
 	if (!stream)
 		return -ENOMEM;
 
+	sfi = vsc9959_psfp_sfi_table_get(&psfp->sfi_list, stream->sfid);
+	if (!sfi)
+		return -ENOMEM;
+
+	if (sfi->sg_valid)
+		vsc9959_psfp_sgi_table_del(ocelot, sfi->sgid);
+
 	vsc9959_psfp_sfi_table_del(ocelot, stream->sfid);
 
 	stream->sfid_valid = 0;
-- 
2.17.1