net: dsa: felix: use vcap policer to set flow meter for psfp

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.

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.

Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index f8d770384344f9eeffcaed7a7ac9cdef28ccef72..eb6c05f29883f937fe76ef6615e0c08751caf0d7 100644 (file)
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -1341,6 +1341,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
@@ -1841,7 +1842,10 @@ static int vsc9959_psfp_filter_add(struct ocelot *ocelot,
        struct felix_stream stream = {0};
        struct felix_stream_gate *sgi;
        struct ocelot_psfp_list *psfp;
+       struct ocelot_policer pol;
        int ret, i, size;
+       u64 rate, burst;
+       u32 index;
 
        psfp = &ocelot->psfp;
 
@@ -1860,13 +1864,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;
+                       break;
                default:
                        return -EOPNOTSUPP;
                }
@@ -1903,6 +1927,9 @@ err:
        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;
 }
 
@@ -1926,6 +1953,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;