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