]>
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 | ||
34 | #ifdef CONFIG_TI_CPTS | |
35 | ||
87c0e764 RC |
36 | #define cpts_read32(c, r) __raw_readl(&c->reg->r) |
37 | #define cpts_write32(c, v, r) __raw_writel(v, &c->reg->r) | |
38 | ||
39 | static int event_expired(struct cpts_event *event) | |
40 | { | |
41 | return time_after(jiffies, event->tmo); | |
42 | } | |
43 | ||
44 | static int event_type(struct cpts_event *event) | |
45 | { | |
46 | return (event->high >> EVENT_TYPE_SHIFT) & EVENT_TYPE_MASK; | |
47 | } | |
48 | ||
49 | static int cpts_fifo_pop(struct cpts *cpts, u32 *high, u32 *low) | |
50 | { | |
51 | u32 r = cpts_read32(cpts, intstat_raw); | |
52 | ||
53 | if (r & TS_PEND_RAW) { | |
54 | *high = cpts_read32(cpts, event_high); | |
55 | *low = cpts_read32(cpts, event_low); | |
56 | cpts_write32(cpts, EVENT_POP, event_pop); | |
57 | return 0; | |
58 | } | |
59 | return -1; | |
60 | } | |
61 | ||
62 | /* | |
63 | * Returns zero if matching event type was found. | |
64 | */ | |
65 | static int cpts_fifo_read(struct cpts *cpts, int match) | |
66 | { | |
67 | int i, type = -1; | |
68 | u32 hi, lo; | |
69 | struct cpts_event *event; | |
70 | ||
71 | for (i = 0; i < CPTS_FIFO_DEPTH; i++) { | |
72 | if (cpts_fifo_pop(cpts, &hi, &lo)) | |
73 | break; | |
74 | if (list_empty(&cpts->pool)) { | |
75 | pr_err("cpts: event pool is empty\n"); | |
76 | return -1; | |
77 | } | |
78 | event = list_first_entry(&cpts->pool, struct cpts_event, list); | |
79 | event->tmo = jiffies + 2; | |
80 | event->high = hi; | |
81 | event->low = lo; | |
82 | type = event_type(event); | |
83 | switch (type) { | |
84 | case CPTS_EV_PUSH: | |
85 | case CPTS_EV_RX: | |
86 | case CPTS_EV_TX: | |
87 | list_del_init(&event->list); | |
88 | list_add_tail(&event->list, &cpts->events); | |
89 | break; | |
90 | case CPTS_EV_ROLL: | |
91 | case CPTS_EV_HALF: | |
92 | case CPTS_EV_HW: | |
93 | break; | |
94 | default: | |
07f42258 | 95 | pr_err("cpts: unknown event type\n"); |
87c0e764 RC |
96 | break; |
97 | } | |
98 | if (type == match) | |
99 | break; | |
100 | } | |
101 | return type == match ? 0 : -1; | |
102 | } | |
103 | ||
104 | static cycle_t cpts_systim_read(const struct cyclecounter *cc) | |
105 | { | |
106 | u64 val = 0; | |
107 | struct cpts_event *event; | |
108 | struct list_head *this, *next; | |
109 | struct cpts *cpts = container_of(cc, struct cpts, cc); | |
110 | ||
111 | cpts_write32(cpts, TS_PUSH, ts_push); | |
112 | if (cpts_fifo_read(cpts, CPTS_EV_PUSH)) | |
113 | pr_err("cpts: unable to obtain a time stamp\n"); | |
114 | ||
115 | list_for_each_safe(this, next, &cpts->events) { | |
116 | event = list_entry(this, struct cpts_event, list); | |
117 | if (event_type(event) == CPTS_EV_PUSH) { | |
118 | list_del_init(&event->list); | |
119 | list_add(&event->list, &cpts->pool); | |
120 | val = event->low; | |
121 | break; | |
122 | } | |
123 | } | |
124 | ||
125 | return val; | |
126 | } | |
127 | ||
128 | /* PTP clock operations */ | |
129 | ||
130 | static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb) | |
131 | { | |
132 | u64 adj; | |
133 | u32 diff, mult; | |
134 | int neg_adj = 0; | |
135 | unsigned long flags; | |
136 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
137 | ||
138 | if (ppb < 0) { | |
139 | neg_adj = 1; | |
140 | ppb = -ppb; | |
141 | } | |
142 | mult = cpts->cc_mult; | |
143 | adj = mult; | |
144 | adj *= ppb; | |
145 | diff = div_u64(adj, 1000000000ULL); | |
146 | ||
147 | spin_lock_irqsave(&cpts->lock, flags); | |
148 | ||
149 | timecounter_read(&cpts->tc); | |
150 | ||
151 | cpts->cc.mult = neg_adj ? mult - diff : mult + diff; | |
152 | ||
153 | spin_unlock_irqrestore(&cpts->lock, flags); | |
154 | ||
155 | return 0; | |
156 | } | |
157 | ||
158 | static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) | |
159 | { | |
87c0e764 RC |
160 | unsigned long flags; |
161 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
162 | ||
163 | spin_lock_irqsave(&cpts->lock, flags); | |
f25a30be | 164 | timecounter_adjtime(&cpts->tc, delta); |
87c0e764 RC |
165 | spin_unlock_irqrestore(&cpts->lock, flags); |
166 | ||
167 | return 0; | |
168 | } | |
169 | ||
a5c79c26 | 170 | static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) |
87c0e764 RC |
171 | { |
172 | u64 ns; | |
173 | u32 remainder; | |
174 | unsigned long flags; | |
175 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
176 | ||
177 | spin_lock_irqsave(&cpts->lock, flags); | |
178 | ns = timecounter_read(&cpts->tc); | |
179 | spin_unlock_irqrestore(&cpts->lock, flags); | |
180 | ||
181 | ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder); | |
182 | ts->tv_nsec = remainder; | |
183 | ||
184 | return 0; | |
185 | } | |
186 | ||
187 | static int cpts_ptp_settime(struct ptp_clock_info *ptp, | |
a5c79c26 | 188 | const struct timespec64 *ts) |
87c0e764 RC |
189 | { |
190 | u64 ns; | |
191 | unsigned long flags; | |
192 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
193 | ||
194 | ns = ts->tv_sec * 1000000000ULL; | |
195 | ns += ts->tv_nsec; | |
196 | ||
197 | spin_lock_irqsave(&cpts->lock, flags); | |
198 | timecounter_init(&cpts->tc, &cpts->cc, ns); | |
199 | spin_unlock_irqrestore(&cpts->lock, flags); | |
200 | ||
201 | return 0; | |
202 | } | |
203 | ||
204 | static int cpts_ptp_enable(struct ptp_clock_info *ptp, | |
205 | struct ptp_clock_request *rq, int on) | |
206 | { | |
207 | return -EOPNOTSUPP; | |
208 | } | |
209 | ||
210 | static struct ptp_clock_info cpts_info = { | |
211 | .owner = THIS_MODULE, | |
212 | .name = "CTPS timer", | |
213 | .max_adj = 1000000, | |
214 | .n_ext_ts = 0, | |
4986b4f0 | 215 | .n_pins = 0, |
87c0e764 RC |
216 | .pps = 0, |
217 | .adjfreq = cpts_ptp_adjfreq, | |
218 | .adjtime = cpts_ptp_adjtime, | |
a5c79c26 RC |
219 | .gettime64 = cpts_ptp_gettime, |
220 | .settime64 = cpts_ptp_settime, | |
87c0e764 RC |
221 | .enable = cpts_ptp_enable, |
222 | }; | |
223 | ||
224 | static void cpts_overflow_check(struct work_struct *work) | |
225 | { | |
a5c79c26 | 226 | struct timespec64 ts; |
87c0e764 RC |
227 | struct cpts *cpts = container_of(work, struct cpts, overflow_work.work); |
228 | ||
229 | cpts_write32(cpts, CPTS_EN, control); | |
230 | cpts_write32(cpts, TS_PEND_EN, int_enable); | |
231 | cpts_ptp_gettime(&cpts->info, &ts); | |
a5c79c26 | 232 | pr_debug("cpts overflow check at %lld.%09lu\n", ts.tv_sec, ts.tv_nsec); |
87c0e764 RC |
233 | schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD); |
234 | } | |
235 | ||
d0415e7c | 236 | static void cpts_clk_init(struct device *dev, struct cpts *cpts) |
87c0e764 | 237 | { |
d0415e7c | 238 | cpts->refclk = devm_clk_get(dev, "cpts"); |
87c0e764 | 239 | if (IS_ERR(cpts->refclk)) { |
d0415e7c | 240 | dev_err(dev, "Failed to get cpts refclk\n"); |
87c0e764 RC |
241 | cpts->refclk = NULL; |
242 | return; | |
243 | } | |
ccb6e984 | 244 | clk_prepare_enable(cpts->refclk); |
87c0e764 RC |
245 | } |
246 | ||
247 | static void cpts_clk_release(struct cpts *cpts) | |
248 | { | |
249 | clk_disable(cpts->refclk); | |
87c0e764 RC |
250 | } |
251 | ||
252 | static int cpts_match(struct sk_buff *skb, unsigned int ptp_class, | |
253 | u16 ts_seqid, u8 ts_msgtype) | |
254 | { | |
255 | u16 *seqid; | |
ae5c6c6d | 256 | unsigned int offset = 0; |
87c0e764 RC |
257 | u8 *msgtype, *data = skb->data; |
258 | ||
ae5c6c6d SS |
259 | if (ptp_class & PTP_CLASS_VLAN) |
260 | offset += VLAN_HLEN; | |
261 | ||
262 | switch (ptp_class & PTP_CLASS_PMASK) { | |
263 | case PTP_CLASS_IPV4: | |
cca04b28 | 264 | offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; |
87c0e764 | 265 | break; |
ae5c6c6d SS |
266 | case PTP_CLASS_IPV6: |
267 | offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; | |
87c0e764 | 268 | break; |
ae5c6c6d SS |
269 | case PTP_CLASS_L2: |
270 | offset += ETH_HLEN; | |
87c0e764 RC |
271 | break; |
272 | default: | |
273 | return 0; | |
274 | } | |
275 | ||
276 | if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) | |
277 | return 0; | |
278 | ||
279 | if (unlikely(ptp_class & PTP_CLASS_V1)) | |
280 | msgtype = data + offset + OFF_PTP_CONTROL; | |
281 | else | |
282 | msgtype = data + offset; | |
283 | ||
284 | seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); | |
285 | ||
286 | return (ts_msgtype == (*msgtype & 0xf) && ts_seqid == ntohs(*seqid)); | |
287 | } | |
288 | ||
289 | static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type) | |
290 | { | |
291 | u64 ns = 0; | |
292 | struct cpts_event *event; | |
293 | struct list_head *this, *next; | |
164d8c66 | 294 | unsigned int class = ptp_classify_raw(skb); |
87c0e764 RC |
295 | unsigned long flags; |
296 | u16 seqid; | |
297 | u8 mtype; | |
298 | ||
299 | if (class == PTP_CLASS_NONE) | |
300 | return 0; | |
301 | ||
302 | spin_lock_irqsave(&cpts->lock, flags); | |
303 | cpts_fifo_read(cpts, CPTS_EV_PUSH); | |
304 | list_for_each_safe(this, next, &cpts->events) { | |
305 | event = list_entry(this, struct cpts_event, list); | |
306 | if (event_expired(event)) { | |
307 | list_del_init(&event->list); | |
308 | list_add(&event->list, &cpts->pool); | |
309 | continue; | |
310 | } | |
311 | mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK; | |
312 | seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK; | |
313 | if (ev_type == event_type(event) && | |
314 | cpts_match(skb, class, seqid, mtype)) { | |
315 | ns = timecounter_cyc2time(&cpts->tc, event->low); | |
316 | list_del_init(&event->list); | |
317 | list_add(&event->list, &cpts->pool); | |
318 | break; | |
319 | } | |
320 | } | |
321 | spin_unlock_irqrestore(&cpts->lock, flags); | |
322 | ||
323 | return ns; | |
324 | } | |
325 | ||
326 | void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb) | |
327 | { | |
328 | u64 ns; | |
329 | struct skb_shared_hwtstamps *ssh; | |
330 | ||
331 | if (!cpts->rx_enable) | |
332 | return; | |
333 | ns = cpts_find_ts(cpts, skb, CPTS_EV_RX); | |
334 | if (!ns) | |
335 | return; | |
336 | ssh = skb_hwtstamps(skb); | |
337 | memset(ssh, 0, sizeof(*ssh)); | |
338 | ssh->hwtstamp = ns_to_ktime(ns); | |
339 | } | |
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 | ||
356 | #endif /*CONFIG_TI_CPTS*/ | |
357 | ||
358 | int cpts_register(struct device *dev, struct cpts *cpts, | |
359 | u32 mult, u32 shift) | |
360 | { | |
361 | #ifdef CONFIG_TI_CPTS | |
362 | int err, i; | |
363 | unsigned long flags; | |
364 | ||
87c0e764 RC |
365 | cpts->info = cpts_info; |
366 | cpts->clock = ptp_clock_register(&cpts->info, dev); | |
367 | if (IS_ERR(cpts->clock)) { | |
368 | err = PTR_ERR(cpts->clock); | |
369 | cpts->clock = NULL; | |
370 | return err; | |
371 | } | |
372 | spin_lock_init(&cpts->lock); | |
373 | ||
374 | cpts->cc.read = cpts_systim_read; | |
375 | cpts->cc.mask = CLOCKSOURCE_MASK(32); | |
376 | cpts->cc_mult = mult; | |
377 | cpts->cc.mult = mult; | |
378 | cpts->cc.shift = shift; | |
379 | ||
380 | INIT_LIST_HEAD(&cpts->events); | |
381 | INIT_LIST_HEAD(&cpts->pool); | |
382 | for (i = 0; i < CPTS_MAX_EVENTS; i++) | |
383 | list_add(&cpts->pool_data[i].list, &cpts->pool); | |
384 | ||
d0415e7c | 385 | cpts_clk_init(dev, cpts); |
87c0e764 RC |
386 | cpts_write32(cpts, CPTS_EN, control); |
387 | cpts_write32(cpts, TS_PEND_EN, int_enable); | |
388 | ||
389 | spin_lock_irqsave(&cpts->lock, flags); | |
390 | timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real())); | |
391 | spin_unlock_irqrestore(&cpts->lock, flags); | |
392 | ||
393 | INIT_DELAYED_WORK(&cpts->overflow_work, cpts_overflow_check); | |
394 | schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD); | |
395 | ||
396 | cpts->phc_index = ptp_clock_index(cpts->clock); | |
397 | #endif | |
398 | return 0; | |
399 | } | |
400 | ||
401 | void cpts_unregister(struct cpts *cpts) | |
402 | { | |
403 | #ifdef CONFIG_TI_CPTS | |
404 | if (cpts->clock) { | |
405 | ptp_clock_unregister(cpts->clock); | |
406 | cancel_delayed_work_sync(&cpts->overflow_work); | |
407 | } | |
408 | if (cpts->refclk) | |
409 | cpts_clk_release(cpts); | |
410 | #endif | |
411 | } |