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