drivers/net/ethernet/ti/cpts.c

   1 /*
   2  * TI Common Platform Time Sync
   3  *
   4  * Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  */
  20 #include <linux/err.h>
  21 #include <linux/if.h>
  22 #include <linux/hrtimer.h>
  23 #include <linux/module.h>
  24 #include <linux/net_tstamp.h>
  25 #include <linux/ptp_classify.h>
  26 #include <linux/time.h>
  27 #include <linux/uaccess.h>
  28 #include <linux/workqueue.h>
  29 #include <linux/if_ether.h>
  30 #include <linux/if_vlan.h>
  31
  32 #include "cpts.h"
  33
  34 #define CPTS_SKB_TX_WORK_TIMEOUT 1 /* jiffies */
  35
  36 struct cpts_skb_cb_data {
  37         unsigned long tmo;
  38 };
  39
  40 #define cpts_read32(c, r)       readl_relaxed(&c->reg->r)
  41 #define cpts_write32(c, v, r)   writel_relaxed(v, &c->reg->r)
  42
  43 static int cpts_match(struct sk_buff *skb, unsigned int ptp_class,
  44                       u16 ts_seqid, u8 ts_msgtype);
  45
  46 static int event_expired(struct cpts_event *event)
  47 {
  48         return time_after(jiffies, event->tmo);
  49 }
  50
  51 static int event_type(struct cpts_event *event)
  52 {
  53         return (event->high >> EVENT_TYPE_SHIFT) & EVENT_TYPE_MASK;
  54 }
  55
  56 static int cpts_fifo_pop(struct cpts *cpts, u32 *high, u32 *low)
  57 {
  58         u32 r = cpts_read32(cpts, intstat_raw);
  59
  60         if (r & TS_PEND_RAW) {
  61                 *high = cpts_read32(cpts, event_high);
  62                 *low  = cpts_read32(cpts, event_low);
  63                 cpts_write32(cpts, EVENT_POP, event_pop);
  64                 return 0;
  65         }
  66         return -1;
  67 }
  68
  69 static int cpts_purge_events(struct cpts *cpts)
  70 {
  71         struct list_head *this, *next;
  72         struct cpts_event *event;
  73         int removed = 0;
  74
  75         list_for_each_safe(this, next, &cpts->events) {
  76                 event = list_entry(this, struct cpts_event, list);
  77                 if (event_expired(event)) {
  78                         list_del_init(&event->list);
  79                         list_add(&event->list, &cpts->pool);
  80                         ++removed;
  81                 }
  82         }
  83
  84         if (removed)
  85                 pr_debug("cpts: event pool cleaned up %d\n", removed);
  86         return removed ? 0 : -1;
  87 }
  88
  89 static bool cpts_match_tx_ts(struct cpts *cpts, struct cpts_event *event)
  90 {
  91         struct sk_buff *skb, *tmp;
  92         u16 seqid;
  93         u8 mtype;
  94         bool found = false;
  95
  96         mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK;
  97         seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK;
  98
  99         /* no need to grab txq.lock as access is always done under cpts->lock */
 100         skb_queue_walk_safe(&cpts->txq, skb, tmp) {
 101                 struct skb_shared_hwtstamps ssh;
 102                 unsigned int class = ptp_classify_raw(skb);
 103                 struct cpts_skb_cb_data *skb_cb =
 104                                         (struct cpts_skb_cb_data *)skb->cb;
 105
 106                 if (cpts_match(skb, class, seqid, mtype)) {
 107                         u64 ns = timecounter_cyc2time(&cpts->tc, event->low);
 108
 109                         memset(&ssh, 0, sizeof(ssh));
 110                         ssh.hwtstamp = ns_to_ktime(ns);
 111                         skb_tstamp_tx(skb, &ssh);
 112                         found = true;
 113                         __skb_unlink(skb, &cpts->txq);
 114                         dev_consume_skb_any(skb);
 115                         dev_dbg(cpts->dev, "match tx timestamp mtype %u seqid %04x\n",
 116                                 mtype, seqid);
 117                         break;
 118                 }
 119
 120                 if (time_after(jiffies, skb_cb->tmo)) {
 121                         /* timeout any expired skbs over 1s */
 122                         dev_dbg(cpts->dev,
 123                                 "expiring tx timestamp mtype %u seqid %04x\n",
 124                                 mtype, seqid);
 125                         __skb_unlink(skb, &cpts->txq);
 126                         dev_consume_skb_any(skb);
 127                 }
 128         }
 129
 130         return found;
 131 }
 132
 133 /*
 134  * Returns zero if matching event type was found.
 135  */
 136 static int cpts_fifo_read(struct cpts *cpts, int match)
 137 {
 138         int i, type = -1;
 139         u32 hi, lo;
 140         struct cpts_event *event;
 141
 142         for (i = 0; i < CPTS_FIFO_DEPTH; i++) {
 143                 if (cpts_fifo_pop(cpts, &hi, &lo))
 144                         break;
 145
 146                 if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) {
 147                         pr_err("cpts: event pool empty\n");
 148                         return -1;
 149                 }
 150
 151                 event = list_first_entry(&cpts->pool, struct cpts_event, list);
 152                 event->tmo = jiffies + 2;
 153                 event->high = hi;
 154                 event->low = lo;
 155                 type = event_type(event);
 156                 switch (type) {
 157                 case CPTS_EV_TX:
 158                         if (cpts_match_tx_ts(cpts, event)) {
 159                                 /* if the new event matches an existing skb,
 160                                  * then don't queue it
 161                                  */
 162                                 break;
 163                         }
 164                 case CPTS_EV_PUSH:
 165                 case CPTS_EV_RX:
 166                         list_del_init(&event->list);
 167                         list_add_tail(&event->list, &cpts->events);
 168                         break;
 169                 case CPTS_EV_ROLL:
 170                 case CPTS_EV_HALF:
 171                 case CPTS_EV_HW:
 172                         break;
 173                 default:
 174                         pr_err("cpts: unknown event type\n");
 175                         break;
 176                 }
 177                 if (type == match)
 178                         break;
 179         }
 180         return type == match ? 0 : -1;
 181 }
 182
 183 static u64 cpts_systim_read(const struct cyclecounter *cc)
 184 {
 185         u64 val = 0;
 186         struct cpts_event *event;
 187         struct list_head *this, *next;
 188         struct cpts *cpts = container_of(cc, struct cpts, cc);
 189
 190         cpts_write32(cpts, TS_PUSH, ts_push);
 191         if (cpts_fifo_read(cpts, CPTS_EV_PUSH))
 192                 pr_err("cpts: unable to obtain a time stamp\n");
 193
 194         list_for_each_safe(this, next, &cpts->events) {
 195                 event = list_entry(this, struct cpts_event, list);
 196                 if (event_type(event) == CPTS_EV_PUSH) {
 197                         list_del_init(&event->list);
 198                         list_add(&event->list, &cpts->pool);
 199                         val = event->low;
 200                         break;
 201                 }
 202         }
 203
 204         return val;
 205 }
 206
 207 /* PTP clock operations */
 208
 209 static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 210 {
 211         u64 adj;
 212         u32 diff, mult;
 213         int neg_adj = 0;
 214         unsigned long flags;
 215         struct cpts *cpts = container_of(ptp, struct cpts, info);
 216
 217         if (ppb < 0) {
 218                 neg_adj = 1;
 219                 ppb = -ppb;
 220         }
 221         mult = cpts->cc_mult;
 222         adj = mult;
 223         adj *= ppb;
 224         diff = div_u64(adj, 1000000000ULL);
 225
 226         spin_lock_irqsave(&cpts->lock, flags);
 227
 228         timecounter_read(&cpts->tc);
 229
 230         cpts->cc.mult = neg_adj ? mult - diff : mult + diff;
 231
 232         spin_unlock_irqrestore(&cpts->lock, flags);
 233
 234         return 0;
 235 }
 236
 237 static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 238 {
 239         unsigned long flags;
 240         struct cpts *cpts = container_of(ptp, struct cpts, info);
 241
 242         spin_lock_irqsave(&cpts->lock, flags);
 243         timecounter_adjtime(&cpts->tc, delta);
 244         spin_unlock_irqrestore(&cpts->lock, flags);
 245
 246         return 0;
 247 }
 248
 249 static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 250 {
 251         u64 ns;
 252         unsigned long flags;
 253         struct cpts *cpts = container_of(ptp, struct cpts, info);
 254
 255         spin_lock_irqsave(&cpts->lock, flags);
 256         ns = timecounter_read(&cpts->tc);
 257         spin_unlock_irqrestore(&cpts->lock, flags);
 258
 259         *ts = ns_to_timespec64(ns);
 260
 261         return 0;
 262 }
 263
 264 static int cpts_ptp_settime(struct ptp_clock_info *ptp,
 265                             const struct timespec64 *ts)
 266 {
 267         u64 ns;
 268         unsigned long flags;
 269         struct cpts *cpts = container_of(ptp, struct cpts, info);
 270
 271         ns = timespec64_to_ns(ts);
 272
 273         spin_lock_irqsave(&cpts->lock, flags);
 274         timecounter_init(&cpts->tc, &cpts->cc, ns);
 275         spin_unlock_irqrestore(&cpts->lock, flags);
 276
 277         return 0;
 278 }
 279
 280 static int cpts_ptp_enable(struct ptp_clock_info *ptp,
 281                            struct ptp_clock_request *rq, int on)
 282 {
 283         return -EOPNOTSUPP;
 284 }
 285
 286 static long cpts_overflow_check(struct ptp_clock_info *ptp)
 287 {
 288         struct cpts *cpts = container_of(ptp, struct cpts, info);
 289         unsigned long delay = cpts->ov_check_period;
 290         struct timespec64 ts;
 291         unsigned long flags;
 292
 293         spin_lock_irqsave(&cpts->lock, flags);
 294         ts = ns_to_timespec64(timecounter_read(&cpts->tc));
 295
 296         if (!skb_queue_empty(&cpts->txq))
 297                 delay = CPTS_SKB_TX_WORK_TIMEOUT;
 298         spin_unlock_irqrestore(&cpts->lock, flags);
 299
 300         pr_debug("cpts overflow check at %lld.%09ld\n",
 301                  (long long)ts.tv_sec, ts.tv_nsec);
 302         return (long)delay;
 303 }
 304
 305 static const struct ptp_clock_info cpts_info = {
 306         .owner          = THIS_MODULE,
 307         .name           = "CTPS timer",
 308         .max_adj        = 1000000,
 309         .n_ext_ts       = 0,
 310         .n_pins         = 0,
 311         .pps            = 0,
 312         .adjfreq        = cpts_ptp_adjfreq,
 313         .adjtime        = cpts_ptp_adjtime,
 314         .gettime64      = cpts_ptp_gettime,
 315         .settime64      = cpts_ptp_settime,
 316         .enable         = cpts_ptp_enable,
 317         .do_aux_work    = cpts_overflow_check,
 318 };
 319
 320 static int cpts_match(struct sk_buff *skb, unsigned int ptp_class,
 321                       u16 ts_seqid, u8 ts_msgtype)
 322 {
 323         u16 *seqid;
 324         unsigned int offset = 0;
 325         u8 *msgtype, *data = skb->data;
 326
 327         if (ptp_class & PTP_CLASS_VLAN)
 328                 offset += VLAN_HLEN;
 329
 330         switch (ptp_class & PTP_CLASS_PMASK) {
 331         case PTP_CLASS_IPV4:
 332                 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
 333                 break;
 334         case PTP_CLASS_IPV6:
 335                 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
 336                 break;
 337         case PTP_CLASS_L2:
 338                 offset += ETH_HLEN;
 339                 break;
 340         default:
 341                 return 0;
 342         }
 343
 344         if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
 345                 return 0;
 346
 347         if (unlikely(ptp_class & PTP_CLASS_V1))
 348                 msgtype = data + offset + OFF_PTP_CONTROL;
 349         else
 350                 msgtype = data + offset;
 351
 352         seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
 353
 354         return (ts_msgtype == (*msgtype & 0xf) && ts_seqid == ntohs(*seqid));
 355 }
 356
 357 static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type)
 358 {
 359         u64 ns = 0;
 360         struct cpts_event *event;
 361         struct list_head *this, *next;
 362         unsigned int class = ptp_classify_raw(skb);
 363         unsigned long flags;
 364         u16 seqid;
 365         u8 mtype;
 366
 367         if (class == PTP_CLASS_NONE)
 368                 return 0;
 369
 370         spin_lock_irqsave(&cpts->lock, flags);
 371         cpts_fifo_read(cpts, -1);
 372         list_for_each_safe(this, next, &cpts->events) {
 373                 event = list_entry(this, struct cpts_event, list);
 374                 if (event_expired(event)) {
 375                         list_del_init(&event->list);
 376                         list_add(&event->list, &cpts->pool);
 377                         continue;
 378                 }
 379                 mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK;
 380                 seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK;
 381                 if (ev_type == event_type(event) &&
 382                     cpts_match(skb, class, seqid, mtype)) {
 383                         ns = timecounter_cyc2time(&cpts->tc, event->low);
 384                         list_del_init(&event->list);
 385                         list_add(&event->list, &cpts->pool);
 386                         break;
 387                 }
 388         }
 389
 390         if (ev_type == CPTS_EV_TX && !ns) {
 391                 struct cpts_skb_cb_data *skb_cb =
 392                                 (struct cpts_skb_cb_data *)skb->cb;
 393                 /* Not found, add frame to queue for processing later.
 394                  * The periodic FIFO check will handle this.
 395                  */
 396                 skb_get(skb);
 397                 /* get the timestamp for timeouts */
 398                 skb_cb->tmo = jiffies + msecs_to_jiffies(100);
 399                 __skb_queue_tail(&cpts->txq, skb);
 400                 ptp_schedule_worker(cpts->clock, 0);
 401         }
 402         spin_unlock_irqrestore(&cpts->lock, flags);
 403
 404         return ns;
 405 }
 406
 407 void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb)
 408 {
 409         u64 ns;
 410         struct skb_shared_hwtstamps *ssh;
 411
 412         if (!cpts->rx_enable)
 413                 return;
 414         ns = cpts_find_ts(cpts, skb, CPTS_EV_RX);
 415         if (!ns)
 416                 return;
 417         ssh = skb_hwtstamps(skb);
 418         memset(ssh, 0, sizeof(*ssh));
 419         ssh->hwtstamp = ns_to_ktime(ns);
 420 }
 421 EXPORT_SYMBOL_GPL(cpts_rx_timestamp);
 422
 423 void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb)
 424 {
 425         u64 ns;
 426         struct skb_shared_hwtstamps ssh;
 427
 428         if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
 429                 return;
 430         ns = cpts_find_ts(cpts, skb, CPTS_EV_TX);
 431         if (!ns)
 432                 return;
 433         memset(&ssh, 0, sizeof(ssh));
 434         ssh.hwtstamp = ns_to_ktime(ns);
 435         skb_tstamp_tx(skb, &ssh);
 436 }
 437 EXPORT_SYMBOL_GPL(cpts_tx_timestamp);
 438
 439 int cpts_register(struct cpts *cpts)
 440 {
 441         int err, i;
 442
 443         skb_queue_head_init(&cpts->txq);
 444         INIT_LIST_HEAD(&cpts->events);
 445         INIT_LIST_HEAD(&cpts->pool);
 446         for (i = 0; i < CPTS_MAX_EVENTS; i++)
 447                 list_add(&cpts->pool_data[i].list, &cpts->pool);
 448
 449         clk_enable(cpts->refclk);
 450
 451         cpts_write32(cpts, CPTS_EN, control);
 452         cpts_write32(cpts, TS_PEND_EN, int_enable);
 453
 454         timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real()));
 455
 456         cpts->clock = ptp_clock_register(&cpts->info, cpts->dev);
 457         if (IS_ERR(cpts->clock)) {
 458                 err = PTR_ERR(cpts->clock);
 459                 cpts->clock = NULL;
 460                 goto err_ptp;
 461         }
 462         cpts->phc_index = ptp_clock_index(cpts->clock);
 463
 464         ptp_schedule_worker(cpts->clock, cpts->ov_check_period);
 465         return 0;
 466
 467 err_ptp:
 468         clk_disable(cpts->refclk);
 469         return err;
 470 }
 471 EXPORT_SYMBOL_GPL(cpts_register);
 472
 473 void cpts_unregister(struct cpts *cpts)
 474 {
 475         if (WARN_ON(!cpts->clock))
 476                 return;
 477
 478         ptp_clock_unregister(cpts->clock);
 479         cpts->clock = NULL;
 480
 481         cpts_write32(cpts, 0, int_enable);
 482         cpts_write32(cpts, 0, control);
 483
 484         /* Drop all packet */
 485         skb_queue_purge(&cpts->txq);
 486
 487         clk_disable(cpts->refclk);
 488 }
 489 EXPORT_SYMBOL_GPL(cpts_unregister);
 490
 491 static void cpts_calc_mult_shift(struct cpts *cpts)
 492 {
 493         u64 frac, maxsec, ns;
 494         u32 freq;
 495
 496         freq = clk_get_rate(cpts->refclk);
 497
 498         /* Calc the maximum number of seconds which we can run before
 499          * wrapping around.
 500          */
 501         maxsec = cpts->cc.mask;
 502         do_div(maxsec, freq);
 503         /* limit conversation rate to 10 sec as higher values will produce
 504          * too small mult factors and so reduce the conversion accuracy
 505          */
 506         if (maxsec > 10)
 507                 maxsec = 10;
 508
 509         /* Calc overflow check period (maxsec / 2) */
 510         cpts->ov_check_period = (HZ * maxsec) / 2;
 511         dev_info(cpts->dev, "cpts: overflow check period %lu (jiffies)\n",
 512                  cpts->ov_check_period);
 513
 514         if (cpts->cc.mult || cpts->cc.shift)
 515                 return;
 516
 517         clocks_calc_mult_shift(&cpts->cc.mult, &cpts->cc.shift,
 518                                freq, NSEC_PER_SEC, maxsec);
 519
 520         frac = 0;
 521         ns = cyclecounter_cyc2ns(&cpts->cc, freq, cpts->cc.mask, &frac);
 522
 523         dev_info(cpts->dev,
 524                  "CPTS: ref_clk_freq:%u calc_mult:%u calc_shift:%u error:%lld nsec/sec\n",
 525                  freq, cpts->cc.mult, cpts->cc.shift, (ns - NSEC_PER_SEC));
 526 }
 527
 528 static int cpts_of_parse(struct cpts *cpts, struct device_node *node)
 529 {
 530         int ret = -EINVAL;
 531         u32 prop;
 532
 533         if (!of_property_read_u32(node, "cpts_clock_mult", &prop))
 534                 cpts->cc.mult = prop;
 535
 536         if (!of_property_read_u32(node, "cpts_clock_shift", &prop))
 537                 cpts->cc.shift = prop;
 538
 539         if ((cpts->cc.mult && !cpts->cc.shift) ||
 540             (!cpts->cc.mult && cpts->cc.shift))
 541                 goto of_error;
 542
 543         return 0;
 544
 545 of_error:
 546         dev_err(cpts->dev, "CPTS: Missing property in the DT.\n");
 547         return ret;
 548 }
 549
 550 struct cpts *cpts_create(struct device *dev, void __iomem *regs,
 551                          struct device_node *node)
 552 {
 553         struct cpts *cpts;
 554         int ret;
 555
 556         cpts = devm_kzalloc(dev, sizeof(*cpts), GFP_KERNEL);
 557         if (!cpts)
 558                 return ERR_PTR(-ENOMEM);
 559
 560         cpts->dev = dev;
 561         cpts->reg = (struct cpsw_cpts __iomem *)regs;
 562         spin_lock_init(&cpts->lock);
 563
 564         ret = cpts_of_parse(cpts, node);
 565         if (ret)
 566                 return ERR_PTR(ret);
 567
 568         cpts->refclk = devm_clk_get(dev, "cpts");
 569         if (IS_ERR(cpts->refclk)) {
 570                 dev_err(dev, "Failed to get cpts refclk\n");
 571                 return ERR_CAST(cpts->refclk);
 572         }
 573
 574         clk_prepare(cpts->refclk);
 575
 576         cpts->cc.read = cpts_systim_read;
 577         cpts->cc.mask = CLOCKSOURCE_MASK(32);
 578         cpts->info = cpts_info;
 579
 580         cpts_calc_mult_shift(cpts);
 581         /* save cc.mult original value as it can be modified
 582          * by cpts_ptp_adjfreq().
 583          */
 584         cpts->cc_mult = cpts->cc.mult;
 585
 586         return cpts;
 587 }
 588 EXPORT_SYMBOL_GPL(cpts_create);
 589
 590 void cpts_release(struct cpts *cpts)
 591 {
 592         if (!cpts)
 593                 return;
 594
 595         if (WARN_ON(!cpts->refclk))
 596                 return;
 597
 598         clk_unprepare(cpts->refclk);
 599 }
 600 EXPORT_SYMBOL_GPL(cpts_release);
 601
 602 MODULE_LICENSE("GPL v2");
 603 MODULE_DESCRIPTION("TI CPTS driver");
 604 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");