OSDN Git Service

net: dsa: felix: add stream gate settings for psfp
authorXiaoliang Yang <xiaoliang.yang_1@nxp.com>
Thu, 18 Nov 2021 10:12:01 +0000 (18:12 +0800)
committerDavid S. Miller <davem@davemloft.net>
Thu, 18 Nov 2021 12:07:24 +0000 (12:07 +0000)
This patch adds stream gate settings for PSFP. Use SGI table to store
stream gate entries. Disable the gate entry when it is not used by any
stream.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>
drivers/net/dsa/ocelot/felix_vsc9959.c

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