drivers/net/ethernet/ti/cpts.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * TI Common Platform Time Sync
   4  *
   5  * Copyright (C) 2012 Richard Cochran <[email protected]>
   6  *
   7  */
   8 #include <linux/clk-provider.h>
   9 #include <linux/err.h>
  10 #include <linux/if.h>
  11 #include <linux/hrtimer.h>
  12 #include <linux/module.h>
  13 #include <linux/net_tstamp.h>
  14 #include <linux/ptp_classify.h>
  15 #include <linux/time.h>
  16 #include <linux/uaccess.h>
  17 #include <linux/workqueue.h>
  18 #include <linux/if_ether.h>
  19 #include <linux/if_vlan.h>
  20
  21 #include "cpts.h"
  22
  23 #define CPTS_SKB_TX_WORK_TIMEOUT 1 /* jiffies */
  24 #define CPTS_SKB_RX_TX_TMO 100 /*ms */
  25 #define CPTS_EVENT_RX_TX_TIMEOUT (100) /* ms */
  26
  27 struct cpts_skb_cb_data {
  28         u32 skb_mtype_seqid;
  29         unsigned long tmo;
  30 };
  31
  32 #define cpts_read32(c, r)       readl_relaxed(&c->reg->r)
  33 #define cpts_write32(c, v, r)   writel_relaxed(v, &c->reg->r)
  34
  35 static int cpts_event_port(struct cpts_event *event)
  36 {
  37         return (event->high >> PORT_NUMBER_SHIFT) & PORT_NUMBER_MASK;
  38 }
  39
  40 static int event_expired(struct cpts_event *event)
  41 {
  42         return time_after(jiffies, event->tmo);
  43 }
  44
  45 static int event_type(struct cpts_event *event)
  46 {
  47         return (event->high >> EVENT_TYPE_SHIFT) & EVENT_TYPE_MASK;
  48 }
  49
  50 static int cpts_fifo_pop(struct cpts *cpts, u32 *high, u32 *low)
  51 {
  52         u32 r = cpts_read32(cpts, intstat_raw);
  53
  54         if (r & TS_PEND_RAW) {
  55                 *high = cpts_read32(cpts, event_high);
  56                 *low  = cpts_read32(cpts, event_low);
  57                 cpts_write32(cpts, EVENT_POP, event_pop);
  58                 return 0;
  59         }
  60         return -1;
  61 }
  62
  63 static int cpts_purge_events(struct cpts *cpts)
  64 {
  65         struct list_head *this, *next;
  66         struct cpts_event *event;
  67         int removed = 0;
  68
  69         list_for_each_safe(this, next, &cpts->events) {
  70                 event = list_entry(this, struct cpts_event, list);
  71                 if (event_expired(event)) {
  72                         list_del_init(&event->list);
  73                         list_add(&event->list, &cpts->pool);
  74                         ++removed;
  75                 }
  76         }
  77
  78         if (removed)
  79                 dev_dbg(cpts->dev, "cpts: event pool cleaned up %d\n", removed);
  80         return removed ? 0 : -1;
  81 }
  82
  83 static void cpts_purge_txq(struct cpts *cpts)
  84 {
  85         struct cpts_skb_cb_data *skb_cb;
  86         struct sk_buff *skb, *tmp;
  87         int removed = 0;
  88
  89         skb_queue_walk_safe(&cpts->txq, skb, tmp) {
  90                 skb_cb = (struct cpts_skb_cb_data *)skb->cb;
  91                 if (time_after(jiffies, skb_cb->tmo)) {
  92                         __skb_unlink(skb, &cpts->txq);
  93                         dev_consume_skb_any(skb);
  94                         ++removed;
  95                 }
  96         }
  97
  98         if (removed)
  99                 dev_dbg(cpts->dev, "txq cleaned up %d\n", removed);
 100 }
 101
 102 /*
 103  * Returns zero if matching event type was found.
 104  */
 105 static int cpts_fifo_read(struct cpts *cpts, int match)
 106 {
 107         struct ptp_clock_event pevent;
 108         bool need_schedule = false;
 109         struct cpts_event *event;
 110         unsigned long flags;
 111         int i, type = -1;
 112         u32 hi, lo;
 113
 114         spin_lock_irqsave(&cpts->lock, flags);
 115
 116         for (i = 0; i < CPTS_FIFO_DEPTH; i++) {
 117                 if (cpts_fifo_pop(cpts, &hi, &lo))
 118                         break;
 119
 120                 if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) {
 121                         dev_warn(cpts->dev, "cpts: event pool empty\n");
 122                         break;
 123                 }
 124
 125                 event = list_first_entry(&cpts->pool, struct cpts_event, list);
 126                 event->high = hi;
 127                 event->low = lo;
 128                 event->timestamp = timecounter_cyc2time(&cpts->tc, event->low);
 129                 type = event_type(event);
 130
 131                 dev_dbg(cpts->dev, "CPTS_EV: %d high:%08X low:%08x\n",
 132                         type, event->high, event->low);
 133                 switch (type) {
 134                 case CPTS_EV_PUSH:
 135                         WRITE_ONCE(cpts->cur_timestamp, lo);
 136                         timecounter_read(&cpts->tc);
 137                         if (cpts->mult_new) {
 138                                 cpts->cc.mult = cpts->mult_new;
 139                                 cpts->mult_new = 0;
 140                         }
 141                         if (!cpts->irq_poll)
 142                                 complete(&cpts->ts_push_complete);
 143                         break;
 144                 case CPTS_EV_TX:
 145                 case CPTS_EV_RX:
 146                         event->tmo = jiffies +
 147                                 msecs_to_jiffies(CPTS_EVENT_RX_TX_TIMEOUT);
 148
 149                         list_del_init(&event->list);
 150                         list_add_tail(&event->list, &cpts->events);
 151                         need_schedule = true;
 152                         break;
 153                 case CPTS_EV_ROLL:
 154                 case CPTS_EV_HALF:
 155                         break;
 156                 case CPTS_EV_HW:
 157                         pevent.timestamp = event->timestamp;
 158                         pevent.type = PTP_CLOCK_EXTTS;
 159                         pevent.index = cpts_event_port(event) - 1;
 160                         ptp_clock_event(cpts->clock, &pevent);
 161                         break;
 162                 default:
 163                         dev_err(cpts->dev, "cpts: unknown event type\n");
 164                         break;
 165                 }
 166                 if (type == match)
 167                         break;
 168         }
 169
 170         spin_unlock_irqrestore(&cpts->lock, flags);
 171
 172         if (!cpts->irq_poll && need_schedule)
 173                 ptp_schedule_worker(cpts->clock, 0);
 174
 175         return type == match ? 0 : -1;
 176 }
 177
 178 void cpts_misc_interrupt(struct cpts *cpts)
 179 {
 180         cpts_fifo_read(cpts, -1);
 181 }
 182 EXPORT_SYMBOL_GPL(cpts_misc_interrupt);
 183
 184 static u64 cpts_systim_read(const struct cyclecounter *cc)
 185 {
 186         struct cpts *cpts = container_of(cc, struct cpts, cc);
 187
 188         return READ_ONCE(cpts->cur_timestamp);
 189 }
 190
 191 static void cpts_update_cur_time(struct cpts *cpts, int match,
 192                                  struct ptp_system_timestamp *sts)
 193 {
 194         unsigned long flags;
 195
 196         reinit_completion(&cpts->ts_push_complete);
 197
 198         /* use spin_lock_irqsave() here as it has to run very fast */
 199         spin_lock_irqsave(&cpts->lock, flags);
 200         ptp_read_system_prets(sts);
 201         cpts_write32(cpts, TS_PUSH, ts_push);
 202         cpts_read32(cpts, ts_push);
 203         ptp_read_system_postts(sts);
 204         spin_unlock_irqrestore(&cpts->lock, flags);
 205
 206         if (cpts->irq_poll && cpts_fifo_read(cpts, match) && match != -1)
 207                 dev_err(cpts->dev, "cpts: unable to obtain a time stamp\n");
 208
 209         if (!cpts->irq_poll &&
 210             !wait_for_completion_timeout(&cpts->ts_push_complete, HZ))
 211                 dev_err(cpts->dev, "cpts: obtain a time stamp timeout\n");
 212 }
 213
 214 /* PTP clock operations */
 215
 216 static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 217 {
 218         struct cpts *cpts = container_of(ptp, struct cpts, info);
 219         int neg_adj = 0;
 220         u32 diff, mult;
 221         u64 adj;
 222
 223         if (ppb < 0) {
 224                 neg_adj = 1;
 225                 ppb = -ppb;
 226         }
 227         mult = cpts->cc_mult;
 228         adj = mult;
 229         adj *= ppb;
 230         diff = div_u64(adj, 1000000000ULL);
 231
 232         mutex_lock(&cpts->ptp_clk_mutex);
 233
 234         cpts->mult_new = neg_adj ? mult - diff : mult + diff;
 235
 236         cpts_update_cur_time(cpts, CPTS_EV_PUSH, NULL);
 237
 238         mutex_unlock(&cpts->ptp_clk_mutex);
 239         return 0;
 240 }
 241
 242 static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 243 {
 244         struct cpts *cpts = container_of(ptp, struct cpts, info);
 245
 246         mutex_lock(&cpts->ptp_clk_mutex);
 247         timecounter_adjtime(&cpts->tc, delta);
 248         mutex_unlock(&cpts->ptp_clk_mutex);
 249
 250         return 0;
 251 }
 252
 253 static int cpts_ptp_gettimeex(struct ptp_clock_info *ptp,
 254                               struct timespec64 *ts,
 255                               struct ptp_system_timestamp *sts)
 256 {
 257         struct cpts *cpts = container_of(ptp, struct cpts, info);
 258         u64 ns;
 259
 260         mutex_lock(&cpts->ptp_clk_mutex);
 261
 262         cpts_update_cur_time(cpts, CPTS_EV_PUSH, sts);
 263
 264         ns = timecounter_read(&cpts->tc);
 265         mutex_unlock(&cpts->ptp_clk_mutex);
 266
 267         *ts = ns_to_timespec64(ns);
 268
 269         return 0;
 270 }
 271
 272 static int cpts_ptp_settime(struct ptp_clock_info *ptp,
 273                             const struct timespec64 *ts)
 274 {
 275         struct cpts *cpts = container_of(ptp, struct cpts, info);
 276         u64 ns;
 277
 278         ns = timespec64_to_ns(ts);
 279
 280         mutex_lock(&cpts->ptp_clk_mutex);
 281         timecounter_init(&cpts->tc, &cpts->cc, ns);
 282         mutex_unlock(&cpts->ptp_clk_mutex);
 283
 284         return 0;
 285 }
 286
 287 static int cpts_extts_enable(struct cpts *cpts, u32 index, int on)
 288 {
 289         u32 v;
 290
 291         if (((cpts->hw_ts_enable & BIT(index)) >> index) == on)
 292                 return 0;
 293
 294         mutex_lock(&cpts->ptp_clk_mutex);
 295
 296         v = cpts_read32(cpts, control);
 297         if (on) {
 298                 v |= BIT(8 + index);
 299                 cpts->hw_ts_enable |= BIT(index);
 300         } else {
 301                 v &= ~BIT(8 + index);
 302                 cpts->hw_ts_enable &= ~BIT(index);
 303         }
 304         cpts_write32(cpts, v, control);
 305
 306         mutex_unlock(&cpts->ptp_clk_mutex);
 307
 308         return 0;
 309 }
 310
 311 static int cpts_ptp_enable(struct ptp_clock_info *ptp,
 312                            struct ptp_clock_request *rq, int on)
 313 {
 314         struct cpts *cpts = container_of(ptp, struct cpts, info);
 315
 316         switch (rq->type) {
 317         case PTP_CLK_REQ_EXTTS:
 318                 return cpts_extts_enable(cpts, rq->extts.index, on);
 319         default:
 320                 break;
 321         }
 322
 323         return -EOPNOTSUPP;
 324 }
 325
 326 static bool cpts_match_tx_ts(struct cpts *cpts, struct cpts_event *event)
 327 {
 328         struct sk_buff_head txq_list;
 329         struct sk_buff *skb, *tmp;
 330         unsigned long flags;
 331         bool found = false;
 332         u32 mtype_seqid;
 333
 334         mtype_seqid = event->high &
 335                       ((MESSAGE_TYPE_MASK << MESSAGE_TYPE_SHIFT) |
 336                        (SEQUENCE_ID_MASK << SEQUENCE_ID_SHIFT) |
 337                        (EVENT_TYPE_MASK << EVENT_TYPE_SHIFT));
 338
 339         __skb_queue_head_init(&txq_list);
 340
 341         spin_lock_irqsave(&cpts->txq.lock, flags);
 342         skb_queue_splice_init(&cpts->txq, &txq_list);
 343         spin_unlock_irqrestore(&cpts->txq.lock, flags);
 344
 345         skb_queue_walk_safe(&txq_list, skb, tmp) {
 346                 struct skb_shared_hwtstamps ssh;
 347                 struct cpts_skb_cb_data *skb_cb =
 348                                         (struct cpts_skb_cb_data *)skb->cb;
 349
 350                 if (mtype_seqid == skb_cb->skb_mtype_seqid) {
 351                         memset(&ssh, 0, sizeof(ssh));
 352                         ssh.hwtstamp = ns_to_ktime(event->timestamp);
 353                         skb_tstamp_tx(skb, &ssh);
 354                         found = true;
 355                         __skb_unlink(skb, &txq_list);
 356                         dev_consume_skb_any(skb);
 357                         dev_dbg(cpts->dev, "match tx timestamp mtype_seqid %08x\n",
 358                                 mtype_seqid);
 359                         break;
 360                 }
 361
 362                 if (time_after(jiffies, skb_cb->tmo)) {
 363                         /* timeout any expired skbs over 1s */
 364                         dev_dbg(cpts->dev, "expiring tx timestamp from txq\n");
 365                         __skb_unlink(skb, &txq_list);
 366                         dev_consume_skb_any(skb);
 367                 }
 368         }
 369
 370         spin_lock_irqsave(&cpts->txq.lock, flags);
 371         skb_queue_splice(&txq_list, &cpts->txq);
 372         spin_unlock_irqrestore(&cpts->txq.lock, flags);
 373
 374         return found;
 375 }
 376
 377 static void cpts_process_events(struct cpts *cpts)
 378 {
 379         struct list_head *this, *next;
 380         struct cpts_event *event;
 381         LIST_HEAD(events_free);
 382         unsigned long flags;
 383         LIST_HEAD(events);
 384
 385         spin_lock_irqsave(&cpts->lock, flags);
 386         list_splice_init(&cpts->events, &events);
 387         spin_unlock_irqrestore(&cpts->lock, flags);
 388
 389         list_for_each_safe(this, next, &events) {
 390                 event = list_entry(this, struct cpts_event, list);
 391                 if (cpts_match_tx_ts(cpts, event) ||
 392                     time_after(jiffies, event->tmo)) {
 393                         list_del_init(&event->list);
 394                         list_add(&event->list, &events_free);
 395                 }
 396         }
 397
 398         spin_lock_irqsave(&cpts->lock, flags);
 399         list_splice_tail(&events, &cpts->events);
 400         list_splice_tail(&events_free, &cpts->pool);
 401         spin_unlock_irqrestore(&cpts->lock, flags);
 402 }
 403
 404 static long cpts_overflow_check(struct ptp_clock_info *ptp)
 405 {
 406         struct cpts *cpts = container_of(ptp, struct cpts, info);
 407         unsigned long delay = cpts->ov_check_period;
 408         unsigned long flags;
 409         u64 ns;
 410
 411         mutex_lock(&cpts->ptp_clk_mutex);
 412
 413         cpts_update_cur_time(cpts, -1, NULL);
 414         ns = timecounter_read(&cpts->tc);
 415
 416         cpts_process_events(cpts);
 417
 418         spin_lock_irqsave(&cpts->txq.lock, flags);
 419         if (!skb_queue_empty(&cpts->txq)) {
 420                 cpts_purge_txq(cpts);
 421                 if (!skb_queue_empty(&cpts->txq))
 422                         delay = CPTS_SKB_TX_WORK_TIMEOUT;
 423         }
 424         spin_unlock_irqrestore(&cpts->txq.lock, flags);
 425
 426         dev_dbg(cpts->dev, "cpts overflow check at %lld\n", ns);
 427         mutex_unlock(&cpts->ptp_clk_mutex);
 428         return (long)delay;
 429 }
 430
 431 static const struct ptp_clock_info cpts_info = {
 432         .owner          = THIS_MODULE,
 433         .name           = "CTPS timer",
 434         .max_adj        = 1000000,
 435         .n_ext_ts       = 0,
 436         .n_pins         = 0,
 437         .pps            = 0,
 438         .adjfreq        = cpts_ptp_adjfreq,
 439         .adjtime        = cpts_ptp_adjtime,
 440         .gettimex64     = cpts_ptp_gettimeex,
 441         .settime64      = cpts_ptp_settime,
 442         .enable         = cpts_ptp_enable,
 443         .do_aux_work    = cpts_overflow_check,
 444 };
 445
 446 static int cpts_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid)
 447 {
 448         unsigned int ptp_class = ptp_classify_raw(skb);
 449         u8 *msgtype, *data = skb->data;
 450         unsigned int offset = 0;
 451         u16 *seqid;
 452
 453         if (ptp_class == PTP_CLASS_NONE)
 454                 return 0;
 455
 456         if (ptp_class & PTP_CLASS_VLAN)
 457                 offset += VLAN_HLEN;
 458
 459         switch (ptp_class & PTP_CLASS_PMASK) {
 460         case PTP_CLASS_IPV4:
 461                 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
 462                 break;
 463         case PTP_CLASS_IPV6:
 464                 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
 465                 break;
 466         case PTP_CLASS_L2:
 467                 offset += ETH_HLEN;
 468                 break;
 469         default:
 470                 return 0;
 471         }
 472
 473         if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
 474                 return 0;
 475
 476         if (unlikely(ptp_class & PTP_CLASS_V1))
 477                 msgtype = data + offset + OFF_PTP_CONTROL;
 478         else
 479                 msgtype = data + offset;
 480
 481         seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
 482         *mtype_seqid = (*msgtype & MESSAGE_TYPE_MASK) << MESSAGE_TYPE_SHIFT;
 483         *mtype_seqid |= (ntohs(*seqid) & SEQUENCE_ID_MASK) << SEQUENCE_ID_SHIFT;
 484
 485         return 1;
 486 }
 487
 488 static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb,
 489                         int ev_type, u32 skb_mtype_seqid)
 490 {
 491         struct list_head *this, *next;
 492         struct cpts_event *event;
 493         unsigned long flags;
 494         u32 mtype_seqid;
 495         u64 ns = 0;
 496
 497         cpts_fifo_read(cpts, -1);
 498         spin_lock_irqsave(&cpts->lock, flags);
 499         list_for_each_safe(this, next, &cpts->events) {
 500                 event = list_entry(this, struct cpts_event, list);
 501                 if (event_expired(event)) {
 502                         list_del_init(&event->list);
 503                         list_add(&event->list, &cpts->pool);
 504                         continue;
 505                 }
 506
 507                 mtype_seqid = event->high &
 508                               ((MESSAGE_TYPE_MASK << MESSAGE_TYPE_SHIFT) |
 509                                (SEQUENCE_ID_MASK << SEQUENCE_ID_SHIFT) |
 510                                (EVENT_TYPE_MASK << EVENT_TYPE_SHIFT));
 511
 512                 if (mtype_seqid == skb_mtype_seqid) {
 513                         ns = event->timestamp;
 514                         list_del_init(&event->list);
 515                         list_add(&event->list, &cpts->pool);
 516                         break;
 517                 }
 518         }
 519         spin_unlock_irqrestore(&cpts->lock, flags);
 520
 521         return ns;
 522 }
 523
 524 void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb)
 525 {
 526         struct cpts_skb_cb_data *skb_cb = (struct cpts_skb_cb_data *)skb->cb;
 527         struct skb_shared_hwtstamps *ssh;
 528         int ret;
 529         u64 ns;
 530
 531         ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
 532         if (!ret)
 533                 return;
 534
 535         skb_cb->skb_mtype_seqid |= (CPTS_EV_RX << EVENT_TYPE_SHIFT);
 536
 537         dev_dbg(cpts->dev, "%s mtype seqid %08x\n",
 538                 __func__, skb_cb->skb_mtype_seqid);
 539
 540         ns = cpts_find_ts(cpts, skb, CPTS_EV_RX, skb_cb->skb_mtype_seqid);
 541         if (!ns)
 542                 return;
 543         ssh = skb_hwtstamps(skb);
 544         memset(ssh, 0, sizeof(*ssh));
 545         ssh->hwtstamp = ns_to_ktime(ns);
 546 }
 547 EXPORT_SYMBOL_GPL(cpts_rx_timestamp);
 548
 549 void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb)
 550 {
 551         struct cpts_skb_cb_data *skb_cb = (struct cpts_skb_cb_data *)skb->cb;
 552         int ret;
 553
 554         if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
 555                 return;
 556
 557         ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
 558         if (!ret)
 559                 return;
 560
 561         skb_cb->skb_mtype_seqid |= (CPTS_EV_TX << EVENT_TYPE_SHIFT);
 562
 563         dev_dbg(cpts->dev, "%s mtype seqid %08x\n",
 564                 __func__, skb_cb->skb_mtype_seqid);
 565
 566         /* Always defer TX TS processing to PTP worker */
 567         skb_get(skb);
 568         /* get the timestamp for timeouts */
 569         skb_cb->tmo = jiffies + msecs_to_jiffies(CPTS_SKB_RX_TX_TMO);
 570         skb_queue_tail(&cpts->txq, skb);
 571         ptp_schedule_worker(cpts->clock, 0);
 572 }
 573 EXPORT_SYMBOL_GPL(cpts_tx_timestamp);
 574
 575 int cpts_register(struct cpts *cpts)
 576 {
 577         int err, i;
 578
 579         skb_queue_head_init(&cpts->txq);
 580         INIT_LIST_HEAD(&cpts->events);
 581         INIT_LIST_HEAD(&cpts->pool);
 582         for (i = 0; i < CPTS_MAX_EVENTS; i++)
 583                 list_add(&cpts->pool_data[i].list, &cpts->pool);
 584
 585         clk_enable(cpts->refclk);
 586
 587         cpts_write32(cpts, CPTS_EN, control);
 588         cpts_write32(cpts, TS_PEND_EN, int_enable);
 589
 590         timecounter_init(&cpts->tc, &cpts->cc, ktime_get_real_ns());
 591
 592         cpts->clock = ptp_clock_register(&cpts->info, cpts->dev);
 593         if (IS_ERR(cpts->clock)) {
 594                 err = PTR_ERR(cpts->clock);
 595                 cpts->clock = NULL;
 596                 goto err_ptp;
 597         }
 598         cpts->phc_index = ptp_clock_index(cpts->clock);
 599
 600         ptp_schedule_worker(cpts->clock, cpts->ov_check_period);
 601         return 0;
 602
 603 err_ptp:
 604         clk_disable(cpts->refclk);
 605         return err;
 606 }
 607 EXPORT_SYMBOL_GPL(cpts_register);
 608
 609 void cpts_unregister(struct cpts *cpts)
 610 {
 611         if (WARN_ON(!cpts->clock))
 612                 return;
 613
 614         ptp_clock_unregister(cpts->clock);
 615         cpts->clock = NULL;
 616
 617         cpts_write32(cpts, 0, int_enable);
 618         cpts_write32(cpts, 0, control);
 619
 620         /* Drop all packet */
 621         skb_queue_purge(&cpts->txq);
 622
 623         clk_disable(cpts->refclk);
 624 }
 625 EXPORT_SYMBOL_GPL(cpts_unregister);
 626
 627 static void cpts_calc_mult_shift(struct cpts *cpts)
 628 {
 629         u64 frac, maxsec, ns;
 630         u32 freq;
 631
 632         freq = clk_get_rate(cpts->refclk);
 633
 634         /* Calc the maximum number of seconds which we can run before
 635          * wrapping around.
 636          */
 637         maxsec = cpts->cc.mask;
 638         do_div(maxsec, freq);
 639         /* limit conversation rate to 10 sec as higher values will produce
 640          * too small mult factors and so reduce the conversion accuracy
 641          */
 642         if (maxsec > 10)
 643                 maxsec = 10;
 644
 645         /* Calc overflow check period (maxsec / 2) */
 646         cpts->ov_check_period = (HZ * maxsec) / 2;
 647         dev_info(cpts->dev, "cpts: overflow check period %lu (jiffies)\n",
 648                  cpts->ov_check_period);
 649
 650         if (cpts->cc.mult || cpts->cc.shift)
 651                 return;
 652
 653         clocks_calc_mult_shift(&cpts->cc.mult, &cpts->cc.shift,
 654                                freq, NSEC_PER_SEC, maxsec);
 655
 656         frac = 0;
 657         ns = cyclecounter_cyc2ns(&cpts->cc, freq, cpts->cc.mask, &frac);
 658
 659         dev_info(cpts->dev,
 660                  "CPTS: ref_clk_freq:%u calc_mult:%u calc_shift:%u error:%lld nsec/sec\n",
 661                  freq, cpts->cc.mult, cpts->cc.shift, (ns - NSEC_PER_SEC));
 662 }
 663
 664 static int cpts_of_mux_clk_setup(struct cpts *cpts, struct device_node *node)
 665 {
 666         struct device_node *refclk_np;
 667         const char **parent_names;
 668         unsigned int num_parents;
 669         struct clk_hw *clk_hw;
 670         int ret = -EINVAL;
 671         u32 *mux_table;
 672
 673         refclk_np = of_get_child_by_name(node, "cpts-refclk-mux");
 674         if (!refclk_np)
 675                 /* refclk selection supported not for all SoCs */
 676                 return 0;
 677
 678         num_parents = of_clk_get_parent_count(refclk_np);
 679         if (num_parents < 1) {
 680                 dev_err(cpts->dev, "mux-clock %s must have parents\n",
 681                         refclk_np->name);
 682                 goto mux_fail;
 683         }
 684
 685         parent_names = devm_kzalloc(cpts->dev, (sizeof(char *) * num_parents),
 686                                     GFP_KERNEL);
 687
 688         mux_table = devm_kzalloc(cpts->dev, sizeof(*mux_table) * num_parents,
 689                                  GFP_KERNEL);
 690         if (!mux_table || !parent_names) {
 691                 ret = -ENOMEM;
 692                 goto mux_fail;
 693         }
 694
 695         of_clk_parent_fill(refclk_np, parent_names, num_parents);
 696
 697         ret = of_property_read_variable_u32_array(refclk_np, "ti,mux-tbl",
 698                                                   mux_table,
 699                                                   num_parents, num_parents);
 700         if (ret < 0)
 701                 goto mux_fail;
 702
 703         clk_hw = clk_hw_register_mux_table(cpts->dev, refclk_np->name,
 704                                            parent_names, num_parents,
 705                                            0,
 706                                            &cpts->reg->rftclk_sel, 0, 0x1F,
 707                                            0, mux_table, NULL);
 708         if (IS_ERR(clk_hw)) {
 709                 ret = PTR_ERR(clk_hw);
 710                 goto mux_fail;
 711         }
 712
 713         ret = devm_add_action_or_reset(cpts->dev,
 714                                        (void(*)(void *))clk_hw_unregister_mux,
 715                                        clk_hw);
 716         if (ret) {
 717                 dev_err(cpts->dev, "add clkmux unreg action %d", ret);
 718                 goto mux_fail;
 719         }
 720
 721         ret = of_clk_add_hw_provider(refclk_np, of_clk_hw_simple_get, clk_hw);
 722         if (ret)
 723                 goto mux_fail;
 724
 725         ret = devm_add_action_or_reset(cpts->dev,
 726                                        (void(*)(void *))of_clk_del_provider,
 727                                        refclk_np);
 728         if (ret) {
 729                 dev_err(cpts->dev, "add clkmux provider unreg action %d", ret);
 730                 goto mux_fail;
 731         }
 732
 733         return ret;
 734
 735 mux_fail:
 736         of_node_put(refclk_np);
 737         return ret;
 738 }
 739
 740 static int cpts_of_parse(struct cpts *cpts, struct device_node *node)
 741 {
 742         int ret = -EINVAL;
 743         u32 prop;
 744
 745         if (!of_property_read_u32(node, "cpts_clock_mult", &prop))
 746                 cpts->cc.mult = prop;
 747
 748         if (!of_property_read_u32(node, "cpts_clock_shift", &prop))
 749                 cpts->cc.shift = prop;
 750
 751         if ((cpts->cc.mult && !cpts->cc.shift) ||
 752             (!cpts->cc.mult && cpts->cc.shift))
 753                 goto of_error;
 754
 755         return cpts_of_mux_clk_setup(cpts, node);
 756
 757 of_error:
 758         dev_err(cpts->dev, "CPTS: Missing property in the DT.\n");
 759         return ret;
 760 }
 761
 762 struct cpts *cpts_create(struct device *dev, void __iomem *regs,
 763                          struct device_node *node, u32 n_ext_ts)
 764 {
 765         struct cpts *cpts;
 766         int ret;
 767
 768         cpts = devm_kzalloc(dev, sizeof(*cpts), GFP_KERNEL);
 769         if (!cpts)
 770                 return ERR_PTR(-ENOMEM);
 771
 772         cpts->dev = dev;
 773         cpts->reg = (struct cpsw_cpts __iomem *)regs;
 774         cpts->irq_poll = true;
 775         spin_lock_init(&cpts->lock);
 776         mutex_init(&cpts->ptp_clk_mutex);
 777         init_completion(&cpts->ts_push_complete);
 778
 779         ret = cpts_of_parse(cpts, node);
 780         if (ret)
 781                 return ERR_PTR(ret);
 782
 783         cpts->refclk = devm_get_clk_from_child(dev, node, "cpts");
 784         if (IS_ERR(cpts->refclk))
 785                 /* try get clk from dev node for compatibility */
 786                 cpts->refclk = devm_clk_get(dev, "cpts");
 787
 788         if (IS_ERR(cpts->refclk)) {
 789                 dev_err(dev, "Failed to get cpts refclk %ld\n",
 790                         PTR_ERR(cpts->refclk));
 791                 return ERR_CAST(cpts->refclk);
 792         }
 793
 794         ret = clk_prepare(cpts->refclk);
 795         if (ret)
 796                 return ERR_PTR(ret);
 797
 798         cpts->cc.read = cpts_systim_read;
 799         cpts->cc.mask = CLOCKSOURCE_MASK(32);
 800         cpts->info = cpts_info;
 801
 802         if (n_ext_ts)
 803                 cpts->info.n_ext_ts = n_ext_ts;
 804
 805         cpts_calc_mult_shift(cpts);
 806         /* save cc.mult original value as it can be modified
 807          * by cpts_ptp_adjfreq().
 808          */
 809         cpts->cc_mult = cpts->cc.mult;
 810
 811         return cpts;
 812 }
 813 EXPORT_SYMBOL_GPL(cpts_create);
 814
 815 void cpts_release(struct cpts *cpts)
 816 {
 817         if (!cpts)
 818                 return;
 819
 820         if (WARN_ON(!cpts->refclk))
 821                 return;
 822
 823         clk_unprepare(cpts->refclk);
 824 }
 825 EXPORT_SYMBOL_GPL(cpts_release);
 826
 827 MODULE_LICENSE("GPL v2");
 828 MODULE_DESCRIPTION("TI CPTS driver");
 829 MODULE_AUTHOR("Richard Cochran <[email protected]>");