[v1,net-next,3/5] net: dsa: felix: add gate action offload based on tc flower

Commit Message

Xiaoliang Yang Oct. 20, 2020, 7:23 a.m. UTC

VSC9959 supports Per-Stream Filtering and Policing(PSFP). Sream is
identified by Null Stream identification which is defined in
IEEE802.1Qci.

For IEEE 802.1Qci, there are four tables need to set: stream table,
stream filter table, stream gate table, and flow meter table. This
patch is using TC flower gate action to set stream gate table, using
TC flower keys{dst_mac, vlan_id} to identify a stream and set the
stream table. Stream filter table is maintained automatically, and
it's index is determined by SGID(stream gate index) and FMID(flow
meter index).

On the ocelot driver, there is also a TC flower offload to set up
VCAPs. We check the chain ID to offload the rule on felix driver to
run PSFP flow.

An example to set stream gate:
	> tc qdisc add dev swp0 clsact
	> tc filter add dev swp0 ingress chain 30000 protocol 802.1Q
		flower skip_sw dst_mac  CA:9C:00:BC:6D:68 vlan_id 1 \
		action gate index 1 base-time 0 \
			sched-entry CLOSE 6000 3 -1

Signed-off-by: Xiaoliang Yang <[e-mail]>
---
 drivers/net/dsa/ocelot/Makefile        |   3 +-
 drivers/net/dsa/ocelot/felix.c         |  23 +
 drivers/net/dsa/ocelot/felix.h         |  16 +
 drivers/net/dsa/ocelot/felix_flower.c  | 651 +++++++++++++++++++++++++
 drivers/net/dsa/ocelot/felix_vsc9959.c |  10 +-
 include/soc/mscc/ocelot_ana.h          |  10 +
 6 files changed, 709 insertions(+), 4 deletions(-)
 create mode 100644 drivers/net/dsa/ocelot/felix_flower.c

Comments

Vladimir Oltean Oct. 21, 2020, 12:23 a.m. UTC | #1

On Tue, Oct 20, 2020 at 03:23:19PM +0800, Xiaoliang Yang wrote:
> VSC9959 supports Per-Stream Filtering and Policing(PSFP). Sream is
> identified by Null Stream identification which is defined in
> IEEE802.1Qci.
> 
> For IEEE 802.1Qci, there are four tables need to set: stream table,
> stream filter table, stream gate table, and flow meter table. This
> patch is using TC flower gate action to set stream gate table, using
> TC flower keys{dst_mac, vlan_id} to identify a stream and set the
> stream table. Stream filter table is maintained automatically, and
> it's index is determined by SGID(stream gate index) and FMID(flow
> meter index).
> 
> On the ocelot driver, there is also a TC flower offload to set up
> VCAPs. We check the chain ID to offload the rule on felix driver to
> run PSFP flow.
> 
> An example to set stream gate:
> 	> tc qdisc add dev swp0 clsact
> 	> tc filter add dev swp0 ingress chain 30000 protocol 802.1Q
> 		flower skip_sw dst_mac  CA:9C:00:BC:6D:68 vlan_id 1 \
> 		action gate index 1 base-time 0 \
> 			sched-entry CLOSE 6000 3 -1

I believe there is one extra level of indentation here than you
intended. Also, since we're speaking of examples. Would you be kind
enough to create a new selftest where you demonstrate the tc-gate
functionality? You can easily base that selftest on
tools/testing/selftests/drivers/net/ocelot/tc_flower_chains.sh which
uses a loopback scheme with the 4 switch ports. And because you would be
using ports in loopback, you don't even need to run PTP, because the
sender, receiver and switch ports will all be in sync with one another,
as they share the same PHC.

What I would like to see is:

           swp0          |        swp1       |    swp2     |   swp3
---------------------------------------------------------------------
           Sender             ingress           egress
(scheduled with tc-taprio)    ------>   switch  -------->   Receiver
                           (tc-gate here)

If you have no better idea how to send time-based traffic and you can't
get mausezahn to do that either (I don't think you can), you can use
isochron:
https://github.com/vladimiroltean/tsn-scripts/tree/isochron/isochron

Once you do that, everybody will have a good understanding of how to use
it, as well as a working example and not just a wall of text.

> 
> Signed-off-by: Xiaoliang Yang <[e-mail]>
> ---
>  drivers/net/dsa/ocelot/Makefile        |   3 +-
>  drivers/net/dsa/ocelot/felix.c         |  23 +
>  drivers/net/dsa/ocelot/felix.h         |  16 +
>  drivers/net/dsa/ocelot/felix_flower.c  | 651 +++++++++++++++++++++++++
>  drivers/net/dsa/ocelot/felix_vsc9959.c |  10 +-
>  include/soc/mscc/ocelot_ana.h          |  10 +
>  6 files changed, 709 insertions(+), 4 deletions(-)
>  create mode 100644 drivers/net/dsa/ocelot/felix_flower.c
> 
> diff --git a/drivers/net/dsa/ocelot/Makefile b/drivers/net/dsa/ocelot/Makefile
> index f6dd131e7491..22f3f98914e3 100644
> --- a/drivers/net/dsa/ocelot/Makefile
> +++ b/drivers/net/dsa/ocelot/Makefile
> @@ -4,7 +4,8 @@ obj-$(CONFIG_NET_DSA_MSCC_SEVILLE) += mscc_seville.o
>  
>  mscc_felix-objs := \
>  	felix.o \
> -	felix_vsc9959.o
> +	felix_vsc9959.o \
> +	felix_flower.o
>  
>  mscc_seville-objs := \
>  	felix.o \
> diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
> index f791860d495f..42f972d10539 100644
> --- a/drivers/net/dsa/ocelot/felix.c
> +++ b/drivers/net/dsa/ocelot/felix.c
> @@ -716,6 +716,13 @@ static int felix_cls_flower_add(struct dsa_switch *ds, int port,
>  				struct flow_cls_offload *cls, bool ingress)
>  {
>  	struct ocelot *ocelot = ds->priv;
> +	struct felix *felix = ocelot_to_felix(ocelot);
> +
> +	if (felix->info->flower_replace) {
> +		if (cls->common.chain_index == OCELOT_PSFP_CHAIN)
> +			return felix->info->flower_replace(ocelot, port, cls,
> +							   ingress);
> +	}
>  
>  	return ocelot_cls_flower_replace(ocelot, port, cls, ingress);
>  }
> @@ -724,6 +731,14 @@ static int felix_cls_flower_del(struct dsa_switch *ds, int port,
>  				struct flow_cls_offload *cls, bool ingress)
>  {
>  	struct ocelot *ocelot = ds->priv;
> +	struct felix *felix = ocelot_to_felix(ocelot);
> +	int ret;
> +
> +	if (felix->info->flower_destroy) {
> +		ret = felix->info->flower_destroy(ocelot, port, cls, ingress);
> +		if (!ret)
> +			return 0;

To be honest I don't particularly enjoy having these function pointers
mask the functionality from mscc_ocelot like this. For example here. If
felix->info->flower_destroy for a PSFP rule returns an error, what you
do is you go ahead and call ocelot_cls_flower_destroy which is incorrect,
you should just return that error code to the user.

Have you tried putting these ops for the PSFP chain in struct
ocelot_ops, and just let ocelot_flower.c check the chain number and the
presence of those function pointers? The code path would be:

slave.c: dsa_slave_add_cls_flower
-> felix.c: felix_cls_flower_add
   -> ocelot_flower.c: ocelot_cls_flower_replace
      -> felix_vsc9959.c: pick a name here (vsc9959_gate_entry_add ?)

I recommend implementing the features that are only available on the
VSC9959 hardware in felix_vsc9959.c. Then, when new hardware will come
and we'll have a real idea of what's common and what isn't (unlike now
when we would just be guessing), and we'll refactor appropriately like
we did with the Lynx PCS. But that doesn't mean we should completely
bypass ocelot_flower.c.

> +	}
>  
>  	return ocelot_cls_flower_destroy(ocelot, port, cls, ingress);
>  }
> @@ -732,6 +747,14 @@ static int felix_cls_flower_stats(struct dsa_switch *ds, int port,
>  				  struct flow_cls_offload *cls, bool ingress)
>  {
>  	struct ocelot *ocelot = ds->priv;
> +	struct felix *felix = ocelot_to_felix(ocelot);
> +	int ret;
> +
> +	if (felix->info->flower_stats) {
> +		ret = felix->info->flower_stats(ocelot, port, cls, ingress);
> +		if (!ret)
> +			return 0;
> +	}
>  
>  	return ocelot_cls_flower_stats(ocelot, port, cls, ingress);
>  }
> diff --git a/drivers/net/dsa/ocelot/felix.h b/drivers/net/dsa/ocelot/felix.h
> index 4c717324ac2f..9ea880deb2a0 100644
> --- a/drivers/net/dsa/ocelot/felix.h
> +++ b/drivers/net/dsa/ocelot/felix.h
> @@ -37,6 +37,12 @@ struct felix_info {
>  	void	(*port_sched_speed_set)(struct ocelot *ocelot, int port,
>  					u32 speed);
>  	void	(*xmit_template_populate)(struct ocelot *ocelot, int port);
> +	int	(*flower_replace)(struct ocelot *ocelot, int port,
> +				  struct flow_cls_offload *f, bool ingress);
> +	int	(*flower_destroy)(struct ocelot *ocelot, int port,
> +				  struct flow_cls_offload *f, bool ingress);
> +	int	(*flower_stats)(struct ocelot *ocelot, int port,
> +				struct flow_cls_offload *f, bool ingress);
>  };
>  
>  extern const struct dsa_switch_ops felix_switch_ops;
> @@ -55,4 +61,14 @@ struct felix {
>  struct net_device *felix_port_to_netdev(struct ocelot *ocelot, int port);
>  int felix_netdev_to_port(struct net_device *dev);
>  
> +void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
> +			   u64 cycle_time, struct timespec64 *new_base_ts);
> +int felix_flower_stream_replace(struct ocelot *ocelot, int port,
> +				struct flow_cls_offload *f, bool ingress);
> +int felix_flower_stream_destroy(struct ocelot *ocelot, int port,
> +				struct flow_cls_offload *f, bool ingress);
> +int felix_flower_stream_stats(struct ocelot *ocelot, int port,
> +			      struct flow_cls_offload *f, bool ingress);
> +void felix_psfp_init(struct ocelot *ocelot);
> +
>  #endif
> diff --git a/drivers/net/dsa/ocelot/felix_flower.c b/drivers/net/dsa/ocelot/felix_flower.c
> new file mode 100644
> index 000000000000..71894dcc0af2
> --- /dev/null
> +++ b/drivers/net/dsa/ocelot/felix_flower.c
> @@ -0,0 +1,651 @@
> +// SPDX-License-Identifier: (GPL-2.0 OR MIT)
> +/* Copyright 2020 NXP Semiconductors
> + */
> +#include <soc/mscc/ocelot_ana.h>
> +#include <soc/mscc/ocelot_sys.h>
> +#include <net/tc_act/tc_gate.h>
> +#include <net/flow_offload.h>
> +#include <soc/mscc/ocelot.h>
> +#include <net/pkt_sched.h>
> +#include "felix.h"
> +
> +#define FELIX_PSFP_SFID_MAX		175
> +#define FELIX_PSFP_GATE_ID_MAX		183
> +#define FELIX_POLICER_PSFP_BASE		63
> +#define FELIX_POLICER_PSFP_MAX		383
> +#define FELIX_PSFP_GATE_LIST_NUM	4
> +#define FELIX_PSFP_GATE_CYCLETIME_MIN	5000
> +
> +struct felix_streamid {
> +	struct list_head list;
> +	u32 id;
> +	u8 dmac[ETH_ALEN];
> +	u16 vid;
> +	s8 prio;
> +	u8 sfid_valid;
> +	u32 sfid;
> +};
> +
> +struct felix_stream_gate_conf {
> +	u32 index;
> +	u8 enable;
> +	u8 ipv_valid;
> +	u8 init_ipv;
> +	u64 basetime;
> +	u64 cycletime;
> +	u64 cycletimext;
> +	u32 num_entries;
> +	struct action_gate_entry entries[0];
> +};
> +
> +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_gate {
> +	struct list_head list;
> +	refcount_t refcount;
> +	u32 index;
> +};
> +
> +struct felix_psfp_stream_counters {
> +	u32 match;
> +	u32 not_pass_gate;
> +	u32 not_pass_sdu;
> +	u32 red;
> +};
> +
> +struct felix_psfp_list {
> +	struct list_head stream_list;
> +	struct list_head gate_list;
> +	struct list_head sfi_list;
> +};
> +
> +static struct felix_psfp_list lpsfp;
> +
> +static u32 felix_sg_cfg_status(struct ocelot *ocelot)
> +{
> +	return ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
> +}
> +
> +static int felix_hw_sgi_set(struct ocelot *ocelot,
> +			    struct felix_stream_gate_conf *sgi)
> +{
> +	struct action_gate_entry *e;
> +	struct timespec64 base_ts;
> +	u32 interval_sum = 0;
> +	u32 val;
> +	int i;
> +
> +	if (sgi->index > FELIX_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 < FELIX_PSFP_GATE_CYCLETIME_MIN ||
> +	    sgi->cycletime > NSEC_PER_SEC)
> +		return -EINVAL;
> +
> +	if (sgi->num_entries > FELIX_PSFP_GATE_LIST_NUM)
> +		return -EINVAL;
> +
> +	vsc9959_new_base_time(ocelot, sgi->basetime, sgi->cycletime, &base_ts);

See, either you rename vsc9959_new_base_time to something more generic
(in my opinion you could even provide this functionality as part of the
tc-taprio API, as long as you replace "struct ocelot *ocelot" with
"u64 now"), or you move the entire felix_flower.c into felix_vsc9959.c,
or you rename felix_flower.c into felix_vsc9959_flower.c to indicate
that it is hardware-specific functionality and not common with seville
(which does use felix.c but would not use felix_flower.c).

> +	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(felix_sg_cfg_status, ocelot, val,
> +				  (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
> +				  10, 100000);
> +}
> +
> +static u32 felix_sfi_access_status(struct ocelot *ocelot)
> +{
> +	return ocelot_read(ocelot, ANA_TABLES_SFIDACCESS);
> +}
> +
> +static int felix_hw_sfi_set(struct ocelot *ocelot,
> +			    struct felix_stream_filter *sfi)
> +{
> +	u32 val;
> +
> +	if (sfi->index > FELIX_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(felix_sfi_access_status, ocelot, val,
> +					  (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
> +					  10, 100000);
> +	}
> +
> +	if (sfi->sgid > FELIX_PSFP_GATE_ID_MAX ||
> +	    sfi->fmid > FELIX_POLICER_PSFP_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(felix_sfi_access_status, ocelot, val,
> +				  (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
> +				  10, 100000);
> +}
> +
> +static u32 felix_mact_status(struct ocelot *ocelot)
> +{
> +	return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
> +}
> +
> +static int felix_mact_stream_update(struct ocelot *ocelot,
> +				    struct felix_streamid *stream,
> +				    struct netlink_ext_ack *extack)
> +{
> +	u32 row, col, reg, val;
> +	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).
> +	 * return -EOPNOTSUPP to continue offloading to other modules.
> +	 */
> +	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_STREAMDATA);
> +	reg &= (ANA_TABLES_STREAMDATA_SFID_VALID | ANA_TABLES_STREAMDATA_SSID_VALID);
> +	type = (reg ? ENTRYTYPE_LOCKED : ENTRYTYPE_NORMAL);
> +	ocelot_rmw(ocelot,  ANA_TABLES_MACACCESS_VALID |
> +		   ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
> +		   ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
> +		   ANA_TABLES_MACACCESS_VALID |
> +		   ANA_TABLES_MACACCESS_ENTRYTYPE_M |
> +		   ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M,
> +		   ANA_TABLES_MACACCESS);
> +
> +	return readx_poll_timeout(felix_mact_status, ocelot, val,
> +				  (!ANA_TABLES_MACACCESS_MAC_TABLE_CMD(val)),
> +				  10, 100000);
> +}
> +
> +static void felix_stream_counters_get(struct ocelot *ocelot, u32 index,
> +				      struct felix_psfp_stream_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);
> +}
> +
> +static int felix_list_gate_add(struct ocelot *ocelot,
> +			       struct felix_stream_gate_conf *sgi)
> +{
> +	struct felix_stream_gate *gate, *tmp;
> +	struct list_head *pos, *q;
> +	int ret;
> +
> +	list_for_each_safe(pos, q, &lpsfp.gate_list) {
> +		tmp = list_entry(pos, struct felix_stream_gate, list);
> +		if (tmp->index == sgi->index) {
> +			refcount_inc(&tmp->refcount);
> +			return 0;
> +		}
> +		if (tmp->index > sgi->index)
> +			break;
> +	}
> +
> +	ret = felix_hw_sgi_set(ocelot, sgi);
> +	if (ret)
> +		return ret;
> +
> +	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
> +	if (!gate)
> +		return -ENOMEM;
> +
> +	gate->index = sgi->index;
> +	refcount_set(&gate->refcount, 1);
> +	list_add(&gate->list, pos->prev);
> +
> +	return 0;
> +}
> +
> +static void felix_list_gate_del(struct ocelot *ocelot, u32 index)
> +{
> +	struct felix_stream_gate *tmp;
> +	struct felix_stream_gate_conf sgi;
> +	struct list_head *pos, *q;
> +	u8 z;
> +
> +	list_for_each_safe(pos, q, &lpsfp.gate_list) {
> +		tmp = list_entry(pos, struct felix_stream_gate, list);
> +		if (tmp->index == index) {
> +			z = refcount_dec_and_test(&tmp->refcount);
> +			if (z) {
> +				sgi.index = index;
> +				sgi.enable = 0;
> +				felix_hw_sgi_set(ocelot, &sgi);
> +				list_del(pos);
> +				kfree(tmp);
> +			}
> +			break;
> +		}
> +	}
> +}
> +
> +static int felix_list_stream_filter_add(struct ocelot *ocelot,
> +					struct felix_stream_filter *sfi)
> +{
> +	struct felix_stream_filter *sfi_entry, *tmp;
> +	struct list_head *last = &lpsfp.sfi_list;
> +	struct list_head *pos, *q;
> +	u32 insert = 0;
> +	int ret;
> +
> +	list_for_each_safe(pos, q, &lpsfp.sfi_list) {
> +		tmp = list_entry(pos, struct felix_stream_filter, list);
> +		if (sfi->sg_valid == tmp->sg_valid &&
> +		    tmp->sgid == sfi->sgid &&
> +		    tmp->fmid == sfi->fmid) {
> +			sfi->index = tmp->index;
> +			refcount_inc(&tmp->refcount);
> +			return 0;
> +		}
> +		if (tmp->index == insert) {
> +			last = pos;
> +			insert++;
> +		}
> +	}
> +	sfi->index = insert;
> +	ret = felix_hw_sfi_set(ocelot, sfi);
> +	if (ret)
> +		return ret;
> +
> +	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);
> +
> +	list_add(&sfi_entry->list, last->next);
> +
> +	return 0;
> +}
> +
> +static void felix_list_stream_filter_del(struct ocelot *ocelot, u32 index)
> +{
> +	struct felix_stream_filter *tmp;
> +	struct list_head *pos, *q;
> +	u8 z;
> +
> +	list_for_each_safe(pos, q, &lpsfp.sfi_list) {
> +		tmp = list_entry(pos, struct felix_stream_filter, list);
> +		if (tmp->index == index) {
> +			if (tmp->sg_valid)
> +				felix_list_gate_del(ocelot, tmp->sgid);
> +
> +			z = refcount_dec_and_test(&tmp->refcount);
> +			if (z) {
> +				tmp->enable = 0;
> +				felix_hw_sfi_set(ocelot, tmp);
> +				list_del(pos);
> +				kfree(tmp);
> +			}
> +			break;
> +		}
> +	}
> +}
> +
> +static int felix_list_stream_add(struct felix_streamid *stream)
> +{
> +	struct felix_streamid *stream_entry;
> +	struct list_head *pos;
> +
> +	stream_entry = kzalloc(sizeof(*stream_entry), GFP_KERNEL);
> +	if (!stream_entry)
> +		return -ENOMEM;
> +
> +	memcpy(stream_entry, stream, sizeof(*stream_entry));
> +
> +	if (list_empty(&lpsfp.stream_list)) {
> +		list_add(&stream_entry->list, &lpsfp.stream_list);
> +		return 0;
> +	}
> +
> +	pos = &lpsfp.stream_list;
> +	list_add(&stream_entry->list, pos->prev);
> +
> +	return 0;
> +}
> +
> +static int felix_list_stream_lookup(struct felix_streamid *stream)
> +{
> +	struct felix_streamid *tmp;
> +
> +	list_for_each_entry(tmp, &lpsfp.stream_list, list) {
> +		if (tmp->dmac[0] == stream->dmac[0] &&
> +		    tmp->dmac[1] == stream->dmac[1] &&
> +		    tmp->dmac[2] == stream->dmac[2] &&
> +		    tmp->dmac[3] == stream->dmac[3] &&
> +		    tmp->dmac[4] == stream->dmac[4] &&
> +		    tmp->dmac[5] == stream->dmac[5] &&

ether_addr_equal

Did you even read the internal review comments?

> +		    tmp->vid == stream->vid &&
> +		    (tmp->sfid_valid & stream->sfid_valid))
> +			return 0;
> +	}
> +
> +	return -ENOENT;
> +}
> +
> +static struct felix_streamid *felix_list_stream_get(u32 id)
> +{
> +	struct felix_streamid *tmp;
> +	struct list_head *pos, *q;
> +
> +	list_for_each_safe(pos, q, &lpsfp.stream_list) {
> +		tmp = list_entry(pos, struct felix_streamid, list);

list_for_each_entry

Please revise the way you use the kernel lists API, I won't go over the
whole file with these comments because it would pollute the entire
discussion.

> +		if (tmp->id == id)
> +			return tmp;
> +	}
> +
> +	return NULL;
> +}
> +
> +static int felix_list_stream_del(struct ocelot *ocelot, u32 id)
> +{
> +	struct felix_streamid *tmp;
> +	struct list_head *pos, *q;
> +
> +	list_for_each_safe(pos, q, &lpsfp.stream_list) {
> +		tmp = list_entry(pos, struct felix_streamid, list);
> +		if (tmp->id == id) {
> +			tmp->sfid_valid = 0;
> +			felix_list_stream_filter_del(ocelot, tmp->sfid);
> +			felix_mact_stream_update(ocelot, tmp, NULL);
> +			list_del(pos);
> +			kfree(tmp);
> +
> +			return 0;
> +		}
> +	}
> +
> +	return -ENOENT;
> +}
> +
> +static int felix_psfp_set(struct ocelot *ocelot,
> +			  struct felix_streamid *stream,
> +			  struct felix_stream_filter *sfi,
> +			  struct netlink_ext_ack *extack)
> +{
> +	int ret;
> +
> +	sfi->prio_valid = (stream->prio < 0 ? 0 : 1);
> +	sfi->prio = (sfi->prio_valid ? stream->prio : 0);
> +	sfi->enable = 1;
> +	ret = felix_list_stream_filter_add(ocelot, sfi);
> +	if (ret) {
> +		if (sfi->sg_valid)
> +			felix_list_gate_del(ocelot, sfi->sgid);
> +		return ret;
> +	}
> +
> +	stream->sfid = sfi->index;
> +	ret = felix_mact_stream_update(ocelot, stream, extack);
> +	if (ret) {
> +		felix_list_stream_filter_del(ocelot, sfi->index);
> +		return ret;
> +	}
> +
> +	ret = felix_list_stream_add(stream);
> +	if (ret)
> +		felix_list_stream_filter_del(ocelot, sfi->index);
> +
> +	return ret;
> +}
> +
> +static void felix_parse_gate(const struct flow_action_entry *entry,
> +			     struct felix_stream_gate_conf *sgi)
> +{
> +	struct action_gate_entry *e;
> +	int i;
> +
> +	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;
> +
> +	e = sgi->entries;
> +	for (i = 0; i < entry->gate.num_entries; i++) {
> +		e[i].gate_state = entry->gate.entries[i].gate_state;
> +		e[i].interval = entry->gate.entries[i].interval;
> +		e[i].ipv = entry->gate.entries[i].ipv;
> +		e[i].maxoctets = entry->gate.entries[i].maxoctets;
> +	}
> +}
> +
> +static int felix_flower_parse_key(struct flow_cls_offload *f,
> +				  struct felix_streamid *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;
> +}
> +
> +int felix_flower_stream_replace(struct ocelot *ocelot, int port,
> +				struct flow_cls_offload *f, bool ingress)
> +{
> +	struct netlink_ext_ack *extack = f->common.extack;
> +	struct felix_stream_filter sfi = {0};
> +	struct felix_streamid stream = {0};
> +	struct felix_stream_gate_conf *sgi;
> +	const struct flow_action_entry *a;
> +	int ret, size, i;
> +	u32 index;
> +
> +	ret = felix_flower_parse_key(f, &stream);
> +	if (ret) {
> +		NL_SET_ERR_MSG_MOD(extack, "Only can match on VID, PCP, and dest MAC");
> +		return ret;
> +	}
> +
> +	if (!flow_action_basic_hw_stats_check(&f->rule->action,
> +					      f->common.extack))
> +		return -EOPNOTSUPP;
> +
> +	flow_action_for_each(i, a, &f->rule->action) {
> +		switch (a->id) {
> +		case FLOW_ACTION_GATE:
> +			if (f->common.chain_index != OCELOT_PSFP_CHAIN) {
> +				NL_SET_ERR_MSG_MOD(extack,
> +						   "Gate action only be offloaded to PSFP chain");
> +				return -EOPNOTSUPP;
> +			}
> +
> +			size = struct_size(sgi, entries, a->gate.num_entries);
> +			sgi = kzalloc(size, GFP_KERNEL);
> +			felix_parse_gate(a, sgi);
> +			ret = felix_list_gate_add(ocelot, sgi);
> +			if (ret) {
> +				kfree(sgi);
> +				return ret;
> +			}
> +
> +			sfi.sg_valid = 1;
> +			sfi.sgid = sgi->index;
> +			stream.sfid_valid = 1;
> +			kfree(sgi);
> +			break;
> +		default:
> +			return -EOPNOTSUPP;
> +		}
> +	}
> +
> +	/* Check if stream is set. */
> +	ret = felix_list_stream_lookup(&stream);
> +	if (!ret) {
> +		if (sfi.sg_valid)
> +			felix_list_gate_del(ocelot, sfi.sgid);
> +
> +		NL_SET_ERR_MSG_MOD(extack, "This stream is already added");
> +
> +		return -EEXIST;
> +	}
> +
> +	if (stream.sfid_valid)
> +		return felix_psfp_set(ocelot, &stream, &sfi, extack);
> +
> +	return -EOPNOTSUPP;
> +}
> +
> +int felix_flower_stream_destroy(struct ocelot *ocelot, int port,
> +				struct flow_cls_offload *f, bool ingress)
> +{
> +	return felix_list_stream_del(ocelot, f->cookie);
> +}
> +
> +int felix_flower_stream_stats(struct ocelot *ocelot, int port,
> +			      struct flow_cls_offload *f, bool ingress)
> +{
> +	struct felix_psfp_stream_counters counters;
> +	struct felix_streamid *stream;
> +	struct flow_stats stats;
> +
> +	stream = felix_list_stream_get(f->cookie);
> +	if (!stream)
> +		return -ENOENT;
> +
> +	felix_stream_counters_get(ocelot, stream->sfid, &counters);
> +	stats.pkts = counters.match;
> +
> +	flow_stats_update(&f->stats, 0x0, stats.pkts, 0x0, 0,
> +			  FLOW_ACTION_HW_STATS_IMMEDIATE);
> +
> +	return 0;
> +}
> +
> +void felix_psfp_init(struct ocelot *ocelot)
> +{
> +	INIT_LIST_HEAD(&lpsfp.stream_list);
> +	INIT_LIST_HEAD(&lpsfp.gate_list);
> +	INIT_LIST_HEAD(&lpsfp.sfi_list);
> +}
> diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
> index 3e925b8d5306..f171e6f3fc98 100644
> --- a/drivers/net/dsa/ocelot/felix_vsc9959.c
> +++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
> @@ -931,6 +931,8 @@ static int vsc9959_reset(struct ocelot *ocelot)
>  	/* enable switch core */
>  	ocelot_field_write(ocelot, SYS_RESET_CFG_CORE_ENA, 1);
>  
> +	felix_psfp_init(ocelot);
> +
>  	return 0;
>  }
>  
> @@ -1150,9 +1152,8 @@ static void vsc9959_sched_speed_set(struct ocelot *ocelot, int port,
>  		       QSYS_TAG_CONFIG, port);
>  }
>  
> -static void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
> -				  u64 cycle_time,
> -				  struct timespec64 *new_base_ts)
> +void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
> +			   u64 cycle_time, struct timespec64 *new_base_ts)
>  {
>  	struct timespec64 ts;
>  	ktime_t new_base_time;
> @@ -1370,6 +1371,9 @@ static const struct felix_info felix_info_vsc9959 = {
>  	.port_setup_tc		= vsc9959_port_setup_tc,
>  	.port_sched_speed_set	= vsc9959_sched_speed_set,
>  	.xmit_template_populate	= vsc9959_xmit_template_populate,
> +	.flower_replace		= felix_flower_stream_replace,
> +	.flower_destroy		= felix_flower_stream_destroy,
> +	.flower_stats		= felix_flower_stream_stats,
>  };
>  
>  static irqreturn_t felix_irq_handler(int irq, void *data)
> diff --git a/include/soc/mscc/ocelot_ana.h b/include/soc/mscc/ocelot_ana.h
> index 1669481d9779..c5a0b7174518 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
> +

Again, do you even read review comments?

>  #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
>

Patch

diff --git a/drivers/net/dsa/ocelot/Makefile b/drivers/net/dsa/ocelot/Makefile
index f6dd131e7491..22f3f98914e3 100644
--- a/drivers/net/dsa/ocelot/Makefile
+++ b/drivers/net/dsa/ocelot/Makefile
@@ -4,7 +4,8 @@  obj-$(CONFIG_NET_DSA_MSCC_SEVILLE) += mscc_seville.o
 
 mscc_felix-objs := \
 	felix.o \
-	felix_vsc9959.o
+	felix_vsc9959.o \
+	felix_flower.o
 
 mscc_seville-objs := \
 	felix.o \
diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index f791860d495f..42f972d10539 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -716,6 +716,13 @@  static int felix_cls_flower_add(struct dsa_switch *ds, int port,
 				struct flow_cls_offload *cls, bool ingress)
 {
 	struct ocelot *ocelot = ds->priv;
+	struct felix *felix = ocelot_to_felix(ocelot);
+
+	if (felix->info->flower_replace) {
+		if (cls->common.chain_index == OCELOT_PSFP_CHAIN)
+			return felix->info->flower_replace(ocelot, port, cls,
+							   ingress);
+	}
 
 	return ocelot_cls_flower_replace(ocelot, port, cls, ingress);
 }
@@ -724,6 +731,14 @@  static int felix_cls_flower_del(struct dsa_switch *ds, int port,
 				struct flow_cls_offload *cls, bool ingress)
 {
 	struct ocelot *ocelot = ds->priv;
+	struct felix *felix = ocelot_to_felix(ocelot);
+	int ret;
+
+	if (felix->info->flower_destroy) {
+		ret = felix->info->flower_destroy(ocelot, port, cls, ingress);
+		if (!ret)
+			return 0;
+	}
 
 	return ocelot_cls_flower_destroy(ocelot, port, cls, ingress);
 }
@@ -732,6 +747,14 @@  static int felix_cls_flower_stats(struct dsa_switch *ds, int port,
 				  struct flow_cls_offload *cls, bool ingress)
 {
 	struct ocelot *ocelot = ds->priv;
+	struct felix *felix = ocelot_to_felix(ocelot);
+	int ret;
+
+	if (felix->info->flower_stats) {
+		ret = felix->info->flower_stats(ocelot, port, cls, ingress);
+		if (!ret)
+			return 0;
+	}
 
 	return ocelot_cls_flower_stats(ocelot, port, cls, ingress);
 }
diff --git a/drivers/net/dsa/ocelot/felix.h b/drivers/net/dsa/ocelot/felix.h
index 4c717324ac2f..9ea880deb2a0 100644
--- a/drivers/net/dsa/ocelot/felix.h
+++ b/drivers/net/dsa/ocelot/felix.h
@@ -37,6 +37,12 @@  struct felix_info {
 	void	(*port_sched_speed_set)(struct ocelot *ocelot, int port,
 					u32 speed);
 	void	(*xmit_template_populate)(struct ocelot *ocelot, int port);
+	int	(*flower_replace)(struct ocelot *ocelot, int port,
+				  struct flow_cls_offload *f, bool ingress);
+	int	(*flower_destroy)(struct ocelot *ocelot, int port,
+				  struct flow_cls_offload *f, bool ingress);
+	int	(*flower_stats)(struct ocelot *ocelot, int port,
+				struct flow_cls_offload *f, bool ingress);
 };
 
 extern const struct dsa_switch_ops felix_switch_ops;
@@ -55,4 +61,14 @@  struct felix {
 struct net_device *felix_port_to_netdev(struct ocelot *ocelot, int port);
 int felix_netdev_to_port(struct net_device *dev);
 
+void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
+			   u64 cycle_time, struct timespec64 *new_base_ts);
+int felix_flower_stream_replace(struct ocelot *ocelot, int port,
+				struct flow_cls_offload *f, bool ingress);
+int felix_flower_stream_destroy(struct ocelot *ocelot, int port,
+				struct flow_cls_offload *f, bool ingress);
+int felix_flower_stream_stats(struct ocelot *ocelot, int port,
+			      struct flow_cls_offload *f, bool ingress);
+void felix_psfp_init(struct ocelot *ocelot);
+
 #endif
diff --git a/drivers/net/dsa/ocelot/felix_flower.c b/drivers/net/dsa/ocelot/felix_flower.c
new file mode 100644
index 000000000000..71894dcc0af2
--- /dev/null
+++ b/drivers/net/dsa/ocelot/felix_flower.c
@@ -0,0 +1,651 @@ 
+// SPDX-License-Identifier: (GPL-2.0 OR MIT)
+/* Copyright 2020 NXP Semiconductors
+ */
+#include <soc/mscc/ocelot_ana.h>
+#include <soc/mscc/ocelot_sys.h>
+#include <net/tc_act/tc_gate.h>
+#include <net/flow_offload.h>
+#include <soc/mscc/ocelot.h>
+#include <net/pkt_sched.h>
+#include "felix.h"
+
+#define FELIX_PSFP_SFID_MAX		175
+#define FELIX_PSFP_GATE_ID_MAX		183
+#define FELIX_POLICER_PSFP_BASE		63
+#define FELIX_POLICER_PSFP_MAX		383
+#define FELIX_PSFP_GATE_LIST_NUM	4
+#define FELIX_PSFP_GATE_CYCLETIME_MIN	5000
+
+struct felix_streamid {
+	struct list_head list;
+	u32 id;
+	u8 dmac[ETH_ALEN];
+	u16 vid;
+	s8 prio;
+	u8 sfid_valid;
+	u32 sfid;
+};
+
+struct felix_stream_gate_conf {
+	u32 index;
+	u8 enable;
+	u8 ipv_valid;
+	u8 init_ipv;
+	u64 basetime;
+	u64 cycletime;
+	u64 cycletimext;
+	u32 num_entries;
+	struct action_gate_entry entries[0];
+};
+
+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_gate {
+	struct list_head list;
+	refcount_t refcount;
+	u32 index;
+};
+
+struct felix_psfp_stream_counters {
+	u32 match;
+	u32 not_pass_gate;
+	u32 not_pass_sdu;
+	u32 red;
+};
+
+struct felix_psfp_list {
+	struct list_head stream_list;
+	struct list_head gate_list;
+	struct list_head sfi_list;
+};
+
+static struct felix_psfp_list lpsfp;
+
+static u32 felix_sg_cfg_status(struct ocelot *ocelot)
+{
+	return ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
+}
+
+static int felix_hw_sgi_set(struct ocelot *ocelot,
+			    struct felix_stream_gate_conf *sgi)
+{
+	struct action_gate_entry *e;
+	struct timespec64 base_ts;
+	u32 interval_sum = 0;
+	u32 val;
+	int i;
+
+	if (sgi->index > FELIX_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 < FELIX_PSFP_GATE_CYCLETIME_MIN ||
+	    sgi->cycletime > NSEC_PER_SEC)
+		return -EINVAL;
+
+	if (sgi->num_entries > FELIX_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(felix_sg_cfg_status, ocelot, val,
+				  (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
+				  10, 100000);
+}
+
+static u32 felix_sfi_access_status(struct ocelot *ocelot)
+{
+	return ocelot_read(ocelot, ANA_TABLES_SFIDACCESS);
+}
+
+static int felix_hw_sfi_set(struct ocelot *ocelot,
+			    struct felix_stream_filter *sfi)
+{
+	u32 val;
+
+	if (sfi->index > FELIX_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(felix_sfi_access_status, ocelot, val,
+					  (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
+					  10, 100000);
+	}
+
+	if (sfi->sgid > FELIX_PSFP_GATE_ID_MAX ||
+	    sfi->fmid > FELIX_POLICER_PSFP_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(felix_sfi_access_status, ocelot, val,
+				  (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
+				  10, 100000);
+}
+
+static u32 felix_mact_status(struct ocelot *ocelot)
+{
+	return ocelot_read(ocelot, ANA_TABLES_MACACCESS);
+}
+
+static int felix_mact_stream_update(struct ocelot *ocelot,
+				    struct felix_streamid *stream,
+				    struct netlink_ext_ack *extack)
+{
+	u32 row, col, reg, val;
+	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).
+	 * return -EOPNOTSUPP to continue offloading to other modules.
+	 */
+	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_STREAMDATA);
+	reg &= (ANA_TABLES_STREAMDATA_SFID_VALID | ANA_TABLES_STREAMDATA_SSID_VALID);
+	type = (reg ? ENTRYTYPE_LOCKED : ENTRYTYPE_NORMAL);
+	ocelot_rmw(ocelot,  ANA_TABLES_MACACCESS_VALID |
+		   ANA_TABLES_MACACCESS_ENTRYTYPE(type) |
+		   ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
+		   ANA_TABLES_MACACCESS_VALID |
+		   ANA_TABLES_MACACCESS_ENTRYTYPE_M |
+		   ANA_TABLES_MACACCESS_MAC_TABLE_CMD_M,
+		   ANA_TABLES_MACACCESS);
+
+	return readx_poll_timeout(felix_mact_status, ocelot, val,
+				  (!ANA_TABLES_MACACCESS_MAC_TABLE_CMD(val)),
+				  10, 100000);
+}
+
+static void felix_stream_counters_get(struct ocelot *ocelot, u32 index,
+				      struct felix_psfp_stream_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);
+}
+
+static int felix_list_gate_add(struct ocelot *ocelot,
+			       struct felix_stream_gate_conf *sgi)
+{
+	struct felix_stream_gate *gate, *tmp;
+	struct list_head *pos, *q;
+	int ret;
+
+	list_for_each_safe(pos, q, &lpsfp.gate_list) {
+		tmp = list_entry(pos, struct felix_stream_gate, list);
+		if (tmp->index == sgi->index) {
+			refcount_inc(&tmp->refcount);
+			return 0;
+		}
+		if (tmp->index > sgi->index)
+			break;
+	}
+
+	ret = felix_hw_sgi_set(ocelot, sgi);
+	if (ret)
+		return ret;
+
+	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+	if (!gate)
+		return -ENOMEM;
+
+	gate->index = sgi->index;
+	refcount_set(&gate->refcount, 1);
+	list_add(&gate->list, pos->prev);
+
+	return 0;
+}
+
+static void felix_list_gate_del(struct ocelot *ocelot, u32 index)
+{
+	struct felix_stream_gate *tmp;
+	struct felix_stream_gate_conf sgi;
+	struct list_head *pos, *q;
+	u8 z;
+
+	list_for_each_safe(pos, q, &lpsfp.gate_list) {
+		tmp = list_entry(pos, struct felix_stream_gate, list);
+		if (tmp->index == index) {
+			z = refcount_dec_and_test(&tmp->refcount);
+			if (z) {
+				sgi.index = index;
+				sgi.enable = 0;
+				felix_hw_sgi_set(ocelot, &sgi);
+				list_del(pos);
+				kfree(tmp);
+			}
+			break;
+		}
+	}
+}
+
+static int felix_list_stream_filter_add(struct ocelot *ocelot,
+					struct felix_stream_filter *sfi)
+{
+	struct felix_stream_filter *sfi_entry, *tmp;
+	struct list_head *last = &lpsfp.sfi_list;
+	struct list_head *pos, *q;
+	u32 insert = 0;
+	int ret;
+
+	list_for_each_safe(pos, q, &lpsfp.sfi_list) {
+		tmp = list_entry(pos, struct felix_stream_filter, list);
+		if (sfi->sg_valid == tmp->sg_valid &&
+		    tmp->sgid == sfi->sgid &&
+		    tmp->fmid == sfi->fmid) {
+			sfi->index = tmp->index;
+			refcount_inc(&tmp->refcount);
+			return 0;
+		}
+		if (tmp->index == insert) {
+			last = pos;
+			insert++;
+		}
+	}
+	sfi->index = insert;
+	ret = felix_hw_sfi_set(ocelot, sfi);
+	if (ret)
+		return ret;
+
+	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);
+
+	list_add(&sfi_entry->list, last->next);
+
+	return 0;
+}
+
+static void felix_list_stream_filter_del(struct ocelot *ocelot, u32 index)
+{
+	struct felix_stream_filter *tmp;
+	struct list_head *pos, *q;
+	u8 z;
+
+	list_for_each_safe(pos, q, &lpsfp.sfi_list) {
+		tmp = list_entry(pos, struct felix_stream_filter, list);
+		if (tmp->index == index) {
+			if (tmp->sg_valid)
+				felix_list_gate_del(ocelot, tmp->sgid);
+
+			z = refcount_dec_and_test(&tmp->refcount);
+			if (z) {
+				tmp->enable = 0;
+				felix_hw_sfi_set(ocelot, tmp);
+				list_del(pos);
+				kfree(tmp);
+			}
+			break;
+		}
+	}
+}
+
+static int felix_list_stream_add(struct felix_streamid *stream)
+{
+	struct felix_streamid *stream_entry;
+	struct list_head *pos;
+
+	stream_entry = kzalloc(sizeof(*stream_entry), GFP_KERNEL);
+	if (!stream_entry)
+		return -ENOMEM;
+
+	memcpy(stream_entry, stream, sizeof(*stream_entry));
+
+	if (list_empty(&lpsfp.stream_list)) {
+		list_add(&stream_entry->list, &lpsfp.stream_list);
+		return 0;
+	}
+
+	pos = &lpsfp.stream_list;
+	list_add(&stream_entry->list, pos->prev);
+
+	return 0;
+}
+
+static int felix_list_stream_lookup(struct felix_streamid *stream)
+{
+	struct felix_streamid *tmp;
+
+	list_for_each_entry(tmp, &lpsfp.stream_list, list) {
+		if (tmp->dmac[0] == stream->dmac[0] &&
+		    tmp->dmac[1] == stream->dmac[1] &&
+		    tmp->dmac[2] == stream->dmac[2] &&
+		    tmp->dmac[3] == stream->dmac[3] &&
+		    tmp->dmac[4] == stream->dmac[4] &&
+		    tmp->dmac[5] == stream->dmac[5] &&
+		    tmp->vid == stream->vid &&
+		    (tmp->sfid_valid & stream->sfid_valid))
+			return 0;
+	}
+
+	return -ENOENT;
+}
+
+static struct felix_streamid *felix_list_stream_get(u32 id)
+{
+	struct felix_streamid *tmp;
+	struct list_head *pos, *q;
+
+	list_for_each_safe(pos, q, &lpsfp.stream_list) {
+		tmp = list_entry(pos, struct felix_streamid, list);
+		if (tmp->id == id)
+			return tmp;
+	}
+
+	return NULL;
+}
+
+static int felix_list_stream_del(struct ocelot *ocelot, u32 id)
+{
+	struct felix_streamid *tmp;
+	struct list_head *pos, *q;
+
+	list_for_each_safe(pos, q, &lpsfp.stream_list) {
+		tmp = list_entry(pos, struct felix_streamid, list);
+		if (tmp->id == id) {
+			tmp->sfid_valid = 0;
+			felix_list_stream_filter_del(ocelot, tmp->sfid);
+			felix_mact_stream_update(ocelot, tmp, NULL);
+			list_del(pos);
+			kfree(tmp);
+
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
+
+static int felix_psfp_set(struct ocelot *ocelot,
+			  struct felix_streamid *stream,
+			  struct felix_stream_filter *sfi,
+			  struct netlink_ext_ack *extack)
+{
+	int ret;
+
+	sfi->prio_valid = (stream->prio < 0 ? 0 : 1);
+	sfi->prio = (sfi->prio_valid ? stream->prio : 0);
+	sfi->enable = 1;
+	ret = felix_list_stream_filter_add(ocelot, sfi);
+	if (ret) {
+		if (sfi->sg_valid)
+			felix_list_gate_del(ocelot, sfi->sgid);
+		return ret;
+	}
+
+	stream->sfid = sfi->index;
+	ret = felix_mact_stream_update(ocelot, stream, extack);
+	if (ret) {
+		felix_list_stream_filter_del(ocelot, sfi->index);
+		return ret;
+	}
+
+	ret = felix_list_stream_add(stream);
+	if (ret)
+		felix_list_stream_filter_del(ocelot, sfi->index);
+
+	return ret;
+}
+
+static void felix_parse_gate(const struct flow_action_entry *entry,
+			     struct felix_stream_gate_conf *sgi)
+{
+	struct action_gate_entry *e;
+	int i;
+
+	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;
+
+	e = sgi->entries;
+	for (i = 0; i < entry->gate.num_entries; i++) {
+		e[i].gate_state = entry->gate.entries[i].gate_state;
+		e[i].interval = entry->gate.entries[i].interval;
+		e[i].ipv = entry->gate.entries[i].ipv;
+		e[i].maxoctets = entry->gate.entries[i].maxoctets;
+	}
+}
+
+static int felix_flower_parse_key(struct flow_cls_offload *f,
+				  struct felix_streamid *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;
+}
+
+int felix_flower_stream_replace(struct ocelot *ocelot, int port,
+				struct flow_cls_offload *f, bool ingress)
+{
+	struct netlink_ext_ack *extack = f->common.extack;
+	struct felix_stream_filter sfi = {0};
+	struct felix_streamid stream = {0};
+	struct felix_stream_gate_conf *sgi;
+	const struct flow_action_entry *a;
+	int ret, size, i;
+	u32 index;
+
+	ret = felix_flower_parse_key(f, &stream);
+	if (ret) {
+		NL_SET_ERR_MSG_MOD(extack, "Only can match on VID, PCP, and dest MAC");
+		return ret;
+	}
+
+	if (!flow_action_basic_hw_stats_check(&f->rule->action,
+					      f->common.extack))
+		return -EOPNOTSUPP;
+
+	flow_action_for_each(i, a, &f->rule->action) {
+		switch (a->id) {
+		case FLOW_ACTION_GATE:
+			if (f->common.chain_index != OCELOT_PSFP_CHAIN) {
+				NL_SET_ERR_MSG_MOD(extack,
+						   "Gate action only be offloaded to PSFP chain");
+				return -EOPNOTSUPP;
+			}
+
+			size = struct_size(sgi, entries, a->gate.num_entries);
+			sgi = kzalloc(size, GFP_KERNEL);
+			felix_parse_gate(a, sgi);
+			ret = felix_list_gate_add(ocelot, sgi);
+			if (ret) {
+				kfree(sgi);
+				return ret;
+			}
+
+			sfi.sg_valid = 1;
+			sfi.sgid = sgi->index;
+			stream.sfid_valid = 1;
+			kfree(sgi);
+			break;
+		default:
+			return -EOPNOTSUPP;
+		}
+	}
+
+	/* Check if stream is set. */
+	ret = felix_list_stream_lookup(&stream);
+	if (!ret) {
+		if (sfi.sg_valid)
+			felix_list_gate_del(ocelot, sfi.sgid);
+
+		NL_SET_ERR_MSG_MOD(extack, "This stream is already added");
+
+		return -EEXIST;
+	}
+
+	if (stream.sfid_valid)
+		return felix_psfp_set(ocelot, &stream, &sfi, extack);
+
+	return -EOPNOTSUPP;
+}
+
+int felix_flower_stream_destroy(struct ocelot *ocelot, int port,
+				struct flow_cls_offload *f, bool ingress)
+{
+	return felix_list_stream_del(ocelot, f->cookie);
+}
+
+int felix_flower_stream_stats(struct ocelot *ocelot, int port,
+			      struct flow_cls_offload *f, bool ingress)
+{
+	struct felix_psfp_stream_counters counters;
+	struct felix_streamid *stream;
+	struct flow_stats stats;
+
+	stream = felix_list_stream_get(f->cookie);
+	if (!stream)
+		return -ENOENT;
+
+	felix_stream_counters_get(ocelot, stream->sfid, &counters);
+	stats.pkts = counters.match;
+
+	flow_stats_update(&f->stats, 0x0, stats.pkts, 0x0, 0,
+			  FLOW_ACTION_HW_STATS_IMMEDIATE);
+
+	return 0;
+}
+
+void felix_psfp_init(struct ocelot *ocelot)
+{
+	INIT_LIST_HEAD(&lpsfp.stream_list);
+	INIT_LIST_HEAD(&lpsfp.gate_list);
+	INIT_LIST_HEAD(&lpsfp.sfi_list);
+}
diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index 3e925b8d5306..f171e6f3fc98 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -931,6 +931,8 @@  static int vsc9959_reset(struct ocelot *ocelot)
 	/* enable switch core */
 	ocelot_field_write(ocelot, SYS_RESET_CFG_CORE_ENA, 1);
 
+	felix_psfp_init(ocelot);
+
 	return 0;
 }
 
@@ -1150,9 +1152,8 @@  static void vsc9959_sched_speed_set(struct ocelot *ocelot, int port,
 		       QSYS_TAG_CONFIG, port);
 }
 
-static void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
-				  u64 cycle_time,
-				  struct timespec64 *new_base_ts)
+void vsc9959_new_base_time(struct ocelot *ocelot, ktime_t base_time,
+			   u64 cycle_time, struct timespec64 *new_base_ts)
 {
 	struct timespec64 ts;
 	ktime_t new_base_time;
@@ -1370,6 +1371,9 @@  static const struct felix_info felix_info_vsc9959 = {
 	.port_setup_tc		= vsc9959_port_setup_tc,
 	.port_sched_speed_set	= vsc9959_sched_speed_set,
 	.xmit_template_populate	= vsc9959_xmit_template_populate,
+	.flower_replace		= felix_flower_stream_replace,
+	.flower_destroy		= felix_flower_stream_destroy,
+	.flower_stats		= felix_flower_stream_stats,
 };
 
 static irqreturn_t felix_irq_handler(int irq, void *data)
diff --git a/include/soc/mscc/ocelot_ana.h b/include/soc/mscc/ocelot_ana.h
index 1669481d9779..c5a0b7174518 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