Re: [RFC PATCH 2/3] net: macb: Add support for 1588 for Zynq Ultrascale+ MPSoC

+static inline void macb_handle_txtstamp(struct macb *bp, struct sk_buff *skb,
+                                       struct macb_dma_desc *desc)
+{
+       u32 ts_s, ts_ns;
+       u8 msg_type;
+
+       skb_copy_from_linear_data_offset(skb, GEM_TX_PTPHDR_OFFSET,
+                                        &msg_type, 1);
+
+       /* Bit[32:6] of TS secs from register
+        * Bit[5:0] of TS secs from BD
+        * TS nano secs is available in BD
+        */
+       if (msg_type & 0x2) {
+               /* PTP Peer Event Frame packets */
+               ts_s = (gem_readl(bp, 1588PEERTXSEC) & GEM_SEC_MASK) |
+                      ((desc->tsl >> GEM_TSL_SEC_RS) |
+                      (desc->tsh << GEM_TSH_SEC_LS));
+               ts_ns = desc->tsl & GEM_TSL_NSEC_MASK;
+       } else {
+               /* PTP Event Frame packets */
+               ts_s = (gem_readl(bp, 1588TXSEC) & GEM_SEC_MASK) |
+                      ((desc->tsl >> GEM_TSL_SEC_RS) |
+                      (desc->tsh << GEM_TSH_SEC_LS));
+               ts_ns = desc->tsl & GEM_TSL_NSEC_MASK;
+       }
+
+       struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
+
+       memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
+       shhwtstamps->hwtstamp = ns_to_ktime((ts_s * NS_PER_SEC) + ts_ns);
+       skb_tstamp_tx(skb, skb_hwtstamps(skb));
+}
+
static void macb_tx_interrupt(struct macb_queue *queue)
{
       unsigned int tail;

@@ -703,6 +745,10 @@ static void macb_tx_interrupt(struct macb_queue *queue)
                               bp->stats.tx_bytes += skb->len;
                       }

+#ifdef CONFIG_MACB_EXT_BD
+                       if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
+                               macb_handle_txtstamp(bp, skb, desc);
+#endif
                       /* Now we can safely release resources */
                       macb_tx_unmap(bp, tx_skb);

@@ -796,6 +842,39 @@ static void discard_partial_frame(struct macb *bp,

unsigned int begin,
        */
}