net: ethernet: ti: am65-cpts: Enable RX HW timestamp for PTP packets using CPTS FIFO

Add a new function "am65_cpts_rx_timestamp()" which checks for PTP
packets from header and timestamps them.

Add another function "am65_cpts_find_rx_ts()" which finds CPTS FIFO
Event to get the timestamp of received PTP packet.

Signed-off-by: Chintan Vankar <c-vankar@ti.com>
Signed-off-by: Paolo Abeni <pabeni@redhat.com>

diff --git a/drivers/net/ethernet/ti/am65-cpts.c b/drivers/net/ethernet/ti/am65-cpts.c
index c66618d91c28fe2bdf9886b3c476bdb014cd5fe5..599454c1d19f434178f2fd83637e6cbc40d2ccb0 100644 (file)
--- a/drivers/net/ethernet/ti/am65-cpts.c
+++ b/drivers/net/ethernet/ti/am65-cpts.c
@@ -275,15 +275,13 @@ static bool am65_cpts_fifo_pop_event(struct am65_cpts *cpts,
        return true;
 }
 
-static int am65_cpts_fifo_read(struct am65_cpts *cpts)
+static int __am65_cpts_fifo_read(struct am65_cpts *cpts)
 {
        struct ptp_clock_event pevent;
        struct am65_cpts_event *event;
        bool schedule = false;
        int i, type, ret = 0;
-       unsigned long flags;
 
-       spin_lock_irqsave(&cpts->lock, flags);
        for (i = 0; i < AM65_CPTS_FIFO_DEPTH; i++) {
                event = list_first_entry_or_null(&cpts->pool,
                                                 struct am65_cpts_event, list);
@@ -312,8 +310,7 @@ static int am65_cpts_fifo_read(struct am65_cpts *cpts)
                        event->tmo = jiffies +
                                msecs_to_jiffies(AM65_CPTS_EVENT_RX_TX_TIMEOUT);
 
-                       list_del_init(&event->list);
-                       list_add_tail(&event->list, &cpts->events);
+                       list_move_tail(&event->list, &cpts->events);
 
                        dev_dbg(cpts->dev,
                                "AM65_CPTS_EV_TX e1:%08x e2:%08x t:%lld\n",
@@ -356,14 +353,24 @@ static int am65_cpts_fifo_read(struct am65_cpts *cpts)
        }
 
 out:
-       spin_unlock_irqrestore(&cpts->lock, flags);
-
        if (schedule)
                ptp_schedule_worker(cpts->ptp_clock, 0);
 
        return ret;
 }
 
+static int am65_cpts_fifo_read(struct am65_cpts *cpts)
+{
+       unsigned long flags;
+       int ret = 0;
+
+       spin_lock_irqsave(&cpts->lock, flags);
+       ret = __am65_cpts_fifo_read(cpts);
+       spin_unlock_irqrestore(&cpts->lock, flags);
+
+       return ret;
+}
+
 static u64 am65_cpts_gettime(struct am65_cpts *cpts,
                             struct ptp_system_timestamp *sts)
 {
@@ -906,6 +913,69 @@ static int am65_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid)
        return 1;
 }
 
+static u64 am65_cpts_find_rx_ts(struct am65_cpts *cpts, u32 skb_mtype_seqid)
+{
+       struct list_head *this, *next;
+       struct am65_cpts_event *event;
+       unsigned long flags;
+       u32 mtype_seqid;
+       u64 ns = 0;
+
+       spin_lock_irqsave(&cpts->lock, flags);
+       __am65_cpts_fifo_read(cpts);
+       list_for_each_safe(this, next, &cpts->events) {
+               event = list_entry(this, struct am65_cpts_event, list);
+               if (time_after(jiffies, event->tmo)) {
+                       list_move(&event->list, &cpts->pool);
+                       continue;
+               }
+
+               mtype_seqid = event->event1 &
+                             (AM65_CPTS_EVENT_1_MESSAGE_TYPE_MASK |
+                              AM65_CPTS_EVENT_1_SEQUENCE_ID_MASK |
+                              AM65_CPTS_EVENT_1_EVENT_TYPE_MASK);
+
+               if (mtype_seqid == skb_mtype_seqid) {
+                       ns = event->timestamp;
+                       list_move(&event->list, &cpts->pool);
+                       break;
+               }
+       }
+       spin_unlock_irqrestore(&cpts->lock, flags);
+
+       return ns;
+}
+
+void am65_cpts_rx_timestamp(struct am65_cpts *cpts, struct sk_buff *skb)
+{
+       struct am65_cpts_skb_cb_data *skb_cb = (struct am65_cpts_skb_cb_data *)skb->cb;
+       struct skb_shared_hwtstamps *ssh;
+       int ret;
+       u64 ns;
+
+       /* am65_cpts_rx_timestamp() is called before eth_type_trans(), so
+        * skb MAC Hdr properties are not configured yet. Hence need to
+        * reset skb MAC header here
+        */
+       skb_reset_mac_header(skb);
+       ret = am65_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
+       if (!ret)
+               return; /* if not PTP class packet */
+
+       skb_cb->skb_mtype_seqid |= (AM65_CPTS_EV_RX << AM65_CPTS_EVENT_1_EVENT_TYPE_SHIFT);
+
+       dev_dbg(cpts->dev, "%s mtype seqid %08x\n", __func__, skb_cb->skb_mtype_seqid);
+
+       ns = am65_cpts_find_rx_ts(cpts, skb_cb->skb_mtype_seqid);
+       if (!ns)
+               return;
+
+       ssh = skb_hwtstamps(skb);
+       memset(ssh, 0, sizeof(*ssh));
+       ssh->hwtstamp = ns_to_ktime(ns);
+}
+EXPORT_SYMBOL_GPL(am65_cpts_rx_timestamp);
+
 /**
  * am65_cpts_tx_timestamp - save tx packet for timestamping
  * @cpts: cpts handle

diff --git a/drivers/net/ethernet/ti/am65-cpts.h b/drivers/net/ethernet/ti/am65-cpts.h
index 6e14df0be1137340e4e56511cd7f0482832b84bc..90296968a75cfc54c88388ea01925ec6f92df75c 100644 (file)
--- a/drivers/net/ethernet/ti/am65-cpts.h
+++ b/drivers/net/ethernet/ti/am65-cpts.h
@@ -22,6 +22,7 @@ void am65_cpts_release(struct am65_cpts *cpts);
 struct am65_cpts *am65_cpts_create(struct device *dev, void __iomem *regs,
                                   struct device_node *node);
 int am65_cpts_phc_index(struct am65_cpts *cpts);
+void am65_cpts_rx_timestamp(struct am65_cpts *cpts, struct sk_buff *skb);
 void am65_cpts_tx_timestamp(struct am65_cpts *cpts, struct sk_buff *skb);
 void am65_cpts_prep_tx_timestamp(struct am65_cpts *cpts, struct sk_buff *skb);
 void am65_cpts_rx_enable(struct am65_cpts *cpts, bool en);
@@ -48,6 +49,11 @@ static inline int am65_cpts_phc_index(struct am65_cpts *cpts)
        return -1;
 }
 
+static inline void am65_cpts_rx_timestamp(struct am65_cpts *cpts,
+                                         struct sk_buff *skb)
+{
+}
+
 static inline void am65_cpts_tx_timestamp(struct am65_cpts *cpts,
                                          struct sk_buff *skb)
 {