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