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

[Yangbo Lu <yangbo.lu@nxp.com>]

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.lu@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