net: mscc: ocelot: cross-check the sequence id from the timestamp FIFO with the skb...

[platform/kernel/linux-rpi.git] / drivers / net / ethernet / mscc / ocelot.c

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 3b1f0bb..f004432 100644 (file)
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -675,6 +675,7 @@ int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
                        return err;
 
                OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
+               OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
        }
 
        return 0;
@@ -708,6 +709,17 @@ static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
        spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 }
 
+static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
+{
+       struct ptp_header *hdr;
+
+       hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
+       if (WARN_ON(!hdr))
+               return false;
+
+       return seqid == ntohs(hdr->sequence_id);
+}
+
 void ocelot_get_txtstamp(struct ocelot *ocelot)
 {
        int budget = OCELOT_PTP_QUEUE_SZ;
@@ -715,10 +727,10 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
        while (budget--) {
                struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
                struct skb_shared_hwtstamps shhwtstamps;
+               u32 val, id, seqid, txport;
                struct ocelot_port *port;
                struct timespec64 ts;
                unsigned long flags;
-               u32 val, id, txport;
 
                val = ocelot_read(ocelot, SYS_PTP_STATUS);
 
@@ -731,6 +743,7 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
                /* Retrieve the ts ID and Tx port */
                id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
                txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
+               seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
 
                port = ocelot->ports[txport];
 
@@ -740,6 +753,7 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
                spin_unlock(&ocelot->ts_id_lock);
 
                /* Retrieve its associated skb */
+try_again:
                spin_lock_irqsave(&port->tx_skbs.lock, flags);
 
                skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
@@ -755,6 +769,14 @@ void ocelot_get_txtstamp(struct ocelot *ocelot)
                if (WARN_ON(!skb_match))
                        continue;
 
+               if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
+                       dev_err_ratelimited(ocelot->dev,
+                                           "port %d received stale TX timestamp for seqid %d, discarding\n",
+                                           txport, seqid);
+                       dev_kfree_skb_any(skb);
+                       goto try_again;
+               }
+
                /* Get the h/w timestamp */
                ocelot_get_hwtimestamp(ocelot, &ts);