Commit | Line | Data |
---|---|---|
87c0e764 RC |
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> | |
79eb9d28 AS |
29 | #include <linux/if_ether.h> |
30 | #include <linux/if_vlan.h> | |
87c0e764 | 31 | |
87c0e764 RC |
32 | #include "cpts.h" |
33 | ||
391fd6ca GS |
34 | #define cpts_read32(c, r) readl_relaxed(&c->reg->r) |
35 | #define cpts_write32(c, v, r) writel_relaxed(v, &c->reg->r) | |
87c0e764 RC |
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 | ||
e4439fa8 WK |
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 | ||
87c0e764 RC |
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; | |
e4439fa8 WK |
92 | |
93 | if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) { | |
94 | pr_err("cpts: event pool empty\n"); | |
87c0e764 RC |
95 | return -1; |
96 | } | |
e4439fa8 | 97 | |
87c0e764 RC |
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: | |
07f42258 | 115 | pr_err("cpts: unknown event type\n"); |
87c0e764 RC |
116 | break; |
117 | } | |
118 | if (type == match) | |
119 | break; | |
120 | } | |
121 | return type == match ? 0 : -1; | |
122 | } | |
123 | ||
a5a1d1c2 | 124 | static u64 cpts_systim_read(const struct cyclecounter *cc) |
87c0e764 RC |
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 | { | |
87c0e764 RC |
180 | unsigned long flags; |
181 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
182 | ||
183 | spin_lock_irqsave(&cpts->lock, flags); | |
f25a30be | 184 | timecounter_adjtime(&cpts->tc, delta); |
87c0e764 RC |
185 | spin_unlock_irqrestore(&cpts->lock, flags); |
186 | ||
187 | return 0; | |
188 | } | |
189 | ||
a5c79c26 | 190 | static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) |
87c0e764 RC |
191 | { |
192 | u64 ns; | |
87c0e764 RC |
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 | ||
84d923ce | 200 | *ts = ns_to_timespec64(ns); |
87c0e764 RC |
201 | |
202 | return 0; | |
203 | } | |
204 | ||
205 | static int cpts_ptp_settime(struct ptp_clock_info *ptp, | |
a5c79c26 | 206 | const struct timespec64 *ts) |
87c0e764 RC |
207 | { |
208 | u64 ns; | |
209 | unsigned long flags; | |
210 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
211 | ||
84d923ce | 212 | ns = timespec64_to_ns(ts); |
87c0e764 RC |
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, | |
4986b4f0 | 232 | .n_pins = 0, |
87c0e764 RC |
233 | .pps = 0, |
234 | .adjfreq = cpts_ptp_adjfreq, | |
235 | .adjtime = cpts_ptp_adjtime, | |
a5c79c26 RC |
236 | .gettime64 = cpts_ptp_gettime, |
237 | .settime64 = cpts_ptp_settime, | |
87c0e764 RC |
238 | .enable = cpts_ptp_enable, |
239 | }; | |
240 | ||
241 | static void cpts_overflow_check(struct work_struct *work) | |
242 | { | |
a5c79c26 | 243 | struct timespec64 ts; |
87c0e764 RC |
244 | struct cpts *cpts = container_of(work, struct cpts, overflow_work.work); |
245 | ||
87c0e764 | 246 | cpts_ptp_gettime(&cpts->info, &ts); |
a5c79c26 | 247 | pr_debug("cpts overflow check at %lld.%09lu\n", ts.tv_sec, ts.tv_nsec); |
20138cf9 | 248 | schedule_delayed_work(&cpts->overflow_work, cpts->ov_check_period); |
87c0e764 RC |
249 | } |
250 | ||
87c0e764 RC |
251 | static int cpts_match(struct sk_buff *skb, unsigned int ptp_class, |
252 | u16 ts_seqid, u8 ts_msgtype) | |
253 | { | |
254 | u16 *seqid; | |
ae5c6c6d | 255 | unsigned int offset = 0; |
87c0e764 RC |
256 | u8 *msgtype, *data = skb->data; |
257 | ||
ae5c6c6d SS |
258 | if (ptp_class & PTP_CLASS_VLAN) |
259 | offset += VLAN_HLEN; | |
260 | ||
261 | switch (ptp_class & PTP_CLASS_PMASK) { | |
262 | case PTP_CLASS_IPV4: | |
cca04b28 | 263 | offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; |
87c0e764 | 264 | break; |
ae5c6c6d SS |
265 | case PTP_CLASS_IPV6: |
266 | offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; | |
87c0e764 | 267 | break; |
ae5c6c6d SS |
268 | case PTP_CLASS_L2: |
269 | offset += ETH_HLEN; | |
87c0e764 RC |
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; | |
164d8c66 | 293 | unsigned int class = ptp_classify_raw(skb); |
87c0e764 RC |
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 | } | |
c8395d4e | 339 | EXPORT_SYMBOL_GPL(cpts_rx_timestamp); |
87c0e764 RC |
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 | } | |
c8395d4e | 355 | EXPORT_SYMBOL_GPL(cpts_tx_timestamp); |
87c0e764 | 356 | |
8a2c9a5a | 357 | int cpts_register(struct cpts *cpts) |
87c0e764 | 358 | { |
87c0e764 | 359 | int err, i; |
87c0e764 | 360 | |
87c0e764 RC |
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 | ||
8a2c9a5a GS |
366 | clk_enable(cpts->refclk); |
367 | ||
87c0e764 RC |
368 | cpts_write32(cpts, CPTS_EN, control); |
369 | cpts_write32(cpts, TS_PEND_EN, int_enable); | |
370 | ||
87c0e764 | 371 | timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real())); |
87c0e764 | 372 | |
8a2c9a5a | 373 | cpts->clock = ptp_clock_register(&cpts->info, cpts->dev); |
6c691405 GS |
374 | if (IS_ERR(cpts->clock)) { |
375 | err = PTR_ERR(cpts->clock); | |
376 | cpts->clock = NULL; | |
377 | goto err_ptp; | |
378 | } | |
87c0e764 | 379 | cpts->phc_index = ptp_clock_index(cpts->clock); |
6c691405 | 380 | |
20138cf9 | 381 | schedule_delayed_work(&cpts->overflow_work, cpts->ov_check_period); |
87c0e764 | 382 | return 0; |
6c691405 GS |
383 | |
384 | err_ptp: | |
8a2c9a5a | 385 | clk_disable(cpts->refclk); |
6c691405 | 386 | return err; |
87c0e764 | 387 | } |
c8395d4e | 388 | EXPORT_SYMBOL_GPL(cpts_register); |
87c0e764 RC |
389 | |
390 | void cpts_unregister(struct cpts *cpts) | |
391 | { | |
8a2c9a5a GS |
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; | |
8fcd6891 GS |
399 | |
400 | cpts_write32(cpts, 0, int_enable); | |
401 | cpts_write32(cpts, 0, control); | |
402 | ||
8a2c9a5a | 403 | clk_disable(cpts->refclk); |
87c0e764 | 404 | } |
c8395d4e GS |
405 | EXPORT_SYMBOL_GPL(cpts_unregister); |
406 | ||
88f0f0b0 GS |
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 | ||
20138cf9 GS |
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 | ||
88f0f0b0 GS |
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 | ||
4a88fb95 GS |
444 | static int cpts_of_parse(struct cpts *cpts, struct device_node *node) |
445 | { | |
446 | int ret = -EINVAL; | |
447 | u32 prop; | |
448 | ||
88f0f0b0 GS |
449 | if (!of_property_read_u32(node, "cpts_clock_mult", &prop)) |
450 | cpts->cc.mult = prop; | |
4a88fb95 | 451 | |
88f0f0b0 GS |
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; | |
4a88fb95 GS |
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 | ||
8a2c9a5a | 466 | struct cpts *cpts_create(struct device *dev, void __iomem *regs, |
4a88fb95 | 467 | struct device_node *node) |
8a2c9a5a GS |
468 | { |
469 | struct cpts *cpts; | |
4a88fb95 | 470 | int ret; |
8a2c9a5a GS |
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 | ||
4a88fb95 GS |
481 | ret = cpts_of_parse(cpts, node); |
482 | if (ret) | |
483 | return ERR_PTR(ret); | |
484 | ||
8a2c9a5a GS |
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); | |
88f0f0b0 GS |
495 | cpts->info = cpts_info; |
496 | ||
497 | cpts_calc_mult_shift(cpts); | |
4a88fb95 GS |
498 | /* save cc.mult original value as it can be modified |
499 | * by cpts_ptp_adjfreq(). | |
500 | */ | |
501 | cpts->cc_mult = cpts->cc.mult; | |
8a2c9a5a GS |
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 | ||
c8395d4e GS |
519 | MODULE_LICENSE("GPL v2"); |
520 | MODULE_DESCRIPTION("TI CPTS driver"); | |
521 | MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); |