net: ethernet: ti: cpts: move tc mult update in cpts_fifo_read()

[linux-2.6-microblaze.git] / drivers / net / ethernet / ti / cpts.c

diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c
index 729ce09..a2974b5 100644 (file)
--- a/drivers/net/ethernet/ti/cpts.c
+++ b/drivers/net/ethernet/ti/cpts.c
@@ -71,7 +71,7 @@ static int cpts_purge_events(struct cpts *cpts)
        }
 
        if (removed)
-               pr_debug("cpts: event pool cleaned up %d\n", removed);
+               dev_dbg(cpts->dev, "cpts: event pool cleaned up %d\n", removed);
        return removed ? 0 : -1;
 }
 
@@ -112,10 +112,8 @@ static bool cpts_match_tx_ts(struct cpts *cpts, struct cpts_event *event)
                                        (struct cpts_skb_cb_data *)skb->cb;
 
                if (cpts_match(skb, class, seqid, mtype)) {
-                       u64 ns = timecounter_cyc2time(&cpts->tc, event->low);
-
                        memset(&ssh, 0, sizeof(ssh));
-                       ssh.hwtstamp = ns_to_ktime(ns);
+                       ssh.hwtstamp = ns_to_ktime(event->timestamp);
                        skb_tstamp_tx(skb, &ssh);
                        found = true;
                        __skb_unlink(skb, &cpts->txq);
@@ -150,7 +148,7 @@ static int cpts_fifo_read(struct cpts *cpts, int match)
                        break;
 
                if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) {
-                       pr_err("cpts: event pool empty\n");
+                       dev_warn(cpts->dev, "cpts: event pool empty\n");
                        return -1;
                }
 
@@ -158,8 +156,20 @@ static int cpts_fifo_read(struct cpts *cpts, int match)
                event->tmo = jiffies + 2;
                event->high = hi;
                event->low = lo;
+               event->timestamp = timecounter_cyc2time(&cpts->tc, event->low);
                type = event_type(event);
+
+               dev_dbg(cpts->dev, "CPTS_EV: %d high:%08X low:%08x\n",
+                       type, event->high, event->low);
                switch (type) {
+               case CPTS_EV_PUSH:
+                       WRITE_ONCE(cpts->cur_timestamp, lo);
+                       timecounter_read(&cpts->tc);
+                       if (cpts->mult_new) {
+                               cpts->cc.mult = cpts->mult_new;
+                               cpts->mult_new = 0;
+                       }
+                       break;
                case CPTS_EV_TX:
                        if (cpts_match_tx_ts(cpts, event)) {
                                /* if the new event matches an existing skb,
@@ -168,7 +178,6 @@ static int cpts_fifo_read(struct cpts *cpts, int match)
                                break;
                        }
                        /* fall through */
-               case CPTS_EV_PUSH:
                case CPTS_EV_RX:
                        list_del_init(&event->list);
                        list_add_tail(&event->list, &cpts->events);
@@ -178,7 +187,7 @@ static int cpts_fifo_read(struct cpts *cpts, int match)
                case CPTS_EV_HW:
                        break;
                default:
-                       pr_err("cpts: unknown event type\n");
+                       dev_err(cpts->dev, "cpts: unknown event type\n");
                        break;
                }
                if (type == match)
@@ -189,26 +198,17 @@ static int cpts_fifo_read(struct cpts *cpts, int match)
 
 static u64 cpts_systim_read(const struct cyclecounter *cc)
 {
-       u64 val = 0;
-       struct cpts_event *event;
-       struct list_head *this, *next;
        struct cpts *cpts = container_of(cc, struct cpts, cc);
 
-       cpts_write32(cpts, TS_PUSH, ts_push);
-       if (cpts_fifo_read(cpts, CPTS_EV_PUSH))
-               pr_err("cpts: unable to obtain a time stamp\n");
+       return READ_ONCE(cpts->cur_timestamp);
+}
 
-       list_for_each_safe(this, next, &cpts->events) {
-               event = list_entry(this, struct cpts_event, list);
-               if (event_type(event) == CPTS_EV_PUSH) {
-                       list_del_init(&event->list);
-                       list_add(&event->list, &cpts->pool);
-                       val = event->low;
-                       break;
-               }
-       }
+static void cpts_update_cur_time(struct cpts *cpts, int match)
+{
+       cpts_write32(cpts, TS_PUSH, ts_push);
 
-       return val;
+       if (cpts_fifo_read(cpts, match) && match != -1)
+               dev_err(cpts->dev, "cpts: unable to obtain a time stamp\n");
 }
 
 /* PTP clock operations */
@@ -232,9 +232,9 @@ static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 
        spin_lock_irqsave(&cpts->lock, flags);
 
-       timecounter_read(&cpts->tc);
+       cpts->mult_new = neg_adj ? mult - diff : mult + diff;
 
-       cpts->cc.mult = neg_adj ? mult - diff : mult + diff;
+       cpts_update_cur_time(cpts, CPTS_EV_PUSH);
 
        spin_unlock_irqrestore(&cpts->lock, flags);
 
@@ -260,6 +260,9 @@ static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
        struct cpts *cpts = container_of(ptp, struct cpts, info);
 
        spin_lock_irqsave(&cpts->lock, flags);
+
+       cpts_update_cur_time(cpts, CPTS_EV_PUSH);
+
        ns = timecounter_read(&cpts->tc);
        spin_unlock_irqrestore(&cpts->lock, flags);
 
@@ -294,11 +297,14 @@ static long cpts_overflow_check(struct ptp_clock_info *ptp)
 {
        struct cpts *cpts = container_of(ptp, struct cpts, info);
        unsigned long delay = cpts->ov_check_period;
-       struct timespec64 ts;
        unsigned long flags;
+       u64 ns;
 
        spin_lock_irqsave(&cpts->lock, flags);
-       ts = ns_to_timespec64(timecounter_read(&cpts->tc));
+
+       cpts_update_cur_time(cpts, -1);
+
+       ns = timecounter_read(&cpts->tc);
 
        if (!skb_queue_empty(&cpts->txq)) {
                cpts_purge_txq(cpts);
@@ -307,8 +313,7 @@ static long cpts_overflow_check(struct ptp_clock_info *ptp)
        }
        spin_unlock_irqrestore(&cpts->lock, flags);
 
-       pr_debug("cpts overflow check at %lld.%09ld\n",
-                (long long)ts.tv_sec, ts.tv_nsec);
+       dev_dbg(cpts->dev, "cpts overflow check at %lld\n", ns);
        return (long)delay;
 }
 
@@ -390,7 +395,7 @@ static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type)
                seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK;
                if (ev_type == event_type(event) &&
                    cpts_match(skb, class, seqid, mtype)) {
-                       ns = timecounter_cyc2time(&cpts->tc, event->low);
+                       ns = event->timestamp;
                        list_del_init(&event->list);
                        list_add(&event->list, &cpts->pool);
                        break;