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; | |
87c0e764 RC |
173 | unsigned long flags; |
174 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
175 | ||
176 | spin_lock_irqsave(&cpts->lock, flags); | |
177 | ns = timecounter_read(&cpts->tc); | |
178 | spin_unlock_irqrestore(&cpts->lock, flags); | |
179 | ||
84d923ce | 180 | *ts = ns_to_timespec64(ns); |
87c0e764 RC |
181 | |
182 | return 0; | |
183 | } | |
184 | ||
185 | static int cpts_ptp_settime(struct ptp_clock_info *ptp, | |
a5c79c26 | 186 | const struct timespec64 *ts) |
87c0e764 RC |
187 | { |
188 | u64 ns; | |
189 | unsigned long flags; | |
190 | struct cpts *cpts = container_of(ptp, struct cpts, info); | |
191 | ||
84d923ce | 192 | ns = timespec64_to_ns(ts); |
87c0e764 RC |
193 | |
194 | spin_lock_irqsave(&cpts->lock, flags); | |
195 | timecounter_init(&cpts->tc, &cpts->cc, ns); | |
196 | spin_unlock_irqrestore(&cpts->lock, flags); | |
197 | ||
198 | return 0; | |
199 | } | |
200 | ||
201 | static int cpts_ptp_enable(struct ptp_clock_info *ptp, | |
202 | struct ptp_clock_request *rq, int on) | |
203 | { | |
204 | return -EOPNOTSUPP; | |
205 | } | |
206 | ||
207 | static struct ptp_clock_info cpts_info = { | |
208 | .owner = THIS_MODULE, | |
209 | .name = "CTPS timer", | |
210 | .max_adj = 1000000, | |
211 | .n_ext_ts = 0, | |
4986b4f0 | 212 | .n_pins = 0, |
87c0e764 RC |
213 | .pps = 0, |
214 | .adjfreq = cpts_ptp_adjfreq, | |
215 | .adjtime = cpts_ptp_adjtime, | |
a5c79c26 RC |
216 | .gettime64 = cpts_ptp_gettime, |
217 | .settime64 = cpts_ptp_settime, | |
87c0e764 RC |
218 | .enable = cpts_ptp_enable, |
219 | }; | |
220 | ||
221 | static void cpts_overflow_check(struct work_struct *work) | |
222 | { | |
a5c79c26 | 223 | struct timespec64 ts; |
87c0e764 RC |
224 | struct cpts *cpts = container_of(work, struct cpts, overflow_work.work); |
225 | ||
226 | cpts_write32(cpts, CPTS_EN, control); | |
227 | cpts_write32(cpts, TS_PEND_EN, int_enable); | |
228 | cpts_ptp_gettime(&cpts->info, &ts); | |
a5c79c26 | 229 | pr_debug("cpts overflow check at %lld.%09lu\n", ts.tv_sec, ts.tv_nsec); |
87c0e764 RC |
230 | schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD); |
231 | } | |
232 | ||
d0415e7c | 233 | static void cpts_clk_init(struct device *dev, struct cpts *cpts) |
87c0e764 | 234 | { |
d0415e7c | 235 | cpts->refclk = devm_clk_get(dev, "cpts"); |
87c0e764 | 236 | if (IS_ERR(cpts->refclk)) { |
d0415e7c | 237 | dev_err(dev, "Failed to get cpts refclk\n"); |
87c0e764 RC |
238 | cpts->refclk = NULL; |
239 | return; | |
240 | } | |
ccb6e984 | 241 | clk_prepare_enable(cpts->refclk); |
87c0e764 RC |
242 | } |
243 | ||
244 | static void cpts_clk_release(struct cpts *cpts) | |
245 | { | |
246 | clk_disable(cpts->refclk); | |
87c0e764 RC |
247 | } |
248 | ||
249 | static int cpts_match(struct sk_buff *skb, unsigned int ptp_class, | |
250 | u16 ts_seqid, u8 ts_msgtype) | |
251 | { | |
252 | u16 *seqid; | |
ae5c6c6d | 253 | unsigned int offset = 0; |
87c0e764 RC |
254 | u8 *msgtype, *data = skb->data; |
255 | ||
ae5c6c6d SS |
256 | if (ptp_class & PTP_CLASS_VLAN) |
257 | offset += VLAN_HLEN; | |
258 | ||
259 | switch (ptp_class & PTP_CLASS_PMASK) { | |
260 | case PTP_CLASS_IPV4: | |
cca04b28 | 261 | offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; |
87c0e764 | 262 | break; |
ae5c6c6d SS |
263 | case PTP_CLASS_IPV6: |
264 | offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; | |
87c0e764 | 265 | break; |
ae5c6c6d SS |
266 | case PTP_CLASS_L2: |
267 | offset += ETH_HLEN; | |
87c0e764 RC |
268 | break; |
269 | default: | |
270 | return 0; | |
271 | } | |
272 | ||
273 | if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) | |
274 | return 0; | |
275 | ||
276 | if (unlikely(ptp_class & PTP_CLASS_V1)) | |
277 | msgtype = data + offset + OFF_PTP_CONTROL; | |
278 | else | |
279 | msgtype = data + offset; | |
280 | ||
281 | seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); | |
282 | ||
283 | return (ts_msgtype == (*msgtype & 0xf) && ts_seqid == ntohs(*seqid)); | |
284 | } | |
285 | ||
286 | static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type) | |
287 | { | |
288 | u64 ns = 0; | |
289 | struct cpts_event *event; | |
290 | struct list_head *this, *next; | |
164d8c66 | 291 | unsigned int class = ptp_classify_raw(skb); |
87c0e764 RC |
292 | unsigned long flags; |
293 | u16 seqid; | |
294 | u8 mtype; | |
295 | ||
296 | if (class == PTP_CLASS_NONE) | |
297 | return 0; | |
298 | ||
299 | spin_lock_irqsave(&cpts->lock, flags); | |
300 | cpts_fifo_read(cpts, CPTS_EV_PUSH); | |
301 | list_for_each_safe(this, next, &cpts->events) { | |
302 | event = list_entry(this, struct cpts_event, list); | |
303 | if (event_expired(event)) { | |
304 | list_del_init(&event->list); | |
305 | list_add(&event->list, &cpts->pool); | |
306 | continue; | |
307 | } | |
308 | mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK; | |
309 | seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK; | |
310 | if (ev_type == event_type(event) && | |
311 | cpts_match(skb, class, seqid, mtype)) { | |
312 | ns = timecounter_cyc2time(&cpts->tc, event->low); | |
313 | list_del_init(&event->list); | |
314 | list_add(&event->list, &cpts->pool); | |
315 | break; | |
316 | } | |
317 | } | |
318 | spin_unlock_irqrestore(&cpts->lock, flags); | |
319 | ||
320 | return ns; | |
321 | } | |
322 | ||
323 | void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb) | |
324 | { | |
325 | u64 ns; | |
326 | struct skb_shared_hwtstamps *ssh; | |
327 | ||
328 | if (!cpts->rx_enable) | |
329 | return; | |
330 | ns = cpts_find_ts(cpts, skb, CPTS_EV_RX); | |
331 | if (!ns) | |
332 | return; | |
333 | ssh = skb_hwtstamps(skb); | |
334 | memset(ssh, 0, sizeof(*ssh)); | |
335 | ssh->hwtstamp = ns_to_ktime(ns); | |
336 | } | |
337 | ||
338 | void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb) | |
339 | { | |
340 | u64 ns; | |
341 | struct skb_shared_hwtstamps ssh; | |
342 | ||
343 | if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) | |
344 | return; | |
345 | ns = cpts_find_ts(cpts, skb, CPTS_EV_TX); | |
346 | if (!ns) | |
347 | return; | |
348 | memset(&ssh, 0, sizeof(ssh)); | |
349 | ssh.hwtstamp = ns_to_ktime(ns); | |
350 | skb_tstamp_tx(skb, &ssh); | |
351 | } | |
352 | ||
353 | #endif /*CONFIG_TI_CPTS*/ | |
354 | ||
355 | int cpts_register(struct device *dev, struct cpts *cpts, | |
356 | u32 mult, u32 shift) | |
357 | { | |
358 | #ifdef CONFIG_TI_CPTS | |
359 | int err, i; | |
360 | unsigned long flags; | |
361 | ||
87c0e764 RC |
362 | cpts->info = cpts_info; |
363 | cpts->clock = ptp_clock_register(&cpts->info, dev); | |
364 | if (IS_ERR(cpts->clock)) { | |
365 | err = PTR_ERR(cpts->clock); | |
366 | cpts->clock = NULL; | |
367 | return err; | |
368 | } | |
369 | spin_lock_init(&cpts->lock); | |
370 | ||
371 | cpts->cc.read = cpts_systim_read; | |
372 | cpts->cc.mask = CLOCKSOURCE_MASK(32); | |
373 | cpts->cc_mult = mult; | |
374 | cpts->cc.mult = mult; | |
375 | cpts->cc.shift = shift; | |
376 | ||
377 | INIT_LIST_HEAD(&cpts->events); | |
378 | INIT_LIST_HEAD(&cpts->pool); | |
379 | for (i = 0; i < CPTS_MAX_EVENTS; i++) | |
380 | list_add(&cpts->pool_data[i].list, &cpts->pool); | |
381 | ||
d0415e7c | 382 | cpts_clk_init(dev, cpts); |
87c0e764 RC |
383 | cpts_write32(cpts, CPTS_EN, control); |
384 | cpts_write32(cpts, TS_PEND_EN, int_enable); | |
385 | ||
386 | spin_lock_irqsave(&cpts->lock, flags); | |
387 | timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real())); | |
388 | spin_unlock_irqrestore(&cpts->lock, flags); | |
389 | ||
390 | INIT_DELAYED_WORK(&cpts->overflow_work, cpts_overflow_check); | |
391 | schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD); | |
392 | ||
393 | cpts->phc_index = ptp_clock_index(cpts->clock); | |
394 | #endif | |
395 | return 0; | |
396 | } | |
397 | ||
398 | void cpts_unregister(struct cpts *cpts) | |
399 | { | |
400 | #ifdef CONFIG_TI_CPTS | |
401 | if (cpts->clock) { | |
402 | ptp_clock_unregister(cpts->clock); | |
403 | cancel_delayed_work_sync(&cpts->overflow_work); | |
404 | } | |
405 | if (cpts->refclk) | |
406 | cpts_clk_release(cpts); | |
407 | #endif | |
408 | } |