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 
17 u16 hellcreek_ptp_read(struct hellcreek *hellcreek, unsigned int offset)
18 {
19 	return readw(hellcreek->ptp_base + offset);
20 }
21 
22 void 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 */
29 static 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 
52 static 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  */
70 u64 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 
83 static 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 
98 static 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 
126 static 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 
173 static 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 
221 static int hellcreek_ptp_enable(struct ptp_clock_info *ptp,
222 				struct ptp_clock_request *rq, int on)
223 {
224 	return -EOPNOTSUPP;
225 }
226 
227 static 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 
242 static enum led_brightness hellcreek_get_brightness(struct hellcreek *hellcreek,
243 						    int led)
244 {
245 	return (hellcreek->status_out & led) ? 1 : 0;
246 }
247 
248 static 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 
263 static 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 
271 static 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 
278 static 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 
286 static 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  */
297 static int hellcreek_led_setup(struct hellcreek *hellcreek)
298 {
299 	struct device_node *leds, *led = NULL;
300 	enum led_default_state state;
301 	const char *label;
302 	int ret = -EINVAL;
303 
304 	of_node_get(hellcreek->dev->of_node);
305 	leds = of_find_node_by_name(hellcreek->dev->of_node, "leds");
306 	if (!leds) {
307 		dev_err(hellcreek->dev, "No LEDs specified in device tree!\n");
308 		return ret;
309 	}
310 
311 	hellcreek->status_out = 0;
312 
313 	led = of_get_next_available_child(leds, led);
314 	if (!led) {
315 		dev_err(hellcreek->dev, "First LED not specified!\n");
316 		goto out;
317 	}
318 
319 	ret = of_property_read_string(led, "label", &label);
320 	hellcreek->led_sync_good.name = ret ? "sync_good" : label;
321 
322 	state = led_init_default_state_get(of_fwnode_handle(led));
323 	switch (state) {
324 	case LEDS_DEFSTATE_ON:
325 		hellcreek->led_sync_good.brightness = 1;
326 		break;
327 	case LEDS_DEFSTATE_KEEP:
328 		hellcreek->led_sync_good.brightness =
329 			hellcreek_get_brightness(hellcreek, STATUS_OUT_SYNC_GOOD);
330 		break;
331 	default:
332 		hellcreek->led_sync_good.brightness = 0;
333 	}
334 
335 	hellcreek->led_sync_good.max_brightness = 1;
336 	hellcreek->led_sync_good.brightness_set = hellcreek_led_sync_good_set;
337 	hellcreek->led_sync_good.brightness_get = hellcreek_led_sync_good_get;
338 
339 	led = of_get_next_available_child(leds, led);
340 	if (!led) {
341 		dev_err(hellcreek->dev, "Second LED not specified!\n");
342 		ret = -EINVAL;
343 		goto out;
344 	}
345 
346 	ret = of_property_read_string(led, "label", &label);
347 	hellcreek->led_is_gm.name = ret ? "is_gm" : label;
348 
349 	state = led_init_default_state_get(of_fwnode_handle(led));
350 	switch (state) {
351 	case LEDS_DEFSTATE_ON:
352 		hellcreek->led_is_gm.brightness = 1;
353 		break;
354 	case LEDS_DEFSTATE_KEEP:
355 		hellcreek->led_is_gm.brightness =
356 			hellcreek_get_brightness(hellcreek, STATUS_OUT_IS_GM);
357 		break;
358 	default:
359 		hellcreek->led_is_gm.brightness = 0;
360 	}
361 
362 	hellcreek->led_is_gm.max_brightness = 1;
363 	hellcreek->led_is_gm.brightness_set = hellcreek_led_is_gm_set;
364 	hellcreek->led_is_gm.brightness_get = hellcreek_led_is_gm_get;
365 
366 	/* Set initial state */
367 	if (hellcreek->led_sync_good.brightness == 1)
368 		hellcreek_set_brightness(hellcreek, STATUS_OUT_SYNC_GOOD, 1);
369 	if (hellcreek->led_is_gm.brightness == 1)
370 		hellcreek_set_brightness(hellcreek, STATUS_OUT_IS_GM, 1);
371 
372 	/* Register both leds */
373 	led_classdev_register(hellcreek->dev, &hellcreek->led_sync_good);
374 	led_classdev_register(hellcreek->dev, &hellcreek->led_is_gm);
375 
376 	ret = 0;
377 
378 out:
379 	of_node_put(leds);
380 
381 	return ret;
382 }
383 
384 int hellcreek_ptp_setup(struct hellcreek *hellcreek)
385 {
386 	u16 status;
387 	int ret;
388 
389 	/* Set up the overflow work */
390 	INIT_DELAYED_WORK(&hellcreek->overflow_work,
391 			  hellcreek_ptp_overflow_check);
392 
393 	/* Setup PTP clock */
394 	hellcreek->ptp_clock_info.owner = THIS_MODULE;
395 	snprintf(hellcreek->ptp_clock_info.name,
396 		 sizeof(hellcreek->ptp_clock_info.name),
397 		 dev_name(hellcreek->dev));
398 
399 	/* IP-Core can add up to 0.5 ns per 8 ns cycle, which means
400 	 * accumulator_overflow_rate shall not exceed 62.5 MHz (which adjusts
401 	 * the nominal frequency by 6.25%)
402 	 */
403 	hellcreek->ptp_clock_info.max_adj     = 62500000;
404 	hellcreek->ptp_clock_info.n_alarm     = 0;
405 	hellcreek->ptp_clock_info.n_pins      = 0;
406 	hellcreek->ptp_clock_info.n_ext_ts    = 0;
407 	hellcreek->ptp_clock_info.n_per_out   = 0;
408 	hellcreek->ptp_clock_info.pps	      = 0;
409 	hellcreek->ptp_clock_info.adjfine     = hellcreek_ptp_adjfine;
410 	hellcreek->ptp_clock_info.adjtime     = hellcreek_ptp_adjtime;
411 	hellcreek->ptp_clock_info.gettime64   = hellcreek_ptp_gettime;
412 	hellcreek->ptp_clock_info.settime64   = hellcreek_ptp_settime;
413 	hellcreek->ptp_clock_info.enable      = hellcreek_ptp_enable;
414 	hellcreek->ptp_clock_info.do_aux_work = hellcreek_hwtstamp_work;
415 
416 	hellcreek->ptp_clock = ptp_clock_register(&hellcreek->ptp_clock_info,
417 						  hellcreek->dev);
418 	if (IS_ERR(hellcreek->ptp_clock))
419 		return PTR_ERR(hellcreek->ptp_clock);
420 
421 	/* Enable the offset correction process, if no offset correction is
422 	 * already taking place
423 	 */
424 	status = hellcreek_ptp_read(hellcreek, PR_CLOCK_STATUS_C);
425 	if (!(status & PR_CLOCK_STATUS_C_OFS_ACT))
426 		hellcreek_ptp_write(hellcreek,
427 				    status | PR_CLOCK_STATUS_C_ENA_OFS,
428 				    PR_CLOCK_STATUS_C);
429 
430 	/* Enable the drift correction process */
431 	hellcreek_ptp_write(hellcreek, status | PR_CLOCK_STATUS_C_ENA_DRIFT,
432 			    PR_CLOCK_STATUS_C);
433 
434 	/* LED setup */
435 	ret = hellcreek_led_setup(hellcreek);
436 	if (ret) {
437 		if (hellcreek->ptp_clock)
438 			ptp_clock_unregister(hellcreek->ptp_clock);
439 		return ret;
440 	}
441 
442 	schedule_delayed_work(&hellcreek->overflow_work,
443 			      HELLCREEK_OVERFLOW_PERIOD);
444 
445 	return 0;
446 }
447 
448 void hellcreek_ptp_free(struct hellcreek *hellcreek)
449 {
450 	led_classdev_unregister(&hellcreek->led_is_gm);
451 	led_classdev_unregister(&hellcreek->led_sync_good);
452 	cancel_delayed_work_sync(&hellcreek->overflow_work);
453 	if (hellcreek->ptp_clock)
454 		ptp_clock_unregister(hellcreek->ptp_clock);
455 	hellcreek->ptp_clock = NULL;
456 }
457