diff options
| -rw-r--r-- | drivers/net/ethernet/mscc/ocelot.c | 24 | ||||
| -rw-r--r-- | include/soc/mscc/ocelot.h | 1 |
2 files changed, 24 insertions, 1 deletions
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c index 3b1f0bb6a414..f0044329e3d7 100644 --- 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); diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h index b0ece85d9a76..cabacef8731c 100644 --- a/include/soc/mscc/ocelot.h +++ b/include/soc/mscc/ocelot.h @@ -697,6 +697,7 @@ struct ocelot_policer { struct ocelot_skb_cb { struct sk_buff *clone; + unsigned int ptp_class; /* valid only for clones */ u8 ptp_cmd; u8 ts_id; }; |