[PATCH net-next 2/2] net: stmmac: add RX frame steering based on VLAN priority in tc flower

[Ong Boon Leong]

We extend tc flower to support configuration of VLAN priority-based RX
frame steering hardware offloading.

To map VLAN <PCP> to Traffic Class <TC>:
  $ tc filter add dev <IFNAME> parent ffff: protocol 802.1Q flower \
       vlan_prio <PCP> hw_tc <TC>

  Note: <TC> < N whereby "tc qdisc ... num_tc N ..."

To delete all tc flower configurations:
  $ tc qdisc delete dev <IFNAME> ingress

Signed-off-by: Ong Boon Leong <boon.leong....@intel.com>
---
 .../net/ethernet/stmicro/stmmac/stmmac_tc.c   | 65 ++++++++++++++++++-
 1 file changed, 63 insertions(+), 2 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c 
b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
index f4d8d7980ec5..b80cb2985b39 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
@@ -598,14 +598,73 @@ static int tc_del_flow(struct stmmac_priv *priv,
        return ret;
 }
 
+#define VLAN_PRIO_FULL_MASK (0x07)
+
+static int tc_add_vlan_flow(struct stmmac_priv *priv,
+                           struct flow_cls_offload *cls)
+{
+       struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
+       struct flow_dissector *dissector = rule->match.dissector;
+       int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
+       struct flow_match_vlan match;
+
+       /* Nothing to do here */
+       if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN))
+               return -EINVAL;
+
+       if (tc < 0) {
+               netdev_err(priv->dev, "Invalid traffic class\n");
+               return -EINVAL;
+       }
+
+       flow_rule_match_vlan(rule, &match);
+
+       if (match.mask->vlan_priority) {
+               u32 prio;
+
+               if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) {
+                       netdev_err(priv->dev, "Only full mask is supported for 
VLAN priority");
+                       return -EINVAL;
+               }
+
+               prio = BIT(match.key->vlan_priority);
+               stmmac_rx_queue_prio(priv, priv->hw, prio, tc);
+       }
+
+       return 0;
+}
+
+static int tc_del_vlan_flow(struct stmmac_priv *priv,
+                           struct flow_cls_offload *cls)
+{
+       struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
+       struct flow_dissector *dissector = rule->match.dissector;
+       int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
+
+       /* Nothing to do here */
+       if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN))
+               return -EINVAL;
+
+       if (tc < 0) {
+               netdev_err(priv->dev, "Invalid traffic class\n");
+               return -EINVAL;
+       }
+
+       stmmac_rx_queue_prio(priv, priv->hw, 0, tc);
+
+       return 0;
+}
+
 static int tc_add_flow_cls(struct stmmac_priv *priv,
                           struct flow_cls_offload *cls)
 {
        int ret;
 
        ret = tc_add_flow(priv, cls);
+       if (!ret)
+               return ret;
 
-       return ret;
+       return tc_add_vlan_flow(priv, cls);
 }
 
 static int tc_del_flow_cls(struct stmmac_priv *priv,
@@ -614,8 +673,10 @@ static int tc_del_flow_cls(struct stmmac_priv *priv,
        int ret;
 
        ret = tc_del_flow(priv, cls);
+       if (!ret)
+               return ret;
 
-       return ret;
+       return tc_del_vlan_flow(priv, cls);
 }
 
 static int tc_setup_cls(struct stmmac_priv *priv,
-- 
2.25.1