1 | // SPDX-License-Identifier: (GPL-2.0 OR MIT) |
2 | /* |
3 | * DSA driver for: |
4 | * Hirschmann Hellcreek TSN switch. |
5 | * |
6 | * Copyright (C) 2019,2020 Hochschule Offenburg |
7 | * Copyright (C) 2019,2020 Linutronix GmbH |
8 | * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de> |
9 | * Kurt Kanzenbach <kurt@linutronix.de> |
10 | */ |
11 | |
12 | #include <linux/of.h> |
13 | #include <linux/ptp_clock_kernel.h> |
14 | #include "hellcreek.h" |
15 | #include "hellcreek_ptp.h" |
16 | #include "hellcreek_hwtstamp.h" |
17 | |
18 | u16 hellcreek_ptp_read(struct hellcreek *hellcreek, unsigned int offset) |
19 | { |
20 | return readw(addr: hellcreek->ptp_base + offset); |
21 | } |
22 | |
23 | void hellcreek_ptp_write(struct hellcreek *hellcreek, u16 data, |
24 | unsigned int offset) |
25 | { |
26 | writew(val: data, addr: hellcreek->ptp_base + offset); |
27 | } |
28 | |
29 | /* Get nanoseconds from PTP clock */ |
30 | static u64 hellcreek_ptp_clock_read(struct hellcreek *hellcreek) |
31 | { |
32 | u16 nsl, nsh; |
33 | |
34 | /* Take a snapshot */ |
35 | hellcreek_ptp_write(hellcreek, PR_COMMAND_C_SS, PR_COMMAND_C); |
36 | |
37 | /* The time of the day is saved as 96 bits. However, due to hardware |
38 | * limitations the seconds are not or only partly kept in the PTP |
39 | * core. Currently only three bits for the seconds are available. That's |
40 | * why only the nanoseconds are used and the seconds are tracked in |
41 | * software. Anyway due to internal locking all five registers should be |
42 | * read. |
43 | */ |
44 | nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C); |
45 | nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C); |
46 | nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C); |
47 | nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C); |
48 | nsl = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C); |
49 | |
50 | return (u64)nsl | ((u64)nsh << 16); |
51 | } |
52 | |
53 | static u64 __hellcreek_ptp_gettime(struct hellcreek *hellcreek) |
54 | { |
55 | u64 ns; |
56 | |
57 | ns = hellcreek_ptp_clock_read(hellcreek); |
58 | if (ns < hellcreek->last_ts) |
59 | hellcreek->seconds++; |
60 | hellcreek->last_ts = ns; |
61 | ns += hellcreek->seconds * NSEC_PER_SEC; |
62 | |
63 | return ns; |
64 | } |
65 | |
66 | /* Retrieve the seconds parts in nanoseconds for a packet timestamped with @ns. |
67 | * There has to be a check whether an overflow occurred between the packet |
68 | * arrival and now. If so use the correct seconds (-1) for calculating the |
69 | * packet arrival time. |
70 | */ |
71 | u64 hellcreek_ptp_gettime_seconds(struct hellcreek *hellcreek, u64 ns) |
72 | { |
73 | u64 s; |
74 | |
75 | __hellcreek_ptp_gettime(hellcreek); |
76 | if (hellcreek->last_ts > ns) |
77 | s = hellcreek->seconds * NSEC_PER_SEC; |
78 | else |
79 | s = (hellcreek->seconds - 1) * NSEC_PER_SEC; |
80 | |
81 | return s; |
82 | } |
83 | |
84 | static int hellcreek_ptp_gettime(struct ptp_clock_info *ptp, |
85 | struct timespec64 *ts) |
86 | { |
87 | struct hellcreek *hellcreek = ptp_to_hellcreek(ptp); |
88 | u64 ns; |
89 | |
90 | mutex_lock(&hellcreek->ptp_lock); |
91 | ns = __hellcreek_ptp_gettime(hellcreek); |
92 | mutex_unlock(lock: &hellcreek->ptp_lock); |
93 | |
94 | *ts = ns_to_timespec64(nsec: ns); |
95 | |
96 | return 0; |
97 | } |
98 | |
99 | static int hellcreek_ptp_settime(struct ptp_clock_info *ptp, |
100 | const struct timespec64 *ts) |
101 | { |
102 | struct hellcreek *hellcreek = ptp_to_hellcreek(ptp); |
103 | u16 secl, nsh, nsl; |
104 | |
105 | secl = ts->tv_sec & 0xffff; |
106 | nsh = ((u32)ts->tv_nsec & 0xffff0000) >> 16; |
107 | nsl = ts->tv_nsec & 0xffff; |
108 | |
109 | mutex_lock(&hellcreek->ptp_lock); |
110 | |
111 | /* Update overflow data structure */ |
112 | hellcreek->seconds = ts->tv_sec; |
113 | hellcreek->last_ts = ts->tv_nsec; |
114 | |
115 | /* Set time in clock */ |
116 | hellcreek_ptp_write(hellcreek, data: 0x00, PR_CLOCK_WRITE_C); |
117 | hellcreek_ptp_write(hellcreek, data: 0x00, PR_CLOCK_WRITE_C); |
118 | hellcreek_ptp_write(hellcreek, data: secl, PR_CLOCK_WRITE_C); |
119 | hellcreek_ptp_write(hellcreek, data: nsh, PR_CLOCK_WRITE_C); |
120 | hellcreek_ptp_write(hellcreek, data: nsl, PR_CLOCK_WRITE_C); |
121 | |
122 | mutex_unlock(lock: &hellcreek->ptp_lock); |
123 | |
124 | return 0; |
125 | } |
126 | |
127 | static int hellcreek_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) |
128 | { |
129 | struct hellcreek *hellcreek = ptp_to_hellcreek(ptp); |
130 | u16 negative = 0, addendh, addendl; |
131 | u32 addend; |
132 | u64 adj; |
133 | |
134 | if (scaled_ppm < 0) { |
135 | negative = 1; |
136 | scaled_ppm = -scaled_ppm; |
137 | } |
138 | |
139 | /* IP-Core adjusts the nominal frequency by adding or subtracting 1 ns |
140 | * from the 8 ns (period of the oscillator) every time the accumulator |
141 | * register overflows. The value stored in the addend register is added |
142 | * to the accumulator register every 8 ns. |
143 | * |
144 | * addend value = (2^30 * accumulator_overflow_rate) / |
145 | * oscillator_frequency |
146 | * where: |
147 | * |
148 | * oscillator_frequency = 125 MHz |
149 | * accumulator_overflow_rate = 125 MHz * scaled_ppm * 2^-16 * 10^-6 * 8 |
150 | */ |
151 | adj = scaled_ppm; |
152 | adj <<= 11; |
153 | addend = (u32)div_u64(dividend: adj, divisor: 15625); |
154 | |
155 | addendh = (addend & 0xffff0000) >> 16; |
156 | addendl = addend & 0xffff; |
157 | |
158 | negative = (negative << 15) & 0x8000; |
159 | |
160 | mutex_lock(&hellcreek->ptp_lock); |
161 | |
162 | /* Set drift register */ |
163 | hellcreek_ptp_write(hellcreek, data: negative, PR_CLOCK_DRIFT_C); |
164 | hellcreek_ptp_write(hellcreek, data: 0x00, PR_CLOCK_DRIFT_C); |
165 | hellcreek_ptp_write(hellcreek, data: 0x00, PR_CLOCK_DRIFT_C); |
166 | hellcreek_ptp_write(hellcreek, data: addendh, PR_CLOCK_DRIFT_C); |
167 | hellcreek_ptp_write(hellcreek, data: addendl, PR_CLOCK_DRIFT_C); |
168 | |
169 | mutex_unlock(lock: &hellcreek->ptp_lock); |
170 | |
171 | return 0; |
172 | } |
173 | |
174 | static int hellcreek_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) |
175 | { |
176 | struct hellcreek *hellcreek = ptp_to_hellcreek(ptp); |
177 | u16 negative = 0, counth, countl; |
178 | u32 count_val; |
179 | |
180 | /* If the offset is larger than IP-Core slow offset resources. Don't |
181 | * consider slow adjustment. Rather, add the offset directly to the |
182 | * current time |
183 | */ |
184 | if (abs(delta) > MAX_SLOW_OFFSET_ADJ) { |
185 | struct timespec64 now, then = ns_to_timespec64(nsec: delta); |
186 | |
187 | hellcreek_ptp_gettime(ptp, ts: &now); |
188 | now = timespec64_add(lhs: now, rhs: then); |
189 | hellcreek_ptp_settime(ptp, ts: &now); |
190 | |
191 | return 0; |
192 | } |
193 | |
194 | if (delta < 0) { |
195 | negative = 1; |
196 | delta = -delta; |
197 | } |
198 | |
199 | /* 'count_val' does not exceed the maximum register size (2^30) */ |
200 | count_val = div_s64(dividend: delta, MAX_NS_PER_STEP); |
201 | |
202 | counth = (count_val & 0xffff0000) >> 16; |
203 | countl = count_val & 0xffff; |
204 | |
205 | negative = (negative << 15) & 0x8000; |
206 | |
207 | mutex_lock(&hellcreek->ptp_lock); |
208 | |
209 | /* Set offset write register */ |
210 | hellcreek_ptp_write(hellcreek, data: negative, PR_CLOCK_OFFSET_C); |
211 | hellcreek_ptp_write(hellcreek, MAX_NS_PER_STEP, PR_CLOCK_OFFSET_C); |
212 | hellcreek_ptp_write(hellcreek, MIN_CLK_CYCLES_BETWEEN_STEPS, |
213 | PR_CLOCK_OFFSET_C); |
214 | hellcreek_ptp_write(hellcreek, data: countl, PR_CLOCK_OFFSET_C); |
215 | hellcreek_ptp_write(hellcreek, data: counth, PR_CLOCK_OFFSET_C); |
216 | |
217 | mutex_unlock(lock: &hellcreek->ptp_lock); |
218 | |
219 | return 0; |
220 | } |
221 | |
222 | static int hellcreek_ptp_enable(struct ptp_clock_info *ptp, |
223 | struct ptp_clock_request *rq, int on) |
224 | { |
225 | return -EOPNOTSUPP; |
226 | } |
227 | |
228 | static void hellcreek_ptp_overflow_check(struct work_struct *work) |
229 | { |
230 | struct delayed_work *dw = to_delayed_work(work); |
231 | struct hellcreek *hellcreek; |
232 | |
233 | hellcreek = dw_overflow_to_hellcreek(dw); |
234 | |
235 | mutex_lock(&hellcreek->ptp_lock); |
236 | __hellcreek_ptp_gettime(hellcreek); |
237 | mutex_unlock(lock: &hellcreek->ptp_lock); |
238 | |
239 | schedule_delayed_work(dwork: &hellcreek->overflow_work, |
240 | HELLCREEK_OVERFLOW_PERIOD); |
241 | } |
242 | |
243 | static enum led_brightness hellcreek_get_brightness(struct hellcreek *hellcreek, |
244 | int led) |
245 | { |
246 | return (hellcreek->status_out & led) ? 1 : 0; |
247 | } |
248 | |
249 | static void hellcreek_set_brightness(struct hellcreek *hellcreek, int led, |
250 | enum led_brightness b) |
251 | { |
252 | mutex_lock(&hellcreek->ptp_lock); |
253 | |
254 | if (b) |
255 | hellcreek->status_out |= led; |
256 | else |
257 | hellcreek->status_out &= ~led; |
258 | |
259 | hellcreek_ptp_write(hellcreek, data: hellcreek->status_out, STATUS_OUT); |
260 | |
261 | mutex_unlock(lock: &hellcreek->ptp_lock); |
262 | } |
263 | |
264 | static void hellcreek_led_sync_good_set(struct led_classdev *ldev, |
265 | enum led_brightness b) |
266 | { |
267 | struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good); |
268 | |
269 | hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, b); |
270 | } |
271 | |
272 | static enum led_brightness hellcreek_led_sync_good_get(struct led_classdev *ldev) |
273 | { |
274 | struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good); |
275 | |
276 | return hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD); |
277 | } |
278 | |
279 | static void hellcreek_led_is_gm_set(struct led_classdev *ldev, |
280 | enum led_brightness b) |
281 | { |
282 | struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm); |
283 | |
284 | hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, b); |
285 | } |
286 | |
287 | static enum led_brightness hellcreek_led_is_gm_get(struct led_classdev *ldev) |
288 | { |
289 | struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm); |
290 | |
291 | return hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM); |
292 | } |
293 | |
294 | /* There two available LEDs internally called sync_good and is_gm. However, the |
295 | * user might want to use a different label and specify the default state. Take |
296 | * those properties from device tree. |
297 | */ |
298 | static int hellcreek_led_setup(struct hellcreek *hellcreek) |
299 | { |
300 | struct device_node *leds, *led = NULL; |
301 | enum led_default_state state; |
302 | const char *label; |
303 | int ret = -EINVAL; |
304 | |
305 | of_node_get(node: hellcreek->dev->of_node); |
306 | leds = of_find_node_by_name(from: hellcreek->dev->of_node, name: "leds" ); |
307 | if (!leds) { |
308 | dev_err(hellcreek->dev, "No LEDs specified in device tree!\n" ); |
309 | return ret; |
310 | } |
311 | |
312 | hellcreek->status_out = 0; |
313 | |
314 | led = of_get_next_available_child(node: leds, prev: led); |
315 | if (!led) { |
316 | dev_err(hellcreek->dev, "First LED not specified!\n" ); |
317 | goto out; |
318 | } |
319 | |
320 | ret = of_property_read_string(np: led, propname: "label" , out_string: &label); |
321 | hellcreek->led_sync_good.name = ret ? "sync_good" : label; |
322 | |
323 | state = led_init_default_state_get(of_fwnode_handle(led)); |
324 | switch (state) { |
325 | case LEDS_DEFSTATE_ON: |
326 | hellcreek->led_sync_good.brightness = 1; |
327 | break; |
328 | case LEDS_DEFSTATE_KEEP: |
329 | hellcreek->led_sync_good.brightness = |
330 | hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD); |
331 | break; |
332 | default: |
333 | hellcreek->led_sync_good.brightness = 0; |
334 | } |
335 | |
336 | hellcreek->led_sync_good.max_brightness = 1; |
337 | hellcreek->led_sync_good.brightness_set = hellcreek_led_sync_good_set; |
338 | hellcreek->led_sync_good.brightness_get = hellcreek_led_sync_good_get; |
339 | |
340 | led = of_get_next_available_child(node: leds, prev: led); |
341 | if (!led) { |
342 | dev_err(hellcreek->dev, "Second LED not specified!\n" ); |
343 | ret = -EINVAL; |
344 | goto out; |
345 | } |
346 | |
347 | ret = of_property_read_string(np: led, propname: "label" , out_string: &label); |
348 | hellcreek->led_is_gm.name = ret ? "is_gm" : label; |
349 | |
350 | state = led_init_default_state_get(of_fwnode_handle(led)); |
351 | switch (state) { |
352 | case LEDS_DEFSTATE_ON: |
353 | hellcreek->led_is_gm.brightness = 1; |
354 | break; |
355 | case LEDS_DEFSTATE_KEEP: |
356 | hellcreek->led_is_gm.brightness = |
357 | hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM); |
358 | break; |
359 | default: |
360 | hellcreek->led_is_gm.brightness = 0; |
361 | } |
362 | |
363 | hellcreek->led_is_gm.max_brightness = 1; |
364 | hellcreek->led_is_gm.brightness_set = hellcreek_led_is_gm_set; |
365 | hellcreek->led_is_gm.brightness_get = hellcreek_led_is_gm_get; |
366 | |
367 | /* Set initial state */ |
368 | if (hellcreek->led_sync_good.brightness == 1) |
369 | hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, b: 1); |
370 | if (hellcreek->led_is_gm.brightness == 1) |
371 | hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, b: 1); |
372 | |
373 | /* Register both leds */ |
374 | led_classdev_register(parent: hellcreek->dev, led_cdev: &hellcreek->led_sync_good); |
375 | led_classdev_register(parent: hellcreek->dev, led_cdev: &hellcreek->led_is_gm); |
376 | |
377 | ret = 0; |
378 | |
379 | out: |
380 | of_node_put(node: leds); |
381 | |
382 | return ret; |
383 | } |
384 | |
385 | int hellcreek_ptp_setup(struct hellcreek *hellcreek) |
386 | { |
387 | u16 status; |
388 | int ret; |
389 | |
390 | /* Set up the overflow work */ |
391 | INIT_DELAYED_WORK(&hellcreek->overflow_work, |
392 | hellcreek_ptp_overflow_check); |
393 | |
394 | /* Setup PTP clock */ |
395 | hellcreek->ptp_clock_info.owner = THIS_MODULE; |
396 | snprintf(buf: hellcreek->ptp_clock_info.name, |
397 | size: sizeof(hellcreek->ptp_clock_info.name), |
398 | fmt: dev_name(dev: hellcreek->dev)); |
399 | |
400 | /* IP-Core can add up to 0.5 ns per 8 ns cycle, which means |
401 | * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts |
402 | * the nominal frequency by 6.25%) |
403 | */ |
404 | hellcreek->ptp_clock_info.max_adj = 62500000; |
405 | hellcreek->ptp_clock_info.n_alarm = 0; |
406 | hellcreek->ptp_clock_info.n_pins = 0; |
407 | hellcreek->ptp_clock_info.n_ext_ts = 0; |
408 | hellcreek->ptp_clock_info.n_per_out = 0; |
409 | hellcreek->ptp_clock_info.pps = 0; |
410 | hellcreek->ptp_clock_info.adjfine = hellcreek_ptp_adjfine; |
411 | hellcreek->ptp_clock_info.adjtime = hellcreek_ptp_adjtime; |
412 | hellcreek->ptp_clock_info.gettime64 = hellcreek_ptp_gettime; |
413 | hellcreek->ptp_clock_info.settime64 = hellcreek_ptp_settime; |
414 | hellcreek->ptp_clock_info.enable = hellcreek_ptp_enable; |
415 | hellcreek->ptp_clock_info.do_aux_work = hellcreek_hwtstamp_work; |
416 | |
417 | hellcreek->ptp_clock = ptp_clock_register(info: &hellcreek->ptp_clock_info, |
418 | parent: hellcreek->dev); |
419 | if (IS_ERR(ptr: hellcreek->ptp_clock)) |
420 | return PTR_ERR(ptr: hellcreek->ptp_clock); |
421 | |
422 | /* Enable the offset correction process, if no offset correction is |
423 | * already taking place |
424 | */ |
425 | status = hellcreek_ptp_read(hellcreek, PR_CLOCK_STATUS_C); |
426 | if (!(status & PR_CLOCK_STATUS_C_OFS_ACT)) |
427 | hellcreek_ptp_write(hellcreek, |
428 | data: status | PR_CLOCK_STATUS_C_ENA_OFS, |
429 | PR_CLOCK_STATUS_C); |
430 | |
431 | /* Enable the drift correction process */ |
432 | hellcreek_ptp_write(hellcreek, data: status | PR_CLOCK_STATUS_C_ENA_DRIFT, |
433 | PR_CLOCK_STATUS_C); |
434 | |
435 | /* LED setup */ |
436 | ret = hellcreek_led_setup(hellcreek); |
437 | if (ret) { |
438 | if (hellcreek->ptp_clock) |
439 | ptp_clock_unregister(ptp: hellcreek->ptp_clock); |
440 | return ret; |
441 | } |
442 | |
443 | schedule_delayed_work(dwork: &hellcreek->overflow_work, |
444 | HELLCREEK_OVERFLOW_PERIOD); |
445 | |
446 | return 0; |
447 | } |
448 | |
449 | void hellcreek_ptp_free(struct hellcreek *hellcreek) |
450 | { |
451 | led_classdev_unregister(led_cdev: &hellcreek->led_is_gm); |
452 | led_classdev_unregister(led_cdev: &hellcreek->led_sync_good); |
453 | cancel_delayed_work_sync(dwork: &hellcreek->overflow_work); |
454 | if (hellcreek->ptp_clock) |
455 | ptp_clock_unregister(ptp: hellcreek->ptp_clock); |
456 | hellcreek->ptp_clock = NULL; |
457 | } |
458 | |