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_read32(c, r) readl_relaxed(&c->reg->r) 35 #define cpts_write32(c, v, r) writel_relaxed(v, &c->reg->r) 36 37 static int event_expired(struct cpts_event *event) 38 { 39 return time_after(jiffies, event->tmo); 40 } 41 42 static int event_type(struct cpts_event *event) 43 { 44 return (event->high >> EVENT_TYPE_SHIFT) & EVENT_TYPE_MASK; 45 } 46 47 static int cpts_fifo_pop(struct cpts *cpts, u32 *high, u32 *low) 48 { 49 u32 r = cpts_read32(cpts, intstat_raw); 50 51 if (r & TS_PEND_RAW) { 52 *high = cpts_read32(cpts, event_high); 53 *low = cpts_read32(cpts, event_low); 54 cpts_write32(cpts, EVENT_POP, event_pop); 55 return 0; 56 } 57 return -1; 58 } 59 60 static int cpts_purge_events(struct cpts *cpts) 61 { 62 struct list_head *this, *next; 63 struct cpts_event *event; 64 int removed = 0; 65 66 list_for_each_safe(this, next, &cpts->events) { 67 event = list_entry(this, struct cpts_event, list); 68 if (event_expired(event)) { 69 list_del_init(&event->list); 70 list_add(&event->list, &cpts->pool); 71 ++removed; 72 } 73 } 74 75 if (removed) 76 pr_debug("cpts: event pool cleaned up %d\n", removed); 77 return removed ? 0 : -1; 78 } 79 80 /* 81 * Returns zero if matching event type was found. 82 */ 83 static int cpts_fifo_read(struct cpts *cpts, int match) 84 { 85 int i, type = -1; 86 u32 hi, lo; 87 struct cpts_event *event; 88 89 for (i = 0; i < CPTS_FIFO_DEPTH; i++) { 90 if (cpts_fifo_pop(cpts, &hi, &lo)) 91 break; 92 93 if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) { 94 pr_err("cpts: event pool empty\n"); 95 return -1; 96 } 97 98 event = list_first_entry(&cpts->pool, struct cpts_event, list); 99 event->tmo = jiffies + 2; 100 event->high = hi; 101 event->low = lo; 102 type = event_type(event); 103 switch (type) { 104 case CPTS_EV_PUSH: 105 case CPTS_EV_RX: 106 case CPTS_EV_TX: 107 list_del_init(&event->list); 108 list_add_tail(&event->list, &cpts->events); 109 break; 110 case CPTS_EV_ROLL: 111 case CPTS_EV_HALF: 112 case CPTS_EV_HW: 113 break; 114 default: 115 pr_err("cpts: unknown event type\n"); 116 break; 117 } 118 if (type == match) 119 break; 120 } 121 return type == match ? 0 : -1; 122 } 123 124 static u64 cpts_systim_read(const struct cyclecounter *cc) 125 { 126 u64 val = 0; 127 struct cpts_event *event; 128 struct list_head *this, *next; 129 struct cpts *cpts = container_of(cc, struct cpts, cc); 130 131 cpts_write32(cpts, TS_PUSH, ts_push); 132 if (cpts_fifo_read(cpts, CPTS_EV_PUSH)) 133 pr_err("cpts: unable to obtain a time stamp\n"); 134 135 list_for_each_safe(this, next, &cpts->events) { 136 event = list_entry(this, struct cpts_event, list); 137 if (event_type(event) == CPTS_EV_PUSH) { 138 list_del_init(&event->list); 139 list_add(&event->list, &cpts->pool); 140 val = event->low; 141 break; 142 } 143 } 144 145 return val; 146 } 147 148 /* PTP clock operations */ 149 150 static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb) 151 { 152 u64 adj; 153 u32 diff, mult; 154 int neg_adj = 0; 155 unsigned long flags; 156 struct cpts *cpts = container_of(ptp, struct cpts, info); 157 158 if (ppb < 0) { 159 neg_adj = 1; 160 ppb = -ppb; 161 } 162 mult = cpts->cc_mult; 163 adj = mult; 164 adj *= ppb; 165 diff = div_u64(adj, 1000000000ULL); 166 167 spin_lock_irqsave(&cpts->lock, flags); 168 169 timecounter_read(&cpts->tc); 170 171 cpts->cc.mult = neg_adj ? mult - diff : mult + diff; 172 173 spin_unlock_irqrestore(&cpts->lock, flags); 174 175 return 0; 176 } 177 178 static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 179 { 180 unsigned long flags; 181 struct cpts *cpts = container_of(ptp, struct cpts, info); 182 183 spin_lock_irqsave(&cpts->lock, flags); 184 timecounter_adjtime(&cpts->tc, delta); 185 spin_unlock_irqrestore(&cpts->lock, flags); 186 187 return 0; 188 } 189 190 static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 191 { 192 u64 ns; 193 unsigned long flags; 194 struct cpts *cpts = container_of(ptp, struct cpts, info); 195 196 spin_lock_irqsave(&cpts->lock, flags); 197 ns = timecounter_read(&cpts->tc); 198 spin_unlock_irqrestore(&cpts->lock, flags); 199 200 *ts = ns_to_timespec64(ns); 201 202 return 0; 203 } 204 205 static int cpts_ptp_settime(struct ptp_clock_info *ptp, 206 const struct timespec64 *ts) 207 { 208 u64 ns; 209 unsigned long flags; 210 struct cpts *cpts = container_of(ptp, struct cpts, info); 211 212 ns = timespec64_to_ns(ts); 213 214 spin_lock_irqsave(&cpts->lock, flags); 215 timecounter_init(&cpts->tc, &cpts->cc, ns); 216 spin_unlock_irqrestore(&cpts->lock, flags); 217 218 return 0; 219 } 220 221 static int cpts_ptp_enable(struct ptp_clock_info *ptp, 222 struct ptp_clock_request *rq, int on) 223 { 224 return -EOPNOTSUPP; 225 } 226 227 static struct ptp_clock_info cpts_info = { 228 .owner = THIS_MODULE, 229 .name = "CTPS timer", 230 .max_adj = 1000000, 231 .n_ext_ts = 0, 232 .n_pins = 0, 233 .pps = 0, 234 .adjfreq = cpts_ptp_adjfreq, 235 .adjtime = cpts_ptp_adjtime, 236 .gettime64 = cpts_ptp_gettime, 237 .settime64 = cpts_ptp_settime, 238 .enable = cpts_ptp_enable, 239 }; 240 241 static void cpts_overflow_check(struct work_struct *work) 242 { 243 struct timespec64 ts; 244 struct cpts *cpts = container_of(work, struct cpts, overflow_work.work); 245 246 cpts_ptp_gettime(&cpts->info, &ts); 247 pr_debug("cpts overflow check at %lld.%09lu\n", ts.tv_sec, ts.tv_nsec); 248 schedule_delayed_work(&cpts->overflow_work, cpts->ov_check_period); 249 } 250 251 static int cpts_match(struct sk_buff *skb, unsigned int ptp_class, 252 u16 ts_seqid, u8 ts_msgtype) 253 { 254 u16 *seqid; 255 unsigned int offset = 0; 256 u8 *msgtype, *data = skb->data; 257 258 if (ptp_class & PTP_CLASS_VLAN) 259 offset += VLAN_HLEN; 260 261 switch (ptp_class & PTP_CLASS_PMASK) { 262 case PTP_CLASS_IPV4: 263 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; 264 break; 265 case PTP_CLASS_IPV6: 266 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; 267 break; 268 case PTP_CLASS_L2: 269 offset += ETH_HLEN; 270 break; 271 default: 272 return 0; 273 } 274 275 if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) 276 return 0; 277 278 if (unlikely(ptp_class & PTP_CLASS_V1)) 279 msgtype = data + offset + OFF_PTP_CONTROL; 280 else 281 msgtype = data + offset; 282 283 seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); 284 285 return (ts_msgtype == (*msgtype & 0xf) && ts_seqid == ntohs(*seqid)); 286 } 287 288 static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type) 289 { 290 u64 ns = 0; 291 struct cpts_event *event; 292 struct list_head *this, *next; 293 unsigned int class = ptp_classify_raw(skb); 294 unsigned long flags; 295 u16 seqid; 296 u8 mtype; 297 298 if (class == PTP_CLASS_NONE) 299 return 0; 300 301 spin_lock_irqsave(&cpts->lock, flags); 302 cpts_fifo_read(cpts, CPTS_EV_PUSH); 303 list_for_each_safe(this, next, &cpts->events) { 304 event = list_entry(this, struct cpts_event, list); 305 if (event_expired(event)) { 306 list_del_init(&event->list); 307 list_add(&event->list, &cpts->pool); 308 continue; 309 } 310 mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK; 311 seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK; 312 if (ev_type == event_type(event) && 313 cpts_match(skb, class, seqid, mtype)) { 314 ns = timecounter_cyc2time(&cpts->tc, event->low); 315 list_del_init(&event->list); 316 list_add(&event->list, &cpts->pool); 317 break; 318 } 319 } 320 spin_unlock_irqrestore(&cpts->lock, flags); 321 322 return ns; 323 } 324 325 void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb) 326 { 327 u64 ns; 328 struct skb_shared_hwtstamps *ssh; 329 330 if (!cpts->rx_enable) 331 return; 332 ns = cpts_find_ts(cpts, skb, CPTS_EV_RX); 333 if (!ns) 334 return; 335 ssh = skb_hwtstamps(skb); 336 memset(ssh, 0, sizeof(*ssh)); 337 ssh->hwtstamp = ns_to_ktime(ns); 338 } 339 EXPORT_SYMBOL_GPL(cpts_rx_timestamp); 340 341 void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb) 342 { 343 u64 ns; 344 struct skb_shared_hwtstamps ssh; 345 346 if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) 347 return; 348 ns = cpts_find_ts(cpts, skb, CPTS_EV_TX); 349 if (!ns) 350 return; 351 memset(&ssh, 0, sizeof(ssh)); 352 ssh.hwtstamp = ns_to_ktime(ns); 353 skb_tstamp_tx(skb, &ssh); 354 } 355 EXPORT_SYMBOL_GPL(cpts_tx_timestamp); 356 357 int cpts_register(struct cpts *cpts) 358 { 359 int err, i; 360 361 INIT_LIST_HEAD(&cpts->events); 362 INIT_LIST_HEAD(&cpts->pool); 363 for (i = 0; i < CPTS_MAX_EVENTS; i++) 364 list_add(&cpts->pool_data[i].list, &cpts->pool); 365 366 clk_enable(cpts->refclk); 367 368 cpts_write32(cpts, CPTS_EN, control); 369 cpts_write32(cpts, TS_PEND_EN, int_enable); 370 371 timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real())); 372 373 cpts->clock = ptp_clock_register(&cpts->info, cpts->dev); 374 if (IS_ERR(cpts->clock)) { 375 err = PTR_ERR(cpts->clock); 376 cpts->clock = NULL; 377 goto err_ptp; 378 } 379 cpts->phc_index = ptp_clock_index(cpts->clock); 380 381 schedule_delayed_work(&cpts->overflow_work, cpts->ov_check_period); 382 return 0; 383 384 err_ptp: 385 clk_disable(cpts->refclk); 386 return err; 387 } 388 EXPORT_SYMBOL_GPL(cpts_register); 389 390 void cpts_unregister(struct cpts *cpts) 391 { 392 if (WARN_ON(!cpts->clock)) 393 return; 394 395 cancel_delayed_work_sync(&cpts->overflow_work); 396 397 ptp_clock_unregister(cpts->clock); 398 cpts->clock = NULL; 399 400 cpts_write32(cpts, 0, int_enable); 401 cpts_write32(cpts, 0, control); 402 403 clk_disable(cpts->refclk); 404 } 405 EXPORT_SYMBOL_GPL(cpts_unregister); 406 407 static void cpts_calc_mult_shift(struct cpts *cpts) 408 { 409 u64 frac, maxsec, ns; 410 u32 freq; 411 412 freq = clk_get_rate(cpts->refclk); 413 414 /* Calc the maximum number of seconds which we can run before 415 * wrapping around. 416 */ 417 maxsec = cpts->cc.mask; 418 do_div(maxsec, freq); 419 /* limit conversation rate to 10 sec as higher values will produce 420 * too small mult factors and so reduce the conversion accuracy 421 */ 422 if (maxsec > 10) 423 maxsec = 10; 424 425 /* Calc overflow check period (maxsec / 2) */ 426 cpts->ov_check_period = (HZ * maxsec) / 2; 427 dev_info(cpts->dev, "cpts: overflow check period %lu (jiffies)\n", 428 cpts->ov_check_period); 429 430 if (cpts->cc.mult || cpts->cc.shift) 431 return; 432 433 clocks_calc_mult_shift(&cpts->cc.mult, &cpts->cc.shift, 434 freq, NSEC_PER_SEC, maxsec); 435 436 frac = 0; 437 ns = cyclecounter_cyc2ns(&cpts->cc, freq, cpts->cc.mask, &frac); 438 439 dev_info(cpts->dev, 440 "CPTS: ref_clk_freq:%u calc_mult:%u calc_shift:%u error:%lld nsec/sec\n", 441 freq, cpts->cc.mult, cpts->cc.shift, (ns - NSEC_PER_SEC)); 442 } 443 444 static int cpts_of_parse(struct cpts *cpts, struct device_node *node) 445 { 446 int ret = -EINVAL; 447 u32 prop; 448 449 if (!of_property_read_u32(node, "cpts_clock_mult", &prop)) 450 cpts->cc.mult = prop; 451 452 if (!of_property_read_u32(node, "cpts_clock_shift", &prop)) 453 cpts->cc.shift = prop; 454 455 if ((cpts->cc.mult && !cpts->cc.shift) || 456 (!cpts->cc.mult && cpts->cc.shift)) 457 goto of_error; 458 459 return 0; 460 461 of_error: 462 dev_err(cpts->dev, "CPTS: Missing property in the DT.\n"); 463 return ret; 464 } 465 466 struct cpts *cpts_create(struct device *dev, void __iomem *regs, 467 struct device_node *node) 468 { 469 struct cpts *cpts; 470 int ret; 471 472 cpts = devm_kzalloc(dev, sizeof(*cpts), GFP_KERNEL); 473 if (!cpts) 474 return ERR_PTR(-ENOMEM); 475 476 cpts->dev = dev; 477 cpts->reg = (struct cpsw_cpts __iomem *)regs; 478 spin_lock_init(&cpts->lock); 479 INIT_DELAYED_WORK(&cpts->overflow_work, cpts_overflow_check); 480 481 ret = cpts_of_parse(cpts, node); 482 if (ret) 483 return ERR_PTR(ret); 484 485 cpts->refclk = devm_clk_get(dev, "cpts"); 486 if (IS_ERR(cpts->refclk)) { 487 dev_err(dev, "Failed to get cpts refclk\n"); 488 return ERR_PTR(PTR_ERR(cpts->refclk)); 489 } 490 491 clk_prepare(cpts->refclk); 492 493 cpts->cc.read = cpts_systim_read; 494 cpts->cc.mask = CLOCKSOURCE_MASK(32); 495 cpts->info = cpts_info; 496 497 cpts_calc_mult_shift(cpts); 498 /* save cc.mult original value as it can be modified 499 * by cpts_ptp_adjfreq(). 500 */ 501 cpts->cc_mult = cpts->cc.mult; 502 503 return cpts; 504 } 505 EXPORT_SYMBOL_GPL(cpts_create); 506 507 void cpts_release(struct cpts *cpts) 508 { 509 if (!cpts) 510 return; 511 512 if (WARN_ON(!cpts->refclk)) 513 return; 514 515 clk_unprepare(cpts->refclk); 516 } 517 EXPORT_SYMBOL_GPL(cpts_release); 518 519 MODULE_LICENSE("GPL v2"); 520 MODULE_DESCRIPTION("TI CPTS driver"); 521 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); 522