--- v4
+++ v7
@@ -1,95 +1,341 @@
-This patch add police action to set flow meter table which is defined
-in IEEE802.1Qci. Flow metering is two rates two buckets and three color
-marker to policing the frames, we only enable one rate one bucket in
-this patch.
+PSFP rules take effect on the streams from any port of VSC9959 switch.
+This patch use ingress port to limit the rule only active on this port.
-Flow metering shares a same policer pool with VCAP policers, so the PSFP
-policer calls ocelot_vcap_policer_add() and ocelot_vcap_policer_del() to
-set flow meter police.
+Each stream can only match two ingress source ports in VSC9959. Streams
+from lowest port gets the configuration of SFID pointed by MAC Table
+lookup and streams from highest port gets the configuration of (SFID+1)
+pointed by MAC Table lookup. This patch defines the PSFP rule on highest
+port as dummy rule, which means that it does not modify the MAC table.
Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
---
- drivers/net/dsa/ocelot/felix_vsc9959.c | 32 +++++++++++++++++++++++++-
- 1 file changed, 31 insertions(+), 1 deletion(-)
+ drivers/net/dsa/ocelot/felix_vsc9959.c | 190 ++++++++++++++++++----
+ drivers/net/ethernet/mscc/ocelot_flower.c | 2 +-
+ include/soc/mscc/ocelot.h | 3 +-
+ 3 files changed, 163 insertions(+), 32 deletions(-)
diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
-index 1418d2a66bd6..1118101d0ee8 100644
+index eb6c05f29883..42ac1952b39a 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
-@@ -1343,6 +1343,7 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
-
- #define VSC9959_PSFP_SFID_MAX 175
- #define VSC9959_PSFP_GATE_ID_MAX 183
-+#define VSC9959_PSFP_POLICER_BASE 63
- #define VSC9959_PSFP_POLICER_MAX 383
- #define VSC9959_PSFP_GATE_LIST_NUM 4
- #define VSC9959_PSFP_GATE_CYCLETIME_MIN 5000
-@@ -1854,7 +1855,10 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
- struct felix_stream stream = {0};
- struct felix_stream_gate *sgi;
+@@ -1349,6 +1349,9 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
+ struct felix_stream {
+ struct list_head list;
+ unsigned long id;
++ bool dummy;
++ int ports;
++ int port;
+ u8 dmac[ETH_ALEN];
+ u16 vid;
+ s8 prio;
+@@ -1363,6 +1366,7 @@ struct felix_stream_filter {
+ refcount_t refcount;
+ u32 index;
+ u8 enable;
++ int portmask;
+ u8 sg_valid;
+ u32 sgid;
+ u8 fm_valid;
+@@ -1505,10 +1509,12 @@ static int vsc9959_stream_table_add(struct ocelot *ocelot,
+
+ memcpy(stream_entry, stream, sizeof(*stream_entry));
+
+- ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack);
+- if (ret) {
+- kfree(stream_entry);
+- return ret;
++ if (!stream->dummy) {
++ ret = vsc9959_mact_stream_set(ocelot, stream_entry, extack);
++ if (ret) {
++ kfree(stream_entry);
++ return ret;
++ }
+ }
+
+ list_add_tail(&stream_entry->list, stream_list);
+@@ -1531,7 +1537,8 @@ vsc9959_stream_table_get(struct list_head *stream_list, unsigned long id)
+ static void vsc9959_stream_table_del(struct ocelot *ocelot,
+ struct felix_stream *stream)
+ {
+- vsc9959_mact_stream_set(ocelot, stream, NULL);
++ if (!stream->dummy)
++ vsc9959_mact_stream_set(ocelot, stream, NULL);
+
+ list_del(&stream->list);
+ kfree(stream);
+@@ -1586,14 +1593,64 @@ static int vsc9959_psfp_sfi_set(struct ocelot *ocelot,
+ 10, 100000);
+ }
+
++static int vsc9959_psfp_sfidmask_set(struct ocelot *ocelot, u32 sfid, int ports)
++{
++ u32 val;
++
++ ocelot_rmw(ocelot,
++ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
++ ANA_TABLES_SFIDTIDX_SFID_INDEX_M,
++ ANA_TABLES_SFIDTIDX);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_SFID_MASK_IGR_PORT_MASK(ports) |
++ ANA_TABLES_SFID_MASK_IGR_SRCPORT_MATCH_ENA,
++ ANA_TABLES_SFID_MASK);
++
++ ocelot_rmw(ocelot,
++ ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE),
++ ANA_TABLES_SFIDACCESS_SFID_TBL_CMD_M,
++ ANA_TABLES_SFIDACCESS);
++
++ return readx_poll_timeout(vsc9959_sfi_access_status, ocelot, val,
++ (!ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(val)),
++ 10, 100000);
++}
++
++static int vsc9959_psfp_sfi_list_add(struct ocelot *ocelot,
++ struct felix_stream_filter *sfi,
++ struct list_head *pos)
++{
++ struct felix_stream_filter *sfi_entry;
++ int 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);
++
++ ret = vsc9959_psfp_sfi_set(ocelot, sfi_entry);
++ if (ret) {
++ kfree(sfi_entry);
++ return ret;
++ }
++
++ vsc9959_psfp_sfidmask_set(ocelot, sfi->index, sfi->portmask);
++
++ list_add(&sfi_entry->list, pos);
++
++ return 0;
++}
++
+ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot,
+ struct felix_stream_filter *sfi)
+ {
+- struct felix_stream_filter *sfi_entry, *tmp;
+ struct list_head *pos, *q, *last;
++ struct felix_stream_filter *tmp;
struct ocelot_psfp_list *psfp;
-+ struct ocelot_policer pol;
- int ret, i, size;
-+ u64 rate, burst;
-+ u32 index;
+ u32 insert = 0;
+- int ret;
psfp = &ocelot->psfp;
-
-@@ -1873,13 +1877,33 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
- ret = vsc9959_psfp_sgi_table_add(ocelot, sgi);
- if (ret) {
- kfree(sgi);
-- return ret;
-+ goto err;
- }
- sfi.sg_valid = 1;
- sfi.sgid = sgi->index;
- kfree(sgi);
- break;
- case FLOW_ACTION_POLICE:
-+ index = a->police.index + VSC9959_PSFP_POLICER_BASE;
-+ if (index > VSC9959_PSFP_POLICER_MAX) {
-+ ret = -EINVAL;
-+ goto err;
-+ }
-+
-+ rate = a->police.rate_bytes_ps;
-+ burst = rate * PSCHED_NS2TICKS(a->police.burst);
-+ pol = (struct ocelot_policer) {
-+ .burst = div_u64(burst, PSCHED_TICKS_PER_SEC),
-+ .rate = div_u64(rate, 1000) * 8,
-+ };
-+ ret = ocelot_vcap_policer_add(ocelot, index, &pol);
-+ if (ret)
-+ goto err;
-+
-+ sfi.fm_valid = 1;
-+ sfi.fmid = index;
-+ sfi.maxsdu = a->police.mtu;
+ last = &psfp->sfi_list;
+@@ -1602,6 +1659,7 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot,
+ tmp = list_entry(pos, struct felix_stream_filter, list);
+ if (sfi->sg_valid == tmp->sg_valid &&
+ sfi->fm_valid == tmp->fm_valid &&
++ sfi->portmask == tmp->portmask &&
+ tmp->sgid == sfi->sgid &&
+ tmp->fmid == sfi->fmid) {
+ sfi->index = tmp->index;
+@@ -1616,22 +1674,40 @@ static int vsc9959_psfp_sfi_table_add(struct ocelot *ocelot,
+ }
+ sfi->index = insert;
+
+- sfi_entry = kzalloc(sizeof(*sfi_entry), GFP_KERNEL);
+- if (!sfi_entry)
+- return -ENOMEM;
++ return vsc9959_psfp_sfi_list_add(ocelot, sfi, last);
++}
+
+- memcpy(sfi_entry, sfi, sizeof(*sfi_entry));
+- refcount_set(&sfi_entry->refcount, 1);
++static int vsc9959_psfp_sfi_table_add2(struct ocelot *ocelot,
++ struct felix_stream_filter *sfi,
++ struct felix_stream_filter *sfi2)
++{
++ struct felix_stream_filter *tmp;
++ struct list_head *pos, *q, *last;
++ struct ocelot_psfp_list *psfp;
++ u32 insert = 0;
++ int ret;
+
+- ret = vsc9959_psfp_sfi_set(ocelot, sfi_entry);
+- if (ret) {
+- kfree(sfi_entry);
+- return ret;
++ psfp = &ocelot->psfp;
++ last = &psfp->sfi_list;
++
++ list_for_each_safe(pos, q, &psfp->sfi_list) {
++ tmp = list_entry(pos, struct felix_stream_filter, list);
++ /* Make sure that the index is increasing in order. */
++ if (tmp->index >= insert + 2)
+ break;
- default:
- return -EOPNOTSUPP;
++
++ insert = tmp->index + 1;
++ last = pos;
+ }
++ sfi->index = insert;
++
++ ret = vsc9959_psfp_sfi_list_add(ocelot, sfi, last);
++ if (ret)
++ return ret;
+
+- list_add(&sfi_entry->list, last);
++ sfi2->index = insert + 1;
+
+- return 0;
++ return vsc9959_psfp_sfi_list_add(ocelot, sfi2, last->next);
+ }
+
+ static struct felix_stream_filter *
+@@ -1832,10 +1908,11 @@ static void vsc9959_psfp_counters_get(struct ocelot *ocelot, u32 index,
+ SYS_STAT_CFG);
+ }
+
+-static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
++static int vsc9959_psfp_filter_add(struct ocelot *ocelot, int port,
+ struct flow_cls_offload *f)
+ {
+ struct netlink_ext_ack *extack = f->common.extack;
++ struct felix_stream_filter old_sfi, *sfi_entry;
+ struct felix_stream_filter sfi = {0};
+ const struct flow_action_entry *a;
+ struct felix_stream *stream_entry;
+@@ -1896,21 +1973,61 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
}
-@@ -1916,6 +1940,9 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
- if (sfi.sg_valid)
- vsc9959_psfp_sgi_table_del(ocelot, sfi.sgid);
-
-+ if (sfi.fm_valid)
-+ ocelot_vcap_policer_del(ocelot, sfi.fmid);
-+
- return ret;
+ }
+
+- /* Check if stream is set. */
+- stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream);
+- if (stream_entry) {
+- NL_SET_ERR_MSG_MOD(extack, "This stream is already added");
+- ret = -EEXIST;
+- goto err;
+- }
++ stream.ports = BIT(port);
++ stream.port = port;
+
++ sfi.portmask = stream.ports;
+ sfi.prio_valid = (stream.prio < 0 ? 0 : 1);
+ sfi.prio = (sfi.prio_valid ? stream.prio : 0);
+ sfi.enable = 1;
+
+- ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi);
+- if (ret)
+- goto err;
++ /* Check if stream is set. */
++ stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &stream);
++ if (stream_entry) {
++ if (stream_entry->ports & BIT(port)) {
++ NL_SET_ERR_MSG_MOD(extack,
++ "The stream is added on this port");
++ ret = -EEXIST;
++ goto err;
++ }
++
++ if (stream_entry->ports != BIT(stream_entry->port)) {
++ NL_SET_ERR_MSG_MOD(extack,
++ "The stream is added on two ports");
++ ret = -EEXIST;
++ goto err;
++ }
++
++ stream_entry->ports |= BIT(port);
++ stream.ports = stream_entry->ports;
++
++ sfi_entry = vsc9959_psfp_sfi_table_get(&psfp->sfi_list,
++ stream_entry->sfid);
++ memcpy(&old_sfi, sfi_entry, sizeof(old_sfi));
++
++ vsc9959_psfp_sfi_table_del(ocelot, stream_entry->sfid);
++
++ old_sfi.portmask = stream_entry->ports;
++ sfi.portmask = stream.ports;
++
++ if (stream_entry->port > port) {
++ ret = vsc9959_psfp_sfi_table_add2(ocelot, &sfi,
++ &old_sfi);
++ stream_entry->dummy = true;
++ } else {
++ ret = vsc9959_psfp_sfi_table_add2(ocelot, &old_sfi,
++ &sfi);
++ stream.dummy = true;
++ }
++ if (ret)
++ goto err;
++
++ stream_entry->sfid = old_sfi.index;
++ } else {
++ ret = vsc9959_psfp_sfi_table_add(ocelot, &sfi);
++ if (ret)
++ goto err;
++ }
+
+ stream.sfid = sfi.index;
+ stream.sfid_valid = 1;
+@@ -1936,9 +2053,9 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
+ static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
+ struct flow_cls_offload *f)
+ {
++ struct felix_stream *stream, tmp, *stream_entry;
+ static struct felix_stream_filter *sfi;
+ struct ocelot_psfp_list *psfp;
+- struct felix_stream *stream;
+
+ psfp = &ocelot->psfp;
+
+@@ -1958,9 +2075,22 @@ static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
+
+ vsc9959_psfp_sfi_table_del(ocelot, stream->sfid);
+
++ memcpy(&tmp, stream, sizeof(tmp));
++
+ stream->sfid_valid = 0;
+ vsc9959_stream_table_del(ocelot, stream);
+
++ stream_entry = vsc9959_stream_table_lookup(&psfp->stream_list, &tmp);
++ if (stream_entry) {
++ stream_entry->ports = BIT(stream_entry->port);
++ if (stream_entry->dummy) {
++ stream_entry->dummy = false;
++ vsc9959_mact_stream_set(ocelot, stream_entry, NULL);
++ }
++ vsc9959_psfp_sfidmask_set(ocelot, stream_entry->sfid,
++ stream_entry->ports);
++ }
++
+ return 0;
}
-@@ -1939,6 +1966,9 @@ static int vsc9959_psfp_filter_del(struct ocelot *ocelot,
- if (sfi->sg_valid)
- vsc9959_psfp_sgi_table_del(ocelot, sfi->sgid);
-
-+ if (sfi->fm_valid)
-+ ocelot_vcap_policer_del(ocelot, sfi->fmid);
-+
- vsc9959_psfp_sfi_table_del(ocelot, stream->sfid);
-
- stream->sfid_valid = 0;
+diff --git a/drivers/net/ethernet/mscc/ocelot_flower.c b/drivers/net/ethernet/mscc/ocelot_flower.c
+index b54b52fd9e1b..58fce173f95b 100644
+--- a/drivers/net/ethernet/mscc/ocelot_flower.c
++++ b/drivers/net/ethernet/mscc/ocelot_flower.c
+@@ -837,7 +837,7 @@ int ocelot_cls_flower_replace(struct ocelot *ocelot, int port,
+ if (filter->type == OCELOT_PSFP_FILTER_OFFLOAD) {
+ kfree(filter);
+ if (ocelot->ops->psfp_filter_add)
+- return ocelot->ops->psfp_filter_add(ocelot, f);
++ return ocelot->ops->psfp_filter_add(ocelot, port, f);
+
+ NL_SET_ERR_MSG_MOD(extack, "PSFP chain is not supported in HW");
+ return -EOPNOTSUPP;
+diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
+index 2a41685b5c7d..89d17629efe5 100644
+--- a/include/soc/mscc/ocelot.h
++++ b/include/soc/mscc/ocelot.h
+@@ -556,7 +556,8 @@ struct ocelot_ops {
+ u16 (*wm_dec)(u16 value);
+ void (*wm_stat)(u32 val, u32 *inuse, u32 *maxuse);
+ void (*psfp_init)(struct ocelot *ocelot);
+- int (*psfp_filter_add)(struct ocelot *ocelot, struct flow_cls_offload *f);
++ int (*psfp_filter_add)(struct ocelot *ocelot, int port,
++ struct flow_cls_offload *f);
+ int (*psfp_filter_del)(struct ocelot *ocelot, struct flow_cls_offload *f);
+ int (*psfp_stats_get)(struct ocelot *ocelot, struct flow_cls_offload *f,
+ struct flow_stats *stats);
--
2.17.1