From: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
Date: Thu, 18 Nov 2021 18:12:04 +0800
Subject: net: dsa: felix: restrict psfp rules on ingress port
Patch-mainline: v5.17-rc1
Git-commit: a7e13edf37beee65f2c2ec60c42e5fb89a2958ce
References: jsc#PED-1549
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.
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>
Signed-off-by: David S. Miller <davem@davemloft.net>
Acked-by: Thomas Bogendoerfer <tbogendoerfer@suse.de>
---
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(-)
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -1351,6 +1351,9 @@ static int vsc9959_port_setup_tc(struct
struct felix_stream {
struct list_head list;
unsigned long id;
+ bool dummy;
+ int ports;
+ int port;
u8 dmac[ETH_ALEN];
u16 vid;
s8 prio;
@@ -1365,6 +1368,7 @@ struct felix_stream_filter {
refcount_t refcount;
u32 index;
u8 enable;
+ int portmask;
u8 sg_valid;
u32 sgid;
u8 fm_valid;
@@ -1507,10 +1511,12 @@ static int vsc9959_stream_table_add(stru
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);
@@ -1533,7 +1539,8 @@ vsc9959_stream_table_get(struct list_hea
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);
@@ -1588,14 +1595,64 @@ static int vsc9959_psfp_sfi_set(struct o
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;
u32 insert = 0;
- int ret;
psfp = &ocelot->psfp;
last = &psfp->sfi_list;
@@ -1604,6 +1661,7 @@ static int vsc9959_psfp_sfi_table_add(st
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;
@@ -1618,22 +1676,40 @@ static int vsc9959_psfp_sfi_table_add(st
}
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;
+
+ insert = tmp->index + 1;
+ last = pos;
}
+ sfi->index = insert;
- list_add(&sfi_entry->list, last);
+ ret = vsc9959_psfp_sfi_list_add(ocelot, sfi, last);
+ if (ret)
+ return ret;
- return 0;
+ sfi2->index = insert + 1;
+
+ return vsc9959_psfp_sfi_list_add(ocelot, sfi2, last->next);
}
static struct felix_stream_filter *
@@ -1834,10 +1910,11 @@ static void vsc9959_psfp_counters_get(st
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;
@@ -1898,21 +1975,61 @@ static int vsc9959_psfp_filter_add(struc
}
}
- /* 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;
@@ -1938,9 +2055,9 @@ err:
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;
@@ -1960,9 +2077,22 @@ static int vsc9959_psfp_filter_del(struc
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;
}
--- 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 oce
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;
--- 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);