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