[v2, 4/4] ocelot: add VCAP IS2 rule to trap PTP Ethernet frames

[Yangbo Lu]

All the PTP messages over Ethernet have etype 0x88f7 on them.
Use etype as the key to trap PTP messages.

Signed-off-by: Yangbo Lu <yangbo...@nxp.com>
---
Changes for v2:
        - Added this patch.
---
 drivers/net/ethernet/mscc/ocelot.c | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/drivers/net/ethernet/mscc/ocelot.c 
b/drivers/net/ethernet/mscc/ocelot.c
index 6932e61..40f4e0d 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -1681,6 +1681,33 @@ int ocelot_probe_port(struct ocelot *ocelot, u8 port,
 }
 EXPORT_SYMBOL(ocelot_probe_port);
 
+static int ocelot_ace_add_ptp_rule(struct ocelot *ocelot)
+{
+       struct ocelot_ace_rule *rule;
+
+       rule = kzalloc(sizeof(*rule), GFP_KERNEL);
+       if (!rule)
+               return -ENOMEM;
+
+       /* Entry for PTP over Ethernet (etype 0x88f7)
+        * Action: trap to CPU port
+        */
+       rule->ocelot = ocelot;
+       rule->prio = 1;
+       rule->type = OCELOT_ACE_TYPE_ETYPE;
+       /* Available on all ingress port except CPU port */
+       rule->ingress_port = ~BIT(ocelot->num_phys_ports);
+       rule->dmac_mc = OCELOT_VCAP_BIT_1;
+       rule->frame.etype.etype.value[0] = 0x88;
+       rule->frame.etype.etype.value[1] = 0xf7;
+       rule->frame.etype.etype.mask[0] = 0xff;
+       rule->frame.etype.etype.mask[1] = 0xff;
+       rule->action = OCELOT_ACL_ACTION_TRAP;
+
+       ocelot_ace_rule_offload_add(rule);
+       return 0;
+}
+
 int ocelot_init(struct ocelot *ocelot)
 {
        u32 port;
@@ -1708,6 +1735,7 @@ int ocelot_init(struct ocelot *ocelot)
        ocelot_mact_init(ocelot);
        ocelot_vlan_init(ocelot);
        ocelot_ace_init(ocelot);
+       ocelot_ace_add_ptp_rule(ocelot);
 
        for (port = 0; port < ocelot->num_phys_ports; port++) {
                /* Clear all counters (5 groups) */
-- 
2.7.4