1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * TI Common Platform Time Sync 4 * 5 * Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com> 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 <richardcochran@gmail.com>"); 830