Merge branch 'sparx5-PSFP-support'

Daniel Machon says:

====================
net: Add support for PSFP in Sparx5

================================================================================
Add support for Per-Stream Filtering and Policing (802.1Q-2018, 8.6.5.1).
================================================================================

The VCAP CLM (VCAP IS0 ingress classifier) classifies streams,
identified by ISDX (Ingress Service Index, frame metadata), and maps
ISDX to streams.

Flow meters are also classified by ISDX, and implemented using service
policers (Service Dual Leacky Buckets, SDLB). Leacky buckets are linked
together in a leak chain of a leak group. Leak groups a preconfigured to serve
buckets within a certain rate interval.

Stream gates are time-based policers used by PSFP. Frames are dropped
based on the gate state (OPEN/ CLOSE), whose state will be altered based
on the Gate Control List (GCL) and current PTP time. Apart from
time-based policing, stream gates can alter egress queue selection for
the frames that pass through the Gate. This is done through Internal
Priority Selector (IPS). Stream gates are mapped from stream filters.

Support for tc actions gate and police, have been added to the VCAP IS0 set of
supported actions.

Examples:

// tc filter with gate action
$ tc filter add dev eth1 ingress chain 1100000 prio 1 handle 1001 protocol \
802.1q flower skip_sw vlan_id 100 action gate base-time 0 sched-entry open \
700000 7 8m sched-entry close 300000 action goto chain 1200000

// tc filter with police action
$ tc filter add dev eth1 ingress chain 1100000 prio 1 handle 1002 protocol \
802.1q flower skip_sw vlan_id 100 action police rate 1gbit burst 8096      \
conform-exceed drop action goto chain 1200000

================================================================================
Patches
================================================================================
Patch #1:  Adds new register needed for PSFP.
Patch #2:  Adds resource pools to control PSFP needed chip resources.
Patch #3:  Adds support for SDLB's needed for flow-meters.
Patch #4:  Adds support for service policers.
Patch #5:  Adds support for PSFP flow-meters, using service policers.
Patch #6:  Adds a new function to calculate basetime, required by flow-meters.
Patch #7:  Adds support for PSFP stream gates.
Patch #8:  Adds support for PSFP stream filters.
Patch #9:  Adds a function to initialize flow-meters, stream gates and stream
           filters.
Patch #10: Adds the required flower code to configure PSFP using the tc command.
====================
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/ethernet/microchip/sparx5/Makefile

@@ -9,7 +9,8 @@ sparx5-switch-y  := sparx5_main.o sparx5_packet.o \
  sparx5_netdev.o sparx5_phylink.o sparx5_port.o sparx5_mactable.o sparx5_vlan.o \
  sparx5_switchdev.o sparx5_calendar.o sparx5_ethtool.o sparx5_fdma.o \
  sparx5_ptp.o sparx5_pgid.o sparx5_tc.o sparx5_qos.o \
- sparx5_vcap_impl.o sparx5_vcap_ag_api.o sparx5_tc_flower.o sparx5_tc_matchall.o
+ sparx5_vcap_impl.o sparx5_vcap_ag_api.o sparx5_tc_flower.o \
+ sparx5_tc_matchall.o sparx5_pool.o sparx5_sdlb.o sparx5_police.o sparx5_psfp.o
 
 sparx5-switch-$(CONFIG_SPARX5_DCB) += sparx5_dcb.o
 sparx5-switch-$(CONFIG_DEBUG_FS) += sparx5_vcap_debugfs.o

drivers/net/ethernet/microchip/sparx5/sparx5_main.c

@@ -205,6 +205,7 @@ static const struct sparx5_main_io_resource sparx5_main_iomap[] =  {
 	{ TARGET_EACL,               0x112c0000, 2 }, /* 0x6112c0000 */
 	{ TARGET_ANA_CL,             0x11400000, 2 }, /* 0x611400000 */
 	{ TARGET_ANA_L3,             0x11480000, 2 }, /* 0x611480000 */
+	{ TARGET_ANA_AC_SDLB,        0x11500000, 2 }, /* 0x611500000 */
 	{ TARGET_HSCH,               0x11580000, 2 }, /* 0x611580000 */
 	{ TARGET_REW,                0x11600000, 2 }, /* 0x611600000 */
 	{ TARGET_ANA_L2,             0x11800000, 2 }, /* 0x611800000 */
@@ -501,8 +502,8 @@ static int sparx5_init_coreclock(struct sparx5 *sparx5)
 
 	clk_period = sparx5_clk_period(freq);
 
-	spx5_rmw(HSCH_SYS_CLK_PER_SYS_CLK_PER_100PS_SET(clk_period / 100),
-		 HSCH_SYS_CLK_PER_SYS_CLK_PER_100PS,
+	spx5_rmw(HSCH_SYS_CLK_PER_100PS_SET(clk_period / 100),
+		 HSCH_SYS_CLK_PER_100PS,
 		 sparx5,
 		 HSCH_SYS_CLK_PER);
 

drivers/net/ethernet/microchip/sparx5/sparx5_main.h

@@ -396,6 +396,7 @@ int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
 void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
 				 struct sk_buff *skb);
 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args);
+int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts);
 
 /* sparx5_vcap_impl.c */
 int sparx5_vcap_init(struct sparx5 *sparx5);
@@ -413,6 +414,129 @@ int sparx5_pgid_alloc_glag(struct sparx5 *spx5, u16 *idx);
 int sparx5_pgid_alloc_mcast(struct sparx5 *spx5, u16 *idx);
 int sparx5_pgid_free(struct sparx5 *spx5, u16 idx);
 
+/* sparx5_pool.c */
+struct sparx5_pool_entry {
+	u16 ref_cnt;
+	u32 idx; /* tc index */
+};
+
+u32 sparx5_pool_idx_to_id(u32 idx);
+int sparx5_pool_put(struct sparx5_pool_entry *pool, int size, u32 id);
+int sparx5_pool_get(struct sparx5_pool_entry *pool, int size, u32 *id);
+int sparx5_pool_get_with_idx(struct sparx5_pool_entry *pool, int size, u32 idx,
+			     u32 *id);
+
+/* sparx5_sdlb.c */
+#define SPX5_SDLB_PUP_TOKEN_DISABLE 0x1FFF
+#define SPX5_SDLB_PUP_TOKEN_MAX (SPX5_SDLB_PUP_TOKEN_DISABLE - 1)
+#define SPX5_SDLB_GROUP_RATE_MAX 25000000000ULL
+#define SPX5_SDLB_2CYCLES_TYPE2_THRES_OFFSET 13
+#define SPX5_SDLB_CNT 4096
+#define SPX5_SDLB_GROUP_CNT 10
+#define SPX5_CLK_PER_100PS_DEFAULT 16
+
+struct sparx5_sdlb_group {
+	u64 max_rate;
+	u32 min_burst;
+	u32 frame_size;
+	u32 pup_interval;
+	u32 nsets;
+};
+
+extern struct sparx5_sdlb_group sdlb_groups[SPX5_SDLB_GROUP_CNT];
+int sparx5_sdlb_pup_token_get(struct sparx5 *sparx5, u32 pup_interval,
+			      u64 rate);
+
+int sparx5_sdlb_clk_hz_get(struct sparx5 *sparx5);
+int sparx5_sdlb_group_get_by_rate(struct sparx5 *sparx5, u32 rate, u32 burst);
+int sparx5_sdlb_group_get_by_index(struct sparx5 *sparx5, u32 idx, u32 *group);
+
+int sparx5_sdlb_group_add(struct sparx5 *sparx5, u32 group, u32 idx);
+int sparx5_sdlb_group_del(struct sparx5 *sparx5, u32 group, u32 idx);
+
+void sparx5_sdlb_group_init(struct sparx5 *sparx5, u64 max_rate, u32 min_burst,
+			    u32 frame_size, u32 idx);
+
+/* sparx5_police.c */
+enum {
+	/* More policer types will be added later */
+	SPX5_POL_SERVICE
+};
+
+struct sparx5_policer {
+	u32 type;
+	u32 idx;
+	u64 rate;
+	u32 burst;
+	u32 group;
+	u8 event_mask;
+};
+
+int sparx5_policer_conf_set(struct sparx5 *sparx5, struct sparx5_policer *pol);
+
+/* sparx5_psfp.c */
+#define SPX5_PSFP_GCE_CNT 4
+#define SPX5_PSFP_SG_CNT 1024
+#define SPX5_PSFP_SG_MIN_CYCLE_TIME_NS (1 * NSEC_PER_USEC)
+#define SPX5_PSFP_SG_MAX_CYCLE_TIME_NS ((1 * NSEC_PER_SEC) - 1)
+#define SPX5_PSFP_SG_MAX_IPV (SPX5_PRIOS - 1)
+#define SPX5_PSFP_SG_OPEN (SPX5_PSFP_SG_CNT - 1)
+#define SPX5_PSFP_SG_CYCLE_TIME_DEFAULT 1000000
+#define SPX5_PSFP_SF_MAX_SDU 16383
+
+struct sparx5_psfp_fm {
+	struct sparx5_policer pol;
+};
+
+struct sparx5_psfp_gce {
+	bool gate_state;            /* StreamGateState */
+	u32 interval;               /* TimeInterval */
+	u32 ipv;                    /* InternalPriorityValue */
+	u32 maxoctets;              /* IntervalOctetMax */
+};
+
+struct sparx5_psfp_sg {
+	bool gate_state;            /* PSFPAdminGateStates */
+	bool gate_enabled;          /* PSFPGateEnabled */
+	u32 ipv;                    /* PSFPAdminIPV */
+	struct timespec64 basetime; /* PSFPAdminBaseTime */
+	u32 cycletime;              /* PSFPAdminCycleTime */
+	u32 cycletimeext;           /* PSFPAdminCycleTimeExtension */
+	u32 num_entries;            /* PSFPAdminControlListLength */
+	struct sparx5_psfp_gce gce[SPX5_PSFP_GCE_CNT];
+};
+
+struct sparx5_psfp_sf {
+	bool sblock_osize_ena;
+	bool sblock_osize;
+	u32 max_sdu;
+	u32 sgid; /* Gate id */
+	u32 fmid; /* Flow meter id */
+};
+
+int sparx5_psfp_fm_add(struct sparx5 *sparx5, u32 uidx,
+		       struct sparx5_psfp_fm *fm, u32 *id);
+int sparx5_psfp_fm_del(struct sparx5 *sparx5, u32 id);
+
+int sparx5_psfp_sg_add(struct sparx5 *sparx5, u32 uidx,
+		       struct sparx5_psfp_sg *sg, u32 *id);
+int sparx5_psfp_sg_del(struct sparx5 *sparx5, u32 id);
+
+int sparx5_psfp_sf_add(struct sparx5 *sparx5, const struct sparx5_psfp_sf *sf,
+		       u32 *id);
+int sparx5_psfp_sf_del(struct sparx5 *sparx5, u32 id);
+
+u32 sparx5_psfp_isdx_get_sf(struct sparx5 *sparx5, u32 isdx);
+u32 sparx5_psfp_isdx_get_fm(struct sparx5 *sparx5, u32 isdx);
+u32 sparx5_psfp_sf_get_sg(struct sparx5 *sparx5, u32 sfid);
+void sparx5_isdx_conf_set(struct sparx5 *sparx5, u32 isdx, u32 sfid, u32 fmid);
+
+void sparx5_psfp_init(struct sparx5 *sparx5);
+
+/* sparx5_qos.c */
+void sparx5_new_base_time(struct sparx5 *sparx5, const u32 cycle_time,
+			  const ktime_t org_base_time, ktime_t *new_base_time);
+
 /* Clock period in picoseconds */
 static inline u32 sparx5_clk_period(enum sparx5_core_clockfreq cclock)
 {

drivers/net/ethernet/microchip/sparx5/sparx5_police.c

+// SPDX-License-Identifier: GPL-2.0+
+/* Microchip Sparx5 Switch driver
+ *
+ * Copyright (c) 2023 Microchip Technology Inc. and its subsidiaries.
+ */
+
+#include "sparx5_main_regs.h"
+#include "sparx5_main.h"
+
+static int sparx5_policer_service_conf_set(struct sparx5 *sparx5,
+					   struct sparx5_policer *pol)
+{
+	u32 idx, pup_tokens, max_pup_tokens, burst, thres;
+	struct sparx5_sdlb_group *g;
+	u64 rate;
+
+	g = &sdlb_groups[pol->group];
+	idx = pol->idx;
+
+	rate = pol->rate * 1000;
+	burst = pol->burst;
+
+	pup_tokens = sparx5_sdlb_pup_token_get(sparx5, g->pup_interval, rate);
+	max_pup_tokens =
+		sparx5_sdlb_pup_token_get(sparx5, g->pup_interval, g->max_rate);
+
+	thres = DIV_ROUND_UP(burst, g->min_burst);
+
+	spx5_wr(ANA_AC_SDLB_PUP_TOKENS_PUP_TOKENS_SET(pup_tokens), sparx5,
+		ANA_AC_SDLB_PUP_TOKENS(idx, 0));
+
+	spx5_rmw(ANA_AC_SDLB_INH_CTRL_PUP_TOKENS_MAX_SET(max_pup_tokens),
+		 ANA_AC_SDLB_INH_CTRL_PUP_TOKENS_MAX, sparx5,
+		 ANA_AC_SDLB_INH_CTRL(idx, 0));
+
+	spx5_rmw(ANA_AC_SDLB_THRES_THRES_SET(thres), ANA_AC_SDLB_THRES_THRES,
+		 sparx5, ANA_AC_SDLB_THRES(idx, 0));
+
+	return 0;
+}
+
+int sparx5_policer_conf_set(struct sparx5 *sparx5, struct sparx5_policer *pol)
+{
+	/* More policer types will be added later */
+	switch (pol->type) {
+	case SPX5_POL_SERVICE:
+		return sparx5_policer_service_conf_set(sparx5, pol);
+	default:
+		break;
+	}
+
+	return 0;
+}

drivers/net/ethernet/microchip/sparx5/sparx5_pool.c

+// SPDX-License-Identifier: GPL-2.0+
+/* Microchip Sparx5 Switch driver
+ *
+ * Copyright (c) 2023 Microchip Technology Inc. and its subsidiaries.
+ */
+
+#include "sparx5_main_regs.h"
+#include "sparx5_main.h"
+
+static u32 sparx5_pool_id_to_idx(u32 id)
+{
+	return --id;
+}
+
+u32 sparx5_pool_idx_to_id(u32 idx)
+{
+	return ++idx;
+}
+
+/* Release resource from pool.
+ * Return reference count on success, otherwise return error.
+ */
+int sparx5_pool_put(struct sparx5_pool_entry *pool, int size, u32 id)
+{
+	struct sparx5_pool_entry *e_itr;
+
+	e_itr = (pool + sparx5_pool_id_to_idx(id));
+	if (e_itr->ref_cnt == 0)
+		return -EINVAL;
+
+	return --e_itr->ref_cnt;
+}
+
+/* Get resource from pool.
+ * Return reference count on success, otherwise return error.
+ */
+int sparx5_pool_get(struct sparx5_pool_entry *pool, int size, u32 *id)
+{
+	struct sparx5_pool_entry *e_itr;
+	int i;
+
+	for (i = 0, e_itr = pool; i < size; i++, e_itr++) {
+		if (e_itr->ref_cnt == 0) {
+			*id = sparx5_pool_idx_to_id(i);
+			return ++e_itr->ref_cnt;
+		}
+	}
+
+	return -ENOSPC;
+}
+
+/* Get resource from pool that matches index.
+ * Return reference count on success, otherwise return error.
+ */
+int sparx5_pool_get_with_idx(struct sparx5_pool_entry *pool, int size, u32 idx,
+			     u32 *id)
+{
+	struct sparx5_pool_entry *e_itr;
+	int i, ret = -ENOSPC;
+
+	for (i = 0, e_itr = pool; i < size; i++, e_itr++) {
+		/* Pool index of first free entry */
+		if (e_itr->ref_cnt == 0 && ret == -ENOSPC)
+			ret = i;
+		/* Tc index already in use ? */
+		if (e_itr->idx == idx && e_itr->ref_cnt > 0) {
+			ret = i;
+			break;
+		}
+	}
+
+	/* Did we find a free entry? */
+	if (ret >= 0) {
+		*id = sparx5_pool_idx_to_id(ret);
+		e_itr = (pool + ret);
+		e_itr->idx = idx;
+		return ++e_itr->ref_cnt;
+	}
+
+	return ret;
+}

drivers/net/ethernet/microchip/sparx5/sparx5_psfp.c

+// SPDX-License-Identifier: GPL-2.0+
+/* Microchip Sparx5 Switch driver
+ *
+ * Copyright (c) 2023 Microchip Technology Inc. and its subsidiaries.
+ */
+
+#include "sparx5_main_regs.h"
+#include "sparx5_main.h"
+
+#define SPX5_PSFP_SF_CNT 1024
+#define SPX5_PSFP_SG_CONFIG_CHANGE_SLEEP 1000
+#define SPX5_PSFP_SG_CONFIG_CHANGE_TIMEO 100000
+
+/* Pool of available service policers */
+static struct sparx5_pool_entry sparx5_psfp_fm_pool[SPX5_SDLB_CNT];
+
+/* Pool of available stream gates */
+static struct sparx5_pool_entry sparx5_psfp_sg_pool[SPX5_PSFP_SG_CNT];
+
+/* Pool of available stream filters */
+static struct sparx5_pool_entry sparx5_psfp_sf_pool[SPX5_PSFP_SF_CNT];
+
+static int sparx5_psfp_sf_get(u32 *id)
+{
+	return sparx5_pool_get(sparx5_psfp_sf_pool, SPX5_PSFP_SF_CNT, id);
+}
+
+static int sparx5_psfp_sf_put(u32 id)
+{
+	return sparx5_pool_put(sparx5_psfp_sf_pool, SPX5_PSFP_SF_CNT, id);
+}
+
+static int sparx5_psfp_sg_get(u32 idx, u32 *id)
+{
+	return sparx5_pool_get_with_idx(sparx5_psfp_sg_pool, SPX5_PSFP_SG_CNT,
+					idx, id);
+}
+
+static int sparx5_psfp_sg_put(u32 id)
+{
+	return sparx5_pool_put(sparx5_psfp_sg_pool, SPX5_PSFP_SG_CNT, id);
+}
+
+static int sparx5_psfp_fm_get(u32 idx, u32 *id)
+{
+	return sparx5_pool_get_with_idx(sparx5_psfp_fm_pool, SPX5_SDLB_CNT, idx,
+					id);
+}
+
+static int sparx5_psfp_fm_put(u32 id)
+{
+	return sparx5_pool_put(sparx5_psfp_fm_pool, SPX5_SDLB_CNT, id);
+}
+
+u32 sparx5_psfp_isdx_get_sf(struct sparx5 *sparx5, u32 isdx)
+{
+	return ANA_L2_TSN_CFG_TSN_SFID_GET(spx5_rd(sparx5,
+						   ANA_L2_TSN_CFG(isdx)));
+}
+
+u32 sparx5_psfp_isdx_get_fm(struct sparx5 *sparx5, u32 isdx)
+{
+	return ANA_L2_DLB_CFG_DLB_IDX_GET(spx5_rd(sparx5,
+						  ANA_L2_DLB_CFG(isdx)));
+}
+
+u32 sparx5_psfp_sf_get_sg(struct sparx5 *sparx5, u32 sfid)
+{
+	return ANA_AC_TSN_SF_CFG_TSN_SGID_GET(spx5_rd(sparx5,
+						      ANA_AC_TSN_SF_CFG(sfid)));
+}
+
+void sparx5_isdx_conf_set(struct sparx5 *sparx5, u32 isdx, u32 sfid, u32 fmid)
+{
+	spx5_rmw(ANA_L2_TSN_CFG_TSN_SFID_SET(sfid), ANA_L2_TSN_CFG_TSN_SFID,
+		 sparx5, ANA_L2_TSN_CFG(isdx));
+
+	spx5_rmw(ANA_L2_DLB_CFG_DLB_IDX_SET(fmid), ANA_L2_DLB_CFG_DLB_IDX,
+		 sparx5, ANA_L2_DLB_CFG(isdx));
+}
+
+/* Internal priority value to internal priority selector */
+static u32 sparx5_psfp_ipv_to_ips(s32 ipv)
+{
+	return ipv > 0 ? (ipv | BIT(3)) : 0;
+}
+
+static int sparx5_psfp_sgid_get_status(struct sparx5 *sparx5)
+{
+	return spx5_rd(sparx5, ANA_AC_SG_ACCESS_CTRL);
+}
+
+static int sparx5_psfp_sgid_wait_for_completion(struct sparx5 *sparx5)
+{
+	u32 val;
+
+	return readx_poll_timeout(sparx5_psfp_sgid_get_status, sparx5, val,
+				  !ANA_AC_SG_ACCESS_CTRL_CONFIG_CHANGE_GET(val),
+				  SPX5_PSFP_SG_CONFIG_CHANGE_SLEEP,
+				  SPX5_PSFP_SG_CONFIG_CHANGE_TIMEO);
+}
+
+static void sparx5_psfp_sg_config_change(struct sparx5 *sparx5, u32 id)
+{
+	spx5_wr(ANA_AC_SG_ACCESS_CTRL_SGID_SET(id), sparx5,
+		ANA_AC_SG_ACCESS_CTRL);
+
+	spx5_wr(ANA_AC_SG_ACCESS_CTRL_CONFIG_CHANGE_SET(1) |
+		ANA_AC_SG_ACCESS_CTRL_SGID_SET(id),
+		sparx5, ANA_AC_SG_ACCESS_CTRL);
+
+	if (sparx5_psfp_sgid_wait_for_completion(sparx5) < 0)
+		pr_debug("%s:%d timed out waiting for sgid completion",
+			 __func__, __LINE__);
+}
+
+static void sparx5_psfp_sf_set(struct sparx5 *sparx5, u32 id,
+			       const struct sparx5_psfp_sf *sf)
+{
+	/* Configure stream gate*/
+	spx5_rmw(ANA_AC_TSN_SF_CFG_TSN_SGID_SET(sf->sgid) |
+		ANA_AC_TSN_SF_CFG_TSN_MAX_SDU_SET(sf->max_sdu) |
+		ANA_AC_TSN_SF_CFG_BLOCK_OVERSIZE_STATE_SET(sf->sblock_osize) |
+		ANA_AC_TSN_SF_CFG_BLOCK_OVERSIZE_ENA_SET(sf->sblock_osize_ena),
+		ANA_AC_TSN_SF_CFG_TSN_SGID | ANA_AC_TSN_SF_CFG_TSN_MAX_SDU |
+		ANA_AC_TSN_SF_CFG_BLOCK_OVERSIZE_STATE |
+		ANA_AC_TSN_SF_CFG_BLOCK_OVERSIZE_ENA,
+		sparx5, ANA_AC_TSN_SF_CFG(id));
+}
+
+static int sparx5_psfp_sg_set(struct sparx5 *sparx5, u32 id,
+			      const struct sparx5_psfp_sg *sg)
+{
+	u32 ips, base_lsb, base_msb, accum_time_interval = 0;
+	const struct sparx5_psfp_gce *gce;
+	int i;
+
+	ips = sparx5_psfp_ipv_to_ips(sg->ipv);
+	base_lsb = sg->basetime.tv_sec & 0xffffffff;
+	base_msb = sg->basetime.tv_sec >> 32;
+
+	/* Set stream gate id */
+	spx5_wr(ANA_AC_SG_ACCESS_CTRL_SGID_SET(id), sparx5,
+		ANA_AC_SG_ACCESS_CTRL);
+
+	/* Write AdminPSFP values */
+	spx5_wr(sg->basetime.tv_nsec, sparx5, ANA_AC_SG_CONFIG_REG_1);
+	spx5_wr(base_lsb, sparx5, ANA_AC_SG_CONFIG_REG_2);
+
+	spx5_rmw(ANA_AC_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB_SET(base_msb) |
+		ANA_AC_SG_CONFIG_REG_3_INIT_IPS_SET(ips) |
+		ANA_AC_SG_CONFIG_REG_3_LIST_LENGTH_SET(sg->num_entries) |
+		ANA_AC_SG_CONFIG_REG_3_INIT_GATE_STATE_SET(sg->gate_state) |
+		ANA_AC_SG_CONFIG_REG_3_GATE_ENABLE_SET(1),
+		ANA_AC_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB |
+		ANA_AC_SG_CONFIG_REG_3_INIT_IPS |
+		ANA_AC_SG_CONFIG_REG_3_LIST_LENGTH |
+		ANA_AC_SG_CONFIG_REG_3_INIT_GATE_STATE |
+		ANA_AC_SG_CONFIG_REG_3_GATE_ENABLE,
+		sparx5, ANA_AC_SG_CONFIG_REG_3);
+
+	spx5_wr(sg->cycletime, sparx5, ANA_AC_SG_CONFIG_REG_4);
+	spx5_wr(sg->cycletimeext, sparx5, ANA_AC_SG_CONFIG_REG_5);
+
+	/* For each scheduling entry */
+	for (i = 0; i < sg->num_entries; i++) {
+		gce = &sg->gce[i];
+		ips = sparx5_psfp_ipv_to_ips(gce->ipv);
+		/* hardware needs TimeInterval to be cumulative */
+		accum_time_interval += gce->interval;
+		/* Set gate state */
+		spx5_wr(ANA_AC_SG_GCL_GS_CONFIG_IPS_SET(ips) |
+			ANA_AC_SG_GCL_GS_CONFIG_GATE_STATE_SET(gce->gate_state),
+			sparx5, ANA_AC_SG_GCL_GS_CONFIG(i));
+
+		/* Set time interval */
+		spx5_wr(accum_time_interval, sparx5,
+			ANA_AC_SG_GCL_TI_CONFIG(i));
+
+		/* Set maximum octets */
+		spx5_wr(gce->maxoctets, sparx5, ANA_AC_SG_GCL_OCT_CONFIG(i));
+	}
+
+	return 0;
+}
+
+static int sparx5_sdlb_conf_set(struct sparx5 *sparx5,
+				struct sparx5_psfp_fm *fm)
+{
+	int (*sparx5_sdlb_group_action)(struct sparx5 *sparx5, u32 group,
+					u32 idx);
+
+	if (!fm->pol.rate && !fm->pol.burst)
+		sparx5_sdlb_group_action = &sparx5_sdlb_group_del;
+	else
+		sparx5_sdlb_group_action = &sparx5_sdlb_group_add;
+
+	sparx5_policer_conf_set(sparx5, &fm->pol);
+
+	return sparx5_sdlb_group_action(sparx5, fm->pol.group, fm->pol.idx);
+}
+
+int sparx5_psfp_sf_add(struct sparx5 *sparx5, const struct sparx5_psfp_sf *sf,
+		       u32 *id)
+{
+	int ret;
+
+	ret = sparx5_psfp_sf_get(id);
+	if (ret < 0)
+		return ret;
+
+	sparx5_psfp_sf_set(sparx5, *id, sf);
+
+	return 0;
+}
+
+int sparx5_psfp_sf_del(struct sparx5 *sparx5, u32 id)
+{
+	const struct sparx5_psfp_sf sf = { 0 };
+
+	sparx5_psfp_sf_set(sparx5, id, &sf);
+
+	return sparx5_psfp_sf_put(id);
+}
+
+int sparx5_psfp_sg_add(struct sparx5 *sparx5, u32 uidx,
+		       struct sparx5_psfp_sg *sg, u32 *id)
+{
+	ktime_t basetime;
+	int ret;
+
+	ret = sparx5_psfp_sg_get(uidx, id);
+	if (ret < 0)
+		return ret;
+	/* Was already in use, no need to reconfigure */
+	if (ret > 1)
+		return 0;
+
+	/* Calculate basetime for this stream gate */
+	sparx5_new_base_time(sparx5, sg->cycletime, 0, &basetime);
+	sg->basetime = ktime_to_timespec64(basetime);
+
+	sparx5_psfp_sg_set(sparx5, *id, sg);
+
+	/* Signal hardware to copy AdminPSFP values into OperPSFP values */
+	sparx5_psfp_sg_config_change(sparx5, *id);
+
+	return 0;
+}
+
+int sparx5_psfp_sg_del(struct sparx5 *sparx5, u32 id)
+{
+	const struct sparx5_psfp_sg sg = { 0 };
+	int ret;
+
+	ret = sparx5_psfp_sg_put(id);
+	if (ret < 0)
+		return ret;
+	/* Stream gate still in use ? */
+	if (ret > 0)
+		return 0;
+
+	return sparx5_psfp_sg_set(sparx5, id, &sg);
+}
+
+int sparx5_psfp_fm_add(struct sparx5 *sparx5, u32 uidx,
+		       struct sparx5_psfp_fm *fm, u32 *id)
+{
+	struct sparx5_policer *pol = &fm->pol;
+	int ret;
+
+	/* Get flow meter */
+	ret = sparx5_psfp_fm_get(uidx, &fm->pol.idx);
+	if (ret < 0)
+		return ret;
+	/* Was already in use, no need to reconfigure */
+	if (ret > 1)
+		return 0;
+
+	ret = sparx5_sdlb_group_get_by_rate(sparx5, pol->rate, pol->burst);
+	if (ret < 0)
+		return ret;
+
+	fm->pol.group = ret;
+
+	ret = sparx5_sdlb_conf_set(sparx5, fm);
+	if (ret < 0)
+		return ret;
+
+	*id = fm->pol.idx;
+
+	return 0;
+}
+
+int sparx5_psfp_fm_del(struct sparx5 *sparx5, u32 id)
+{
+	struct sparx5_psfp_fm fm = { .pol.idx = id,
+				     .pol.type = SPX5_POL_SERVICE };
+	int ret;
+
+	/* Find the group that this lb belongs to */
+	ret = sparx5_sdlb_group_get_by_index(sparx5, id, &fm.pol.group);
+	if (ret < 0)
+		return ret;
+
+	ret = sparx5_psfp_fm_put(id);
+	if (ret < 0)
+		return ret;
+	/* Do not reset flow-meter if still in use. */
+	if (ret > 0)
+		return 0;
+
+	return sparx5_sdlb_conf_set(sparx5, &fm);
+}
+
+void sparx5_psfp_init(struct sparx5 *sparx5)
+{
+	const struct sparx5_sdlb_group *group;
+	int i;
+
+	for (i = 0; i < SPX5_SDLB_GROUP_CNT; i++) {
+		group = &sdlb_groups[i];
+		sparx5_sdlb_group_init(sparx5, group->max_rate,
+				       group->min_burst, group->frame_size, i);
+	}
+
+	spx5_wr(ANA_AC_SG_CYCLETIME_UPDATE_PERIOD_SG_CT_UPDATE_ENA_SET(1),
+		sparx5, ANA_AC_SG_CYCLETIME_UPDATE_PERIOD);
+
+	spx5_rmw(ANA_L2_FWD_CFG_ISDX_LOOKUP_ENA_SET(1),
+		 ANA_L2_FWD_CFG_ISDX_LOOKUP_ENA, sparx5, ANA_L2_FWD_CFG);
+}

drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c

@@ -476,8 +476,7 @@ static int sparx5_ptp_settime64(struct ptp_clock_info *ptp,
 	return 0;
 }
 
-static int sparx5_ptp_gettime64(struct ptp_clock_info *ptp,
-				struct timespec64 *ts)
+int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
 	struct sparx5 *sparx5 = phc->sparx5;

drivers/net/ethernet/microchip/sparx5/sparx5_qos.c

@@ -9,6 +9,63 @@
 #include "sparx5_main.h"
 #include "sparx5_qos.h"
 
+/* Calculate new base_time based on cycle_time.
+ *
+ * The hardware requires a base_time that is always in the future.
+ * We define threshold_time as current_time + (2 * cycle_time).
+ * If base_time is below threshold_time this function recalculates it to be in
+ * the interval:
+ * threshold_time <= base_time < (threshold_time + cycle_time)
+ *
+ * A very simple algorithm could be like this:
+ * new_base_time = org_base_time + N * cycle_time
+ * using the lowest N so (new_base_time >= threshold_time
+ */
+void sparx5_new_base_time(struct sparx5 *sparx5, const u32 cycle_time,
+			  const ktime_t org_base_time, ktime_t *new_base_time)
+{
+	ktime_t current_time, threshold_time, new_time;
+	struct timespec64 ts;
+	u64 nr_of_cycles_p2;
+	u64 nr_of_cycles;
+	u64 diff_time;
+
+	new_time = org_base_time;
+
+	sparx5_ptp_gettime64(&sparx5->phc[SPARX5_PHC_PORT].info, &ts);
+	current_time = timespec64_to_ktime(ts);
+	threshold_time = current_time + (2 * cycle_time);
+	diff_time = threshold_time - new_time;
+	nr_of_cycles = div_u64(diff_time, cycle_time);
+	nr_of_cycles_p2 = 1; /* Use 2^0 as start value */
+
+	if (new_time >= threshold_time) {
+		*new_base_time = new_time;
+		return;
+	}
+
+	/* Calculate the smallest power of 2 (nr_of_cycles_p2)
+	 * that is larger than nr_of_cycles.
+	 */
+	while (nr_of_cycles_p2 < nr_of_cycles)
+		nr_of_cycles_p2 <<= 1; /* Next (higher) power of 2 */
+
+	/* Add as big chunks (power of 2 * cycle_time)
+	 * as possible for each power of 2
+	 */
+	while (nr_of_cycles_p2) {
+		if (new_time < threshold_time) {
+			new_time += cycle_time * nr_of_cycles_p2;
+			while (new_time < threshold_time)
+				new_time += cycle_time * nr_of_cycles_p2;
+			new_time -= cycle_time * nr_of_cycles_p2;
+		}
+		nr_of_cycles_p2 >>= 1; /* Next (lower) power of 2 */
+	}
+	new_time += cycle_time;
+	*new_base_time = new_time;
+}
+
 /* Max rates for leak groups */
 static const u32 spx5_hsch_max_group_rate[SPX5_HSCH_LEAK_GRP_CNT] = {
 	1048568, /*  1.049 Gbps */
@@ -393,6 +450,8 @@ int sparx5_qos_init(struct sparx5 *sparx5)
 	if (ret < 0)
 		return ret;
 
+	sparx5_psfp_init(sparx5);
+
 	return 0;
 }
 

drivers/net/ethernet/microchip/sparx5/sparx5_sdlb.c

+// SPDX-License-Identifier: GPL-2.0+
+/* Microchip Sparx5 Switch driver
+ *
+ * Copyright (c) 2023 Microchip Technology Inc. and its subsidiaries.
+ */
+
+#include "sparx5_main_regs.h"
+#include "sparx5_main.h"
+
+struct sparx5_sdlb_group sdlb_groups[SPX5_SDLB_GROUP_CNT] = {
+	{ SPX5_SDLB_GROUP_RATE_MAX,    8192 / 1, 64 }, /*  25 G */
+	{ 15000000000ULL,              8192 / 1, 64 }, /*  15 G */
+	{ 10000000000ULL,              8192 / 1, 64 }, /*  10 G */
+	{  5000000000ULL,              8192 / 1, 64 }, /*   5 G */
+	{  2500000000ULL,              8192 / 1, 64 }, /* 2.5 G */
+	{  1000000000ULL,              8192 / 2, 64 }, /*   1 G */
+	{   500000000ULL,              8192 / 2, 64 }, /* 500 M */
+	{   100000000ULL,              8192 / 4, 64 }, /* 100 M */
+	{    50000000ULL,              8192 / 4, 64 }, /*  50 M */
+	{     5000000ULL,              8192 / 8, 64 }  /*   5 M */
+};
+
+int sparx5_sdlb_clk_hz_get(struct sparx5 *sparx5)
+{
+	u32 clk_per_100ps;
+	u64 clk_hz;
+
+	clk_per_100ps = HSCH_SYS_CLK_PER_100PS_GET(spx5_rd(sparx5,
+							   HSCH_SYS_CLK_PER));
+	if (!clk_per_100ps)
+		clk_per_100ps = SPX5_CLK_PER_100PS_DEFAULT;
+
+	clk_hz = (10 * 1000 * 1000) / clk_per_100ps;
+	return clk_hz *= 1000;
+}
+
+static int sparx5_sdlb_pup_interval_get(struct sparx5 *sparx5, u32 max_token,
+					u64 max_rate)
+{
+	u64 clk_hz;
+
+	clk_hz = sparx5_sdlb_clk_hz_get(sparx5);
+
+	return div64_u64((8 * clk_hz * max_token), max_rate);
+}
+
+int sparx5_sdlb_pup_token_get(struct sparx5 *sparx5, u32 pup_interval, u64 rate)
+{
+	u64 clk_hz;
+
+	if (!rate)
+		return SPX5_SDLB_PUP_TOKEN_DISABLE;
+
+	clk_hz = sparx5_sdlb_clk_hz_get(sparx5);
+
+	return DIV64_U64_ROUND_UP((rate * pup_interval), (clk_hz * 8));
+}
+
+static void sparx5_sdlb_group_disable(struct sparx5 *sparx5, u32 group)
+{
+	spx5_rmw(ANA_AC_SDLB_PUP_CTRL_PUP_ENA_SET(0),
+		 ANA_AC_SDLB_PUP_CTRL_PUP_ENA, sparx5,
+		 ANA_AC_SDLB_PUP_CTRL(group));
+}
+
+static void sparx5_sdlb_group_enable(struct sparx5 *sparx5, u32 group)
+{
+	spx5_rmw(ANA_AC_SDLB_PUP_CTRL_PUP_ENA_SET(1),
+		 ANA_AC_SDLB_PUP_CTRL_PUP_ENA, sparx5,
+		 ANA_AC_SDLB_PUP_CTRL(group));
+}
+
+static u32 sparx5_sdlb_group_get_first(struct sparx5 *sparx5, u32 group)
+{
+	u32 val;
+
+	val = spx5_rd(sparx5, ANA_AC_SDLB_XLB_START(group));
+
+	return ANA_AC_SDLB_XLB_START_LBSET_START_GET(val);
+}
+
+static u32 sparx5_sdlb_group_get_next(struct sparx5 *sparx5, u32 group,
+				      u32 lb)
+{
+	u32 val;
+
+	val = spx5_rd(sparx5, ANA_AC_SDLB_XLB_NEXT(lb));
+
+	return ANA_AC_SDLB_XLB_NEXT_LBSET_NEXT_GET(val);
+}
+
+static bool sparx5_sdlb_group_is_first(struct sparx5 *sparx5, u32 group,
+				       u32 lb)
+{
+	return lb == sparx5_sdlb_group_get_first(sparx5, group);
+}
+
+static bool sparx5_sdlb_group_is_last(struct sparx5 *sparx5, u32 group,
+				      u32 lb)
+{
+	return lb == sparx5_sdlb_group_get_next(sparx5, group, lb);
+}
+
+static bool sparx5_sdlb_group_is_empty(struct sparx5 *sparx5, u32 group)
+{
+	u32 val;
+
+	val = spx5_rd(sparx5, ANA_AC_SDLB_PUP_CTRL(group));
+
+	return ANA_AC_SDLB_PUP_CTRL_PUP_ENA_GET(val) == 0;
+}
+
+static u32 sparx5_sdlb_group_get_last(struct sparx5 *sparx5, u32 group)
+{
+	u32 itr, next;
+
+	itr = sparx5_sdlb_group_get_first(sparx5, group);
+
+	for (;;) {
+		next = sparx5_sdlb_group_get_next(sparx5, group, itr);
+		if (itr == next)
+			return itr;
+
+		itr = next;
+	}
+}
+
+static bool sparx5_sdlb_group_is_singular(struct sparx5 *sparx5, u32 group)
+{
+	if (sparx5_sdlb_group_is_empty(sparx5, group))
+		return false;
+
+	return sparx5_sdlb_group_get_first(sparx5, group) ==
+	       sparx5_sdlb_group_get_last(sparx5, group);
+}
+
+static int sparx5_sdlb_group_get_adjacent(struct sparx5 *sparx5, u32 group,
+					  u32 idx, u32 *prev, u32 *next,
+					  u32 *first)
+{
+	u32 itr;
+
+	*first = sparx5_sdlb_group_get_first(sparx5, group);
+	*prev = *first;
+	*next = *first;
+	itr = *first;
+
+	for (;;) {
+		*next = sparx5_sdlb_group_get_next(sparx5, group, itr);
+
+		if (itr == idx)
+			return 0; /* Found it */
+
+		if (itr == *next)
+			return -EINVAL; /* Was not found */
+
+		*prev = itr;
+		itr = *next;
+	}
+}
+
+static int sparx5_sdlb_group_get_count(struct sparx5 *sparx5, u32 group)
+{
+	u32 itr, next;
+	int count = 0;
+
+	itr = sparx5_sdlb_group_get_first(sparx5, group);
+
+	for (;;) {
+		next = sparx5_sdlb_group_get_next(sparx5, group, itr);
+		if (itr == next)
+			return count;
+
+		itr = next;
+		count++;
+	}
+}
+
+int sparx5_sdlb_group_get_by_rate(struct sparx5 *sparx5, u32 rate, u32 burst)
+{
+	const struct sparx5_sdlb_group *group;
+	u64 rate_bps;
+	int i, count;
+
+	rate_bps = rate * 1000;
+
+	for (i = SPX5_SDLB_GROUP_CNT - 1; i >= 0; i--) {
+		group = &sdlb_groups[i];
+
+		count = sparx5_sdlb_group_get_count(sparx5, i);
+
+		/* Check that this group is not full.
+		 * According to LB group configuration rules: the number of XLBs
+		 * in a group must not exceed PUP_INTERVAL/4 - 1.
+		 */
+		if (count > ((group->pup_interval / 4) - 1))
+			continue;
+
+		if (rate_bps < group->max_rate)
+			return i;
+	}
+
+	return -ENOSPC;
+}
+
+int sparx5_sdlb_group_get_by_index(struct sparx5 *sparx5, u32 idx, u32 *group)
+{
+	u32 itr, next;
+	int i;
+
+	for (i = 0; i < SPX5_SDLB_GROUP_CNT; i++) {
+		if (sparx5_sdlb_group_is_empty(sparx5, i))
+			continue;
+
+		itr = sparx5_sdlb_group_get_first(sparx5, i);
+
+		for (;;) {
+			next = sparx5_sdlb_group_get_next(sparx5, i, itr);
+
+			if (itr == idx) {
+				*group = i;
+				return 0; /* Found it */
+			}
+			if (itr == next)
+				break; /* Was not found */
+
+			itr = next;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int sparx5_sdlb_group_link(struct sparx5 *sparx5, u32 group, u32 idx,
+				  u32 first, u32 next, bool empty)
+{
+	/* Stop leaking */
+	sparx5_sdlb_group_disable(sparx5, group);
+
+	if (empty)
+		return 0;
+
+	/* Link insertion lb to next lb */
+	spx5_wr(ANA_AC_SDLB_XLB_NEXT_LBSET_NEXT_SET(next) |
+			ANA_AC_SDLB_XLB_NEXT_LBGRP_SET(group),
+		sparx5, ANA_AC_SDLB_XLB_NEXT(idx));
+
+	/* Set the first lb */
+	spx5_wr(ANA_AC_SDLB_XLB_START_LBSET_START_SET(first), sparx5,
+		ANA_AC_SDLB_XLB_START(group));
+
+	/* Start leaking */
+	sparx5_sdlb_group_enable(sparx5, group);
+
+	return 0;
+};
+
+int sparx5_sdlb_group_add(struct sparx5 *sparx5, u32 group, u32 idx)
+{
+	u32 first, next;
+
+	/* We always add to head of the list */
+	first = idx;
+
+	if (sparx5_sdlb_group_is_empty(sparx5, group))
+		next = idx;
+	else
+		next = sparx5_sdlb_group_get_first(sparx5, group);
+
+	return sparx5_sdlb_group_link(sparx5, group, idx, first, next, false);
+}
+
+int sparx5_sdlb_group_del(struct sparx5 *sparx5, u32 group, u32 idx)
+{
+	u32 first, next, prev;
+	bool empty = false;
+
+	if (sparx5_sdlb_group_get_adjacent(sparx5, group, idx, &prev, &next,
+					   &first) < 0) {
+		pr_err("%s:%d Could not find idx: %d in group: %d", __func__,
+		       __LINE__, idx, group);
+		return -EINVAL;
+	}
+
+	if (sparx5_sdlb_group_is_singular(sparx5, group)) {
+		empty = true;
+	} else if (sparx5_sdlb_group_is_last(sparx5, group, idx)) {
+		/* idx is removed, prev is now last */
+		idx = prev;
+		next = prev;
+	} else if (sparx5_sdlb_group_is_first(sparx5, group, idx)) {
+		/* idx is removed and points to itself, first is next */
+		first = next;
+		next = idx;
+	} else {
+		/* Next is not touched */
+		idx = prev;
+	}
+
+	return sparx5_sdlb_group_link(sparx5, group, idx, first, next, empty);
+}
+
+void sparx5_sdlb_group_init(struct sparx5 *sparx5, u64 max_rate, u32 min_burst,
+			    u32 frame_size, u32 idx)
+{
+	u32 thres_shift, mask = 0x01, power = 0;
+	struct sparx5_sdlb_group *group;
+	u64 max_token;
+
+	group = &sdlb_groups[idx];
+
+	/* Number of positions to right-shift LB's threshold value. */
+	while ((min_burst & mask) == 0) {
+		power++;
+		mask <<= 1;
+	}
+	thres_shift = SPX5_SDLB_2CYCLES_TYPE2_THRES_OFFSET - power;
+
+	max_token = (min_burst > SPX5_SDLB_PUP_TOKEN_MAX) ?
+			    SPX5_SDLB_PUP_TOKEN_MAX :
+			    min_burst;
+	group->pup_interval =
+		sparx5_sdlb_pup_interval_get(sparx5, max_token, max_rate);
+
+	group->frame_size = frame_size;
+
+	spx5_wr(ANA_AC_SDLB_PUP_INTERVAL_PUP_INTERVAL_SET(group->pup_interval),
+		sparx5, ANA_AC_SDLB_PUP_INTERVAL(idx));
+
+	spx5_wr(ANA_AC_SDLB_FRM_RATE_TOKENS_FRM_RATE_TOKENS_SET(frame_size),
+		sparx5, ANA_AC_SDLB_FRM_RATE_TOKENS(idx));
+
+	spx5_wr(ANA_AC_SDLB_LBGRP_MISC_THRES_SHIFT_SET(thres_shift), sparx5,
+		ANA_AC_SDLB_LBGRP_MISC(idx));
+}

drivers/net/ethernet/microchip/sparx5/sparx5_tc_flower.c

@@ -4,6 +4,7 @@
  * Copyright (c) 2022 Microchip Technology Inc. and its subsidiaries.
  */
 
+#include <net/tc_act/tc_gate.h>
 #include <net/tcp.h>
 
 #include "sparx5_tc.h"
@@ -989,19 +990,156 @@ static int sparx5_tc_add_rule_link(struct vcap_control *vctrl,
 	return err;
 }
 
+static int sparx5_tc_flower_parse_act_gate(struct sparx5_psfp_sg *sg,
+					   struct flow_action_entry *act,
+					   struct netlink_ext_ack *extack)
+{
+	int i;
+
+	if (act->gate.prio < -1 || act->gate.prio > SPX5_PSFP_SG_MAX_IPV) {
+		NL_SET_ERR_MSG_MOD(extack, "Invalid gate priority");
+		return -EINVAL;
+	}
+
+	if (act->gate.cycletime < SPX5_PSFP_SG_MIN_CYCLE_TIME_NS ||
+	    act->gate.cycletime > SPX5_PSFP_SG_MAX_CYCLE_TIME_NS) {
+		NL_SET_ERR_MSG_MOD(extack, "Invalid gate cycletime");
+		return -EINVAL;
+	}
+
+	if (act->gate.cycletimeext > SPX5_PSFP_SG_MAX_CYCLE_TIME_NS) {
+		NL_SET_ERR_MSG_MOD(extack, "Invalid gate cycletimeext");
+		return -EINVAL;
+	}
+
+	if (act->gate.num_entries >= SPX5_PSFP_GCE_CNT) {
+		NL_SET_ERR_MSG_MOD(extack, "Invalid number of gate entries");
+		return -EINVAL;
+	}
+
+	sg->gate_state = true;
+	sg->ipv = act->gate.prio;
+	sg->num_entries = act->gate.num_entries;
+	sg->cycletime = act->gate.cycletime;
+	sg->cycletimeext = act->gate.cycletimeext;
+
+	for (i = 0; i < sg->num_entries; i++) {
+		sg->gce[i].gate_state = !!act->gate.entries[i].gate_state;
+		sg->gce[i].interval = act->gate.entries[i].interval;
+		sg->gce[i].ipv = act->gate.entries[i].ipv;
+		sg->gce[i].maxoctets = act->gate.entries[i].maxoctets;
+	}
+
+	return 0;
+}
+
+static int sparx5_tc_flower_parse_act_police(struct sparx5_policer *pol,
+					     struct flow_action_entry *act,
+					     struct netlink_ext_ack *extack)
+{
+	pol->type = SPX5_POL_SERVICE;
+	pol->rate = div_u64(act->police.rate_bytes_ps, 1000) * 8;
+	pol->burst = act->police.burst;
+	pol->idx = act->hw_index;
+
+	/* rate is now in kbit */
+	if (pol->rate > DIV_ROUND_UP(SPX5_SDLB_GROUP_RATE_MAX, 1000)) {
+		NL_SET_ERR_MSG_MOD(extack, "Maximum rate exceeded");
+		return -EINVAL;
+	}
+
+	if (act->police.exceed.act_id != FLOW_ACTION_DROP) {
+		NL_SET_ERR_MSG_MOD(extack, "Offload not supported when exceed action is not drop");
+		return -EOPNOTSUPP;
+	}
+
+	if (act->police.notexceed.act_id != FLOW_ACTION_PIPE &&
+	    act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) {
+		NL_SET_ERR_MSG_MOD(extack, "Offload not supported when conform action is not pipe or ok");
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static int sparx5_tc_flower_psfp_setup(struct sparx5 *sparx5,
+				       struct vcap_rule *vrule, int sg_idx,
+				       int pol_idx, struct sparx5_psfp_sg *sg,
+				       struct sparx5_psfp_fm *fm,
+				       struct sparx5_psfp_sf *sf)
+{
+	u32 psfp_sfid = 0, psfp_fmid = 0, psfp_sgid = 0;
+	int ret;
+
+	/* Must always have a stream gate - max sdu (filter option) is evaluated
+	 * after frames have passed the gate, so in case of only a policer, we
+	 * allocate a stream gate that is always open.
+	 */
+	if (sg_idx < 0) {
+		sg_idx = sparx5_pool_idx_to_id(SPX5_PSFP_SG_OPEN);
+		sg->ipv = 0; /* Disabled */
+		sg->cycletime = SPX5_PSFP_SG_CYCLE_TIME_DEFAULT;
+		sg->num_entries = 1;
+		sg->gate_state = 1; /* Open */
+		sg->gate_enabled = 1;
+		sg->gce[0].gate_state = 1;
+		sg->gce[0].interval = SPX5_PSFP_SG_CYCLE_TIME_DEFAULT;
+		sg->gce[0].ipv = 0;
+		sg->gce[0].maxoctets = 0; /* Disabled */
+	}
+
+	ret = sparx5_psfp_sg_add(sparx5, sg_idx, sg, &psfp_sgid);
+	if (ret < 0)
+		return ret;
+
+	if (pol_idx >= 0) {
+		/* Add new flow-meter */
+		ret = sparx5_psfp_fm_add(sparx5, pol_idx, fm, &psfp_fmid);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Map stream filter to stream gate */
+	sf->sgid = psfp_sgid;
+
+	/* Add new stream-filter and map it to a steam gate */
+	ret = sparx5_psfp_sf_add(sparx5, sf, &psfp_sfid);
+	if (ret < 0)
+		return ret;
+
+	/* Streams are classified by ISDX - map ISDX 1:1 to sfid for now. */
+	sparx5_isdx_conf_set(sparx5, psfp_sfid, psfp_sfid, psfp_fmid);
+
+	ret = vcap_rule_add_action_bit(vrule, VCAP_AF_ISDX_ADD_REPLACE_SEL,
+				       VCAP_BIT_1);
+	if (ret)
+		return ret;
+
+	ret = vcap_rule_add_action_u32(vrule, VCAP_AF_ISDX_VAL, psfp_sfid);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 static int sparx5_tc_flower_replace(struct net_device *ndev,
 				    struct flow_cls_offload *fco,
 				    struct vcap_admin *admin,
 				    bool ingress)
 {
+	struct sparx5_psfp_sf sf = { .max_sdu = SPX5_PSFP_SF_MAX_SDU };
+	struct netlink_ext_ack *extack = fco->common.extack;
+	int err, idx, tc_sg_idx = -1, tc_pol_idx = -1;
 	struct sparx5_port *port = netdev_priv(ndev);
 	struct sparx5_multiple_rules multi = {};
+	struct sparx5 *sparx5 = port->sparx5;
+	struct sparx5_psfp_sg sg = { 0 };
+	struct sparx5_psfp_fm fm = { 0 };
 	struct flow_action_entry *act;
 	struct vcap_control *vctrl;
 	struct flow_rule *frule;
 	struct vcap_rule *vrule;
 	u16 l3_proto;
-	int err, idx;
 
 	vctrl = port->sparx5->vcap_ctrl;
 
@@ -1033,6 +1171,26 @@ static int sparx5_tc_flower_replace(struct net_device *ndev,
 	frule = flow_cls_offload_flow_rule(fco);
 	flow_action_for_each(idx, act, &frule->action) {
 		switch (act->id) {
+		case FLOW_ACTION_GATE: {
+			err = sparx5_tc_flower_parse_act_gate(&sg, act, extack);
+			if (err < 0)
+				goto out;
+
+			tc_sg_idx = act->hw_index;
+
+			break;
+		}
+		case FLOW_ACTION_POLICE: {
+			err = sparx5_tc_flower_parse_act_police(&fm.pol, act,
+								extack);
+			if (err < 0)
+				goto out;
+
+			tc_pol_idx = fm.pol.idx;
+			sf.max_sdu = act->police.mtu;
+
+			break;
+		}
 		case FLOW_ACTION_TRAP:
 			if (admin->vtype != VCAP_TYPE_IS2 &&
 			    admin->vtype != VCAP_TYPE_ES2) {
@@ -1079,6 +1237,14 @@ static int sparx5_tc_flower_replace(struct net_device *ndev,
 		}
 	}
 
+	/* Setup PSFP */
+	if (tc_sg_idx >= 0 || tc_pol_idx >= 0) {
+		err = sparx5_tc_flower_psfp_setup(sparx5, vrule, tc_sg_idx,
+						  tc_pol_idx, &sg, &fm, &sf);
+		if (err)
+			goto out;
+	}
+
 	err = sparx5_tc_select_protocol_keyset(ndev, vrule, admin, l3_proto,
 					       &multi);
 	if (err) {
@@ -1107,19 +1273,86 @@ static int sparx5_tc_flower_replace(struct net_device *ndev,
 	return err;
 }
 
+static void sparx5_tc_free_psfp_resources(struct sparx5 *sparx5,
+					  struct vcap_rule *vrule)
+{
+	struct vcap_client_actionfield *afield;
+	u32 isdx, sfid, sgid, fmid;
+
+	/* Check if VCAP_AF_ISDX_VAL action is set for this rule - and if
+	 * it is used for stream and/or flow-meter classification.
+	 */
+	afield = vcap_find_actionfield(vrule, VCAP_AF_ISDX_VAL);
+	if (!afield)
+		return;
+
+	isdx = afield->data.u32.value;
+	sfid = sparx5_psfp_isdx_get_sf(sparx5, isdx);
+
+	if (!sfid)
+		return;
+
+	fmid = sparx5_psfp_isdx_get_fm(sparx5, isdx);
+	sgid = sparx5_psfp_sf_get_sg(sparx5, sfid);
+
+	if (fmid && sparx5_psfp_fm_del(sparx5, fmid) < 0)
+		pr_err("%s:%d Could not delete invalid fmid: %d", __func__,
+		       __LINE__, fmid);
+
+	if (sgid && sparx5_psfp_sg_del(sparx5, sgid) < 0)
+		pr_err("%s:%d Could not delete invalid sgid: %d", __func__,
+		       __LINE__, sgid);
+
+	if (sparx5_psfp_sf_del(sparx5, sfid) < 0)
+		pr_err("%s:%d Could not delete invalid sfid: %d", __func__,
+		       __LINE__, sfid);
+
+	sparx5_isdx_conf_set(sparx5, isdx, 0, 0);
+}
+
+static int sparx5_tc_free_rule_resources(struct net_device *ndev,
+					 struct vcap_control *vctrl,
+					 int rule_id)
+{
+	struct sparx5_port *port = netdev_priv(ndev);
+	struct sparx5 *sparx5 = port->sparx5;
+	struct vcap_rule *vrule;
+	int ret = 0;
+
+	vrule = vcap_get_rule(vctrl, rule_id);
+	if (!vrule || IS_ERR(vrule))
+		return -EINVAL;
+
+	sparx5_tc_free_psfp_resources(sparx5, vrule);
+
+	vcap_free_rule(vrule);
+	return ret;
+}
+
 static int sparx5_tc_flower_destroy(struct net_device *ndev,
 				    struct flow_cls_offload *fco,
 				    struct vcap_admin *admin)
 {
 	struct sparx5_port *port = netdev_priv(ndev);
+	int err = -ENOENT, count = 0, rule_id;
 	struct vcap_control *vctrl;
-	int err = -ENOENT, rule_id;
 
 	vctrl = port->sparx5->vcap_ctrl;
 	while (true) {
 		rule_id = vcap_lookup_rule_by_cookie(vctrl, fco->cookie);
 		if (rule_id <= 0)
 			break;
+		if (count == 0) {
+			/* Resources are attached to the first rule of
+			 * a set of rules. Only works if the rules are
+			 * in the correct order.
+			 */
+			err = sparx5_tc_free_rule_resources(ndev, vctrl,
+							    rule_id);
+			if (err)
+				pr_err("%s:%d: could not free resources %d\n",
+				       __func__, __LINE__, rule_id);
+		}
 		err = vcap_del_rule(vctrl, ndev, rule_id);
 		if (err) {
 			pr_err("%s:%d: could not delete rule %d\n",

drivers/net/ethernet/microchip/vcap/vcap_api.c

@@ -2755,7 +2755,7 @@ int vcap_rule_get_key_u32(struct vcap_rule *rule, enum vcap_key_field key,
 EXPORT_SYMBOL_GPL(vcap_rule_get_key_u32);
 
 /* Find a client action field in a rule */
-static struct vcap_client_actionfield *
+struct vcap_client_actionfield *
 vcap_find_actionfield(struct vcap_rule *rule, enum vcap_action_field act)
 {
 	struct vcap_rule_internal *ri = (struct vcap_rule_internal *)rule;
@@ -2766,6 +2766,7 @@ vcap_find_actionfield(struct vcap_rule *rule, enum vcap_action_field act)
 			return caf;
 	return NULL;
 }
+EXPORT_SYMBOL_GPL(vcap_find_actionfield);
 
 /* Check if the actionfield is already in the rule */
 static bool vcap_actionfield_unique(struct vcap_rule *rule,

drivers/net/ethernet/microchip/vcap/vcap_api_client.h

@@ -268,4 +268,7 @@ int vcap_rule_mod_action_u32(struct vcap_rule *rule,
 /* Get a 32 bit key field value and mask from the rule */
 int vcap_rule_get_key_u32(struct vcap_rule *rule, enum vcap_key_field key,
 			  u32 *value, u32 *mask);
+
+struct vcap_client_actionfield *
+vcap_find_actionfield(struct vcap_rule *rule, enum vcap_action_field act);
 #endif /* __VCAP_API_CLIENT__ */