1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
4 * Hirschmann Hellcreek TSN switch.
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>
12 #include <linux/ptp_clock_kernel.h>
13 #include "hellcreek.h"
14 #include "hellcreek_ptp.h"
15 #include "hellcreek_hwtstamp.h"
17 u16 hellcreek_ptp_read(struct hellcreek *hellcreek, unsigned int offset)
19 return readw(hellcreek->ptp_base + offset);
22 void hellcreek_ptp_write(struct hellcreek *hellcreek, u16 data,
25 writew(data, hellcreek->ptp_base + offset);
28 /* Get nanoseconds from PTP clock */
29 static u64 hellcreek_ptp_clock_read(struct hellcreek *hellcreek)
34 hellcreek_ptp_write(hellcreek, PR_COMMAND_C_SS, PR_COMMAND_C);
36 /* The time of the day is saved as 96 bits. However, due to hardware
37 * limitations the seconds are not or only partly kept in the PTP
38 * core. Currently only three bits for the seconds are available. That's
39 * why only the nanoseconds are used and the seconds are tracked in
40 * software. Anyway due to internal locking all five registers should be
43 nsh = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
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 nsl = hellcreek_ptp_read(hellcreek, PR_SS_SYNC_DATA_C);
49 return (u64)nsl | ((u64)nsh << 16);
52 static u64 __hellcreek_ptp_gettime(struct hellcreek *hellcreek)
56 ns = hellcreek_ptp_clock_read(hellcreek);
57 if (ns < hellcreek->last_ts)
59 hellcreek->last_ts = ns;
60 ns += hellcreek->seconds * NSEC_PER_SEC;
65 /* Retrieve the seconds parts in nanoseconds for a packet timestamped with @ns.
66 * There has to be a check whether an overflow occurred between the packet
67 * arrival and now. If so use the correct seconds (-1) for calculating the
68 * packet arrival time.
70 u64 hellcreek_ptp_gettime_seconds(struct hellcreek *hellcreek, u64 ns)
74 __hellcreek_ptp_gettime(hellcreek);
75 if (hellcreek->last_ts > ns)
76 s = hellcreek->seconds * NSEC_PER_SEC;
78 s = (hellcreek->seconds - 1) * NSEC_PER_SEC;
83 static int hellcreek_ptp_gettime(struct ptp_clock_info *ptp,
84 struct timespec64 *ts)
86 struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
89 mutex_lock(&hellcreek->ptp_lock);
90 ns = __hellcreek_ptp_gettime(hellcreek);
91 mutex_unlock(&hellcreek->ptp_lock);
93 *ts = ns_to_timespec64(ns);
98 static int hellcreek_ptp_settime(struct ptp_clock_info *ptp,
99 const struct timespec64 *ts)
101 struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
104 secl = ts->tv_sec & 0xffff;
105 nsh = ((u32)ts->tv_nsec & 0xffff0000) >> 16;
106 nsl = ts->tv_nsec & 0xffff;
108 mutex_lock(&hellcreek->ptp_lock);
110 /* Update overflow data structure */
111 hellcreek->seconds = ts->tv_sec;
112 hellcreek->last_ts = ts->tv_nsec;
114 /* Set time in clock */
115 hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_WRITE_C);
116 hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_WRITE_C);
117 hellcreek_ptp_write(hellcreek, secl, PR_CLOCK_WRITE_C);
118 hellcreek_ptp_write(hellcreek, nsh, PR_CLOCK_WRITE_C);
119 hellcreek_ptp_write(hellcreek, nsl, PR_CLOCK_WRITE_C);
121 mutex_unlock(&hellcreek->ptp_lock);
126 static int hellcreek_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
128 struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
129 u16 negative = 0, addendh, addendl;
133 if (scaled_ppm < 0) {
135 scaled_ppm = -scaled_ppm;
138 /* IP-Core adjusts the nominal frequency by adding or subtracting 1 ns
139 * from the 8 ns (period of the oscillator) every time the accumulator
140 * register overflows. The value stored in the addend register is added
141 * to the accumulator register every 8 ns.
143 * addend value = (2^30 * accumulator_overflow_rate) /
144 * oscillator_frequency
147 * oscillator_frequency = 125 MHz
148 * accumulator_overflow_rate = 125 MHz * scaled_ppm * 2^-16 * 10^-6 * 8
152 addend = (u32)div_u64(adj, 15625);
154 addendh = (addend & 0xffff0000) >> 16;
155 addendl = addend & 0xffff;
157 negative = (negative << 15) & 0x8000;
159 mutex_lock(&hellcreek->ptp_lock);
161 /* Set drift register */
162 hellcreek_ptp_write(hellcreek, negative, PR_CLOCK_DRIFT_C);
163 hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_DRIFT_C);
164 hellcreek_ptp_write(hellcreek, 0x00, PR_CLOCK_DRIFT_C);
165 hellcreek_ptp_write(hellcreek, addendh, PR_CLOCK_DRIFT_C);
166 hellcreek_ptp_write(hellcreek, addendl, PR_CLOCK_DRIFT_C);
168 mutex_unlock(&hellcreek->ptp_lock);
173 static int hellcreek_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
175 struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
176 u16 negative = 0, counth, countl;
179 /* If the offset is larger than IP-Core slow offset resources. Don't
180 * consider slow adjustment. Rather, add the offset directly to the
183 if (abs(delta) > MAX_SLOW_OFFSET_ADJ) {
184 struct timespec64 now, then = ns_to_timespec64(delta);
186 hellcreek_ptp_gettime(ptp, &now);
187 now = timespec64_add(now, then);
188 hellcreek_ptp_settime(ptp, &now);
198 /* 'count_val' does not exceed the maximum register size (2^30) */
199 count_val = div_s64(delta, MAX_NS_PER_STEP);
201 counth = (count_val & 0xffff0000) >> 16;
202 countl = count_val & 0xffff;
204 negative = (negative << 15) & 0x8000;
206 mutex_lock(&hellcreek->ptp_lock);
208 /* Set offset write register */
209 hellcreek_ptp_write(hellcreek, negative, PR_CLOCK_OFFSET_C);
210 hellcreek_ptp_write(hellcreek, MAX_NS_PER_STEP, PR_CLOCK_OFFSET_C);
211 hellcreek_ptp_write(hellcreek, MIN_CLK_CYCLES_BETWEEN_STEPS,
213 hellcreek_ptp_write(hellcreek, countl, PR_CLOCK_OFFSET_C);
214 hellcreek_ptp_write(hellcreek, counth, PR_CLOCK_OFFSET_C);
216 mutex_unlock(&hellcreek->ptp_lock);
221 static int hellcreek_ptp_enable(struct ptp_clock_info *ptp,
222 struct ptp_clock_request *rq, int on)
227 static void hellcreek_ptp_overflow_check(struct work_struct *work)
229 struct delayed_work *dw = to_delayed_work(work);
230 struct hellcreek *hellcreek;
232 hellcreek = dw_overflow_to_hellcreek(dw);
234 mutex_lock(&hellcreek->ptp_lock);
235 __hellcreek_ptp_gettime(hellcreek);
236 mutex_unlock(&hellcreek->ptp_lock);
238 schedule_delayed_work(&hellcreek->overflow_work,
239 HELLCREEK_OVERFLOW_PERIOD);
242 static enum led_brightness hellcreek_get_brightness(struct hellcreek *hellcreek,
245 return (hellcreek->status_out & led) ? 1 : 0;
248 static void hellcreek_set_brightness(struct hellcreek *hellcreek, int led,
249 enum led_brightness b)
251 mutex_lock(&hellcreek->ptp_lock);
254 hellcreek->status_out |= led;
256 hellcreek->status_out &= ~led;
258 hellcreek_ptp_write(hellcreek, hellcreek->status_out, STATUS_OUT);
260 mutex_unlock(&hellcreek->ptp_lock);
263 static void hellcreek_led_sync_good_set(struct led_classdev *ldev,
264 enum led_brightness b)
266 struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good);
268 hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, b);
271 static enum led_brightness hellcreek_led_sync_good_get(struct led_classdev *ldev)
273 struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_sync_good);
275 return hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD);
278 static void hellcreek_led_is_gm_set(struct led_classdev *ldev,
279 enum led_brightness b)
281 struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm);
283 hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, b);
286 static enum led_brightness hellcreek_led_is_gm_get(struct led_classdev *ldev)
288 struct hellcreek *hellcreek = led_to_hellcreek(ldev, led_is_gm);
290 return hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM);
293 /* There two available LEDs internally called sync_good and is_gm. However, the
294 * user might want to use a different label and specify the default state. Take
295 * those properties from device tree.
297 static int hellcreek_led_setup(struct hellcreek *hellcreek)
299 struct device_node *leds, *led = NULL;
300 const char *label, *state;
303 leds = of_find_node_by_name(hellcreek->dev->of_node, "leds");
305 dev_err(hellcreek->dev, "No LEDs specified in device tree!\n");
309 hellcreek->status_out = 0;
311 led = of_get_next_available_child(leds, led);
313 dev_err(hellcreek->dev, "First LED not specified!\n");
317 ret = of_property_read_string(led, "label", &label);
318 hellcreek->led_sync_good.name = ret ? "sync_good" : label;
320 ret = of_property_read_string(led, "default-state", &state);
322 if (!strcmp(state, "on"))
323 hellcreek->led_sync_good.brightness = 1;
324 else if (!strcmp(state, "off"))
325 hellcreek->led_sync_good.brightness = 0;
326 else if (!strcmp(state, "keep"))
327 hellcreek->led_sync_good.brightness =
328 hellcreek_get_brightness(hellcreek,
329 STATUS_OUT_SYNC_GOOD);
332 hellcreek->led_sync_good.max_brightness = 1;
333 hellcreek->led_sync_good.brightness_set = hellcreek_led_sync_good_set;
334 hellcreek->led_sync_good.brightness_get = hellcreek_led_sync_good_get;
336 led = of_get_next_available_child(leds, led);
338 dev_err(hellcreek->dev, "Second LED not specified!\n");
343 ret = of_property_read_string(led, "label", &label);
344 hellcreek->led_is_gm.name = ret ? "is_gm" : label;
346 ret = of_property_read_string(led, "default-state", &state);
348 if (!strcmp(state, "on"))
349 hellcreek->led_is_gm.brightness = 1;
350 else if (!strcmp(state, "off"))
351 hellcreek->led_is_gm.brightness = 0;
352 else if (!strcmp(state, "keep"))
353 hellcreek->led_is_gm.brightness =
354 hellcreek_get_brightness(hellcreek,
358 hellcreek->led_is_gm.max_brightness = 1;
359 hellcreek->led_is_gm.brightness_set = hellcreek_led_is_gm_set;
360 hellcreek->led_is_gm.brightness_get = hellcreek_led_is_gm_get;
362 /* Set initial state */
363 if (hellcreek->led_sync_good.brightness == 1)
364 hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, 1);
365 if (hellcreek->led_is_gm.brightness == 1)
366 hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, 1);
368 /* Register both leds */
369 led_classdev_register(hellcreek->dev, &hellcreek->led_sync_good);
370 led_classdev_register(hellcreek->dev, &hellcreek->led_is_gm);
380 int hellcreek_ptp_setup(struct hellcreek *hellcreek)
385 /* Set up the overflow work */
386 INIT_DELAYED_WORK(&hellcreek->overflow_work,
387 hellcreek_ptp_overflow_check);
389 /* Setup PTP clock */
390 hellcreek->ptp_clock_info.owner = THIS_MODULE;
391 snprintf(hellcreek->ptp_clock_info.name,
392 sizeof(hellcreek->ptp_clock_info.name),
393 dev_name(hellcreek->dev));
395 /* IP-Core can add up to 0.5 ns per 8 ns cycle, which means
396 * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts
397 * the nominal frequency by 6.25%)
399 hellcreek->ptp_clock_info.max_adj = 62500000;
400 hellcreek->ptp_clock_info.n_alarm = 0;
401 hellcreek->ptp_clock_info.n_pins = 0;
402 hellcreek->ptp_clock_info.n_ext_ts = 0;
403 hellcreek->ptp_clock_info.n_per_out = 0;
404 hellcreek->ptp_clock_info.pps = 0;
405 hellcreek->ptp_clock_info.adjfine = hellcreek_ptp_adjfine;
406 hellcreek->ptp_clock_info.adjtime = hellcreek_ptp_adjtime;
407 hellcreek->ptp_clock_info.gettime64 = hellcreek_ptp_gettime;
408 hellcreek->ptp_clock_info.settime64 = hellcreek_ptp_settime;
409 hellcreek->ptp_clock_info.enable = hellcreek_ptp_enable;
410 hellcreek->ptp_clock_info.do_aux_work = hellcreek_hwtstamp_work;
412 hellcreek->ptp_clock = ptp_clock_register(&hellcreek->ptp_clock_info,
414 if (IS_ERR(hellcreek->ptp_clock))
415 return PTR_ERR(hellcreek->ptp_clock);
417 /* Enable the offset correction process, if no offset correction is
418 * already taking place
420 status = hellcreek_ptp_read(hellcreek, PR_CLOCK_STATUS_C);
421 if (!(status & PR_CLOCK_STATUS_C_OFS_ACT))
422 hellcreek_ptp_write(hellcreek,
423 status | PR_CLOCK_STATUS_C_ENA_OFS,
426 /* Enable the drift correction process */
427 hellcreek_ptp_write(hellcreek, status | PR_CLOCK_STATUS_C_ENA_DRIFT,
431 ret = hellcreek_led_setup(hellcreek);
433 if (hellcreek->ptp_clock)
434 ptp_clock_unregister(hellcreek->ptp_clock);
438 schedule_delayed_work(&hellcreek->overflow_work,
439 HELLCREEK_OVERFLOW_PERIOD);
444 void hellcreek_ptp_free(struct hellcreek *hellcreek)
446 led_classdev_unregister(&hellcreek->led_is_gm);
447 led_classdev_unregister(&hellcreek->led_sync_good);
448 cancel_delayed_work_sync(&hellcreek->overflow_work);
449 if (hellcreek->ptp_clock)
450 ptp_clock_unregister(hellcreek->ptp_clock);
451 hellcreek->ptp_clock = NULL;