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