Merge tag 'net-5.15-rc7' of git://git.kernel.org/pub/scm/linux/kernel/git/netdev/net

[linux-2.6-microblaze.git] / net / dsa / tag_sja1105.c

diff --git a/net/dsa/tag_sja1105.c b/net/dsa/tag_sja1105.c
index c054f48..2edede9 100644 (file)
--- a/net/dsa/tag_sja1105.c
+++ b/net/dsa/tag_sja1105.c
@@ -4,6 +4,7 @@
 #include <linux/if_vlan.h>
 #include <linux/dsa/sja1105.h>
 #include <linux/dsa/8021q.h>
+#include <linux/skbuff.h>
 #include <linux/packing.h>
 #include "dsa_priv.h"
 
@@ -53,6 +54,11 @@
 #define SJA1110_TX_TRAILER_LEN                 4
 #define SJA1110_MAX_PADDING_LEN                        15
 
+enum sja1110_meta_tstamp {
+       SJA1110_META_TSTAMP_TX = 0,
+       SJA1110_META_TSTAMP_RX = 1,
+};
+
 /* Similar to is_link_local_ether_addr(hdr->h_dest) but also covers PTP */
 static inline bool sja1105_is_link_local(const struct sk_buff *skb)
 {
@@ -520,6 +526,43 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
                                              is_meta);
 }
 
+static void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port,
+                                       u8 ts_id, enum sja1110_meta_tstamp dir,
+                                       u64 tstamp)
+{
+       struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
+       struct dsa_port *dp = dsa_to_port(ds, port);
+       struct skb_shared_hwtstamps shwt = {0};
+       struct sja1105_port *sp = dp->priv;
+
+       if (!dsa_port_is_sja1105(dp))
+               return;
+
+       /* We don't care about RX timestamps on the CPU port */
+       if (dir == SJA1110_META_TSTAMP_RX)
+               return;
+
+       spin_lock(&sp->data->skb_txtstamp_queue.lock);
+
+       skb_queue_walk_safe(&sp->data->skb_txtstamp_queue, skb, skb_tmp) {
+               if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
+                       continue;
+
+               __skb_unlink(skb, &sp->data->skb_txtstamp_queue);
+               skb_match = skb;
+
+               break;
+       }
+
+       spin_unlock(&sp->data->skb_txtstamp_queue.lock);
+
+       if (WARN_ON(!skb_match))
+               return;
+
+       shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
+       skb_complete_tx_timestamp(skb_match, &shwt);
+}
+
 static struct sk_buff *sja1110_rcv_meta(struct sk_buff *skb, u16 rx_header)
 {
        u8 *buf = dsa_etype_header_pos_rx(skb) + SJA1110_HEADER_LEN;