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 /* fall through */ 165 case CPTS_EV_PUSH: 166 case CPTS_EV_RX: 167 list_del_init(&event->list); 168 list_add_tail(&event->list, &cpts->events); 169 break; 170 case CPTS_EV_ROLL: 171 case CPTS_EV_HALF: 172 case CPTS_EV_HW: 173 break; 174 default: 175 pr_err("cpts: unknown event type\n"); 176 break; 177 } 178 if (type == match) 179 break; 180 } 181 return type == match ? 0 : -1; 182 } 183 184 static u64 cpts_systim_read(const struct cyclecounter *cc) 185 { 186 u64 val = 0; 187 struct cpts_event *event; 188 struct list_head *this, *next; 189 struct cpts *cpts = container_of(cc, struct cpts, cc); 190 191 cpts_write32(cpts, TS_PUSH, ts_push); 192 if (cpts_fifo_read(cpts, CPTS_EV_PUSH)) 193 pr_err("cpts: unable to obtain a time stamp\n"); 194 195 list_for_each_safe(this, next, &cpts->events) { 196 event = list_entry(this, struct cpts_event, list); 197 if (event_type(event) == CPTS_EV_PUSH) { 198 list_del_init(&event->list); 199 list_add(&event->list, &cpts->pool); 200 val = event->low; 201 break; 202 } 203 } 204 205 return val; 206 } 207 208 /* PTP clock operations */ 209 210 static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb) 211 { 212 u64 adj; 213 u32 diff, mult; 214 int neg_adj = 0; 215 unsigned long flags; 216 struct cpts *cpts = container_of(ptp, struct cpts, info); 217 218 if (ppb < 0) { 219 neg_adj = 1; 220 ppb = -ppb; 221 } 222 mult = cpts->cc_mult; 223 adj = mult; 224 adj *= ppb; 225 diff = div_u64(adj, 1000000000ULL); 226 227 spin_lock_irqsave(&cpts->lock, flags); 228 229 timecounter_read(&cpts->tc); 230 231 cpts->cc.mult = neg_adj ? mult - diff : mult + diff; 232 233 spin_unlock_irqrestore(&cpts->lock, flags); 234 235 return 0; 236 } 237 238 static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 239 { 240 unsigned long flags; 241 struct cpts *cpts = container_of(ptp, struct cpts, info); 242 243 spin_lock_irqsave(&cpts->lock, flags); 244 timecounter_adjtime(&cpts->tc, delta); 245 spin_unlock_irqrestore(&cpts->lock, flags); 246 247 return 0; 248 } 249 250 static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 251 { 252 u64 ns; 253 unsigned long flags; 254 struct cpts *cpts = container_of(ptp, struct cpts, info); 255 256 spin_lock_irqsave(&cpts->lock, flags); 257 ns = timecounter_read(&cpts->tc); 258 spin_unlock_irqrestore(&cpts->lock, flags); 259 260 *ts = ns_to_timespec64(ns); 261 262 return 0; 263 } 264 265 static int cpts_ptp_settime(struct ptp_clock_info *ptp, 266 const struct timespec64 *ts) 267 { 268 u64 ns; 269 unsigned long flags; 270 struct cpts *cpts = container_of(ptp, struct cpts, info); 271 272 ns = timespec64_to_ns(ts); 273 274 spin_lock_irqsave(&cpts->lock, flags); 275 timecounter_init(&cpts->tc, &cpts->cc, ns); 276 spin_unlock_irqrestore(&cpts->lock, flags); 277 278 return 0; 279 } 280 281 static int cpts_ptp_enable(struct ptp_clock_info *ptp, 282 struct ptp_clock_request *rq, int on) 283 { 284 return -EOPNOTSUPP; 285 } 286 287 static long cpts_overflow_check(struct ptp_clock_info *ptp) 288 { 289 struct cpts *cpts = container_of(ptp, struct cpts, info); 290 unsigned long delay = cpts->ov_check_period; 291 struct timespec64 ts; 292 unsigned long flags; 293 294 spin_lock_irqsave(&cpts->lock, flags); 295 ts = ns_to_timespec64(timecounter_read(&cpts->tc)); 296 297 if (!skb_queue_empty(&cpts->txq)) 298 delay = CPTS_SKB_TX_WORK_TIMEOUT; 299 spin_unlock_irqrestore(&cpts->lock, flags); 300 301 pr_debug("cpts overflow check at %lld.%09ld\n", 302 (long long)ts.tv_sec, ts.tv_nsec); 303 return (long)delay; 304 } 305 306 static const struct ptp_clock_info cpts_info = { 307 .owner = THIS_MODULE, 308 .name = "CTPS timer", 309 .max_adj = 1000000, 310 .n_ext_ts = 0, 311 .n_pins = 0, 312 .pps = 0, 313 .adjfreq = cpts_ptp_adjfreq, 314 .adjtime = cpts_ptp_adjtime, 315 .gettime64 = cpts_ptp_gettime, 316 .settime64 = cpts_ptp_settime, 317 .enable = cpts_ptp_enable, 318 .do_aux_work = cpts_overflow_check, 319 }; 320 321 static int cpts_match(struct sk_buff *skb, unsigned int ptp_class, 322 u16 ts_seqid, u8 ts_msgtype) 323 { 324 u16 *seqid; 325 unsigned int offset = 0; 326 u8 *msgtype, *data = skb->data; 327 328 if (ptp_class & PTP_CLASS_VLAN) 329 offset += VLAN_HLEN; 330 331 switch (ptp_class & PTP_CLASS_PMASK) { 332 case PTP_CLASS_IPV4: 333 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; 334 break; 335 case PTP_CLASS_IPV6: 336 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; 337 break; 338 case PTP_CLASS_L2: 339 offset += ETH_HLEN; 340 break; 341 default: 342 return 0; 343 } 344 345 if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) 346 return 0; 347 348 if (unlikely(ptp_class & PTP_CLASS_V1)) 349 msgtype = data + offset + OFF_PTP_CONTROL; 350 else 351 msgtype = data + offset; 352 353 seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); 354 355 return (ts_msgtype == (*msgtype & 0xf) && ts_seqid == ntohs(*seqid)); 356 } 357 358 static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type) 359 { 360 u64 ns = 0; 361 struct cpts_event *event; 362 struct list_head *this, *next; 363 unsigned int class = ptp_classify_raw(skb); 364 unsigned long flags; 365 u16 seqid; 366 u8 mtype; 367 368 if (class == PTP_CLASS_NONE) 369 return 0; 370 371 spin_lock_irqsave(&cpts->lock, flags); 372 cpts_fifo_read(cpts, -1); 373 list_for_each_safe(this, next, &cpts->events) { 374 event = list_entry(this, struct cpts_event, list); 375 if (event_expired(event)) { 376 list_del_init(&event->list); 377 list_add(&event->list, &cpts->pool); 378 continue; 379 } 380 mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK; 381 seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK; 382 if (ev_type == event_type(event) && 383 cpts_match(skb, class, seqid, mtype)) { 384 ns = timecounter_cyc2time(&cpts->tc, event->low); 385 list_del_init(&event->list); 386 list_add(&event->list, &cpts->pool); 387 break; 388 } 389 } 390 391 if (ev_type == CPTS_EV_TX && !ns) { 392 struct cpts_skb_cb_data *skb_cb = 393 (struct cpts_skb_cb_data *)skb->cb; 394 /* Not found, add frame to queue for processing later. 395 * The periodic FIFO check will handle this. 396 */ 397 skb_get(skb); 398 /* get the timestamp for timeouts */ 399 skb_cb->tmo = jiffies + msecs_to_jiffies(100); 400 __skb_queue_tail(&cpts->txq, skb); 401 ptp_schedule_worker(cpts->clock, 0); 402 } 403 spin_unlock_irqrestore(&cpts->lock, flags); 404 405 return ns; 406 } 407 408 void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb) 409 { 410 u64 ns; 411 struct skb_shared_hwtstamps *ssh; 412 413 if (!cpts->rx_enable) 414 return; 415 ns = cpts_find_ts(cpts, skb, CPTS_EV_RX); 416 if (!ns) 417 return; 418 ssh = skb_hwtstamps(skb); 419 memset(ssh, 0, sizeof(*ssh)); 420 ssh->hwtstamp = ns_to_ktime(ns); 421 } 422 EXPORT_SYMBOL_GPL(cpts_rx_timestamp); 423 424 void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb) 425 { 426 u64 ns; 427 struct skb_shared_hwtstamps ssh; 428 429 if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) 430 return; 431 ns = cpts_find_ts(cpts, skb, CPTS_EV_TX); 432 if (!ns) 433 return; 434 memset(&ssh, 0, sizeof(ssh)); 435 ssh.hwtstamp = ns_to_ktime(ns); 436 skb_tstamp_tx(skb, &ssh); 437 } 438 EXPORT_SYMBOL_GPL(cpts_tx_timestamp); 439 440 int cpts_register(struct cpts *cpts) 441 { 442 int err, i; 443 444 skb_queue_head_init(&cpts->txq); 445 INIT_LIST_HEAD(&cpts->events); 446 INIT_LIST_HEAD(&cpts->pool); 447 for (i = 0; i < CPTS_MAX_EVENTS; i++) 448 list_add(&cpts->pool_data[i].list, &cpts->pool); 449 450 clk_enable(cpts->refclk); 451 452 cpts_write32(cpts, CPTS_EN, control); 453 cpts_write32(cpts, TS_PEND_EN, int_enable); 454 455 timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real())); 456 457 cpts->clock = ptp_clock_register(&cpts->info, cpts->dev); 458 if (IS_ERR(cpts->clock)) { 459 err = PTR_ERR(cpts->clock); 460 cpts->clock = NULL; 461 goto err_ptp; 462 } 463 cpts->phc_index = ptp_clock_index(cpts->clock); 464 465 ptp_schedule_worker(cpts->clock, cpts->ov_check_period); 466 return 0; 467 468 err_ptp: 469 clk_disable(cpts->refclk); 470 return err; 471 } 472 EXPORT_SYMBOL_GPL(cpts_register); 473 474 void cpts_unregister(struct cpts *cpts) 475 { 476 if (WARN_ON(!cpts->clock)) 477 return; 478 479 ptp_clock_unregister(cpts->clock); 480 cpts->clock = NULL; 481 482 cpts_write32(cpts, 0, int_enable); 483 cpts_write32(cpts, 0, control); 484 485 /* Drop all packet */ 486 skb_queue_purge(&cpts->txq); 487 488 clk_disable(cpts->refclk); 489 } 490 EXPORT_SYMBOL_GPL(cpts_unregister); 491 492 static void cpts_calc_mult_shift(struct cpts *cpts) 493 { 494 u64 frac, maxsec, ns; 495 u32 freq; 496 497 freq = clk_get_rate(cpts->refclk); 498 499 /* Calc the maximum number of seconds which we can run before 500 * wrapping around. 501 */ 502 maxsec = cpts->cc.mask; 503 do_div(maxsec, freq); 504 /* limit conversation rate to 10 sec as higher values will produce 505 * too small mult factors and so reduce the conversion accuracy 506 */ 507 if (maxsec > 10) 508 maxsec = 10; 509 510 /* Calc overflow check period (maxsec / 2) */ 511 cpts->ov_check_period = (HZ * maxsec) / 2; 512 dev_info(cpts->dev, "cpts: overflow check period %lu (jiffies)\n", 513 cpts->ov_check_period); 514 515 if (cpts->cc.mult || cpts->cc.shift) 516 return; 517 518 clocks_calc_mult_shift(&cpts->cc.mult, &cpts->cc.shift, 519 freq, NSEC_PER_SEC, maxsec); 520 521 frac = 0; 522 ns = cyclecounter_cyc2ns(&cpts->cc, freq, cpts->cc.mask, &frac); 523 524 dev_info(cpts->dev, 525 "CPTS: ref_clk_freq:%u calc_mult:%u calc_shift:%u error:%lld nsec/sec\n", 526 freq, cpts->cc.mult, cpts->cc.shift, (ns - NSEC_PER_SEC)); 527 } 528 529 static int cpts_of_parse(struct cpts *cpts, struct device_node *node) 530 { 531 int ret = -EINVAL; 532 u32 prop; 533 534 if (!of_property_read_u32(node, "cpts_clock_mult", &prop)) 535 cpts->cc.mult = prop; 536 537 if (!of_property_read_u32(node, "cpts_clock_shift", &prop)) 538 cpts->cc.shift = prop; 539 540 if ((cpts->cc.mult && !cpts->cc.shift) || 541 (!cpts->cc.mult && cpts->cc.shift)) 542 goto of_error; 543 544 return 0; 545 546 of_error: 547 dev_err(cpts->dev, "CPTS: Missing property in the DT.\n"); 548 return ret; 549 } 550 551 struct cpts *cpts_create(struct device *dev, void __iomem *regs, 552 struct device_node *node) 553 { 554 struct cpts *cpts; 555 int ret; 556 557 cpts = devm_kzalloc(dev, sizeof(*cpts), GFP_KERNEL); 558 if (!cpts) 559 return ERR_PTR(-ENOMEM); 560 561 cpts->dev = dev; 562 cpts->reg = (struct cpsw_cpts __iomem *)regs; 563 spin_lock_init(&cpts->lock); 564 565 ret = cpts_of_parse(cpts, node); 566 if (ret) 567 return ERR_PTR(ret); 568 569 cpts->refclk = devm_clk_get(dev, "cpts"); 570 if (IS_ERR(cpts->refclk)) { 571 dev_err(dev, "Failed to get cpts refclk\n"); 572 return ERR_CAST(cpts->refclk); 573 } 574 575 clk_prepare(cpts->refclk); 576 577 cpts->cc.read = cpts_systim_read; 578 cpts->cc.mask = CLOCKSOURCE_MASK(32); 579 cpts->info = cpts_info; 580 581 cpts_calc_mult_shift(cpts); 582 /* save cc.mult original value as it can be modified 583 * by cpts_ptp_adjfreq(). 584 */ 585 cpts->cc_mult = cpts->cc.mult; 586 587 return cpts; 588 } 589 EXPORT_SYMBOL_GPL(cpts_create); 590 591 void cpts_release(struct cpts *cpts) 592 { 593 if (!cpts) 594 return; 595 596 if (WARN_ON(!cpts->refclk)) 597 return; 598 599 clk_unprepare(cpts->refclk); 600 } 601 EXPORT_SYMBOL_GPL(cpts_release); 602 603 MODULE_LICENSE("GPL v2"); 604 MODULE_DESCRIPTION("TI CPTS driver"); 605 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); 606