1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
2 /* Microsemi Ocelot PTP clock driver
3  *
4  * Copyright (c) 2017 Microsemi Corporation
5  * Copyright 2020 NXP
6  */
7 #include <linux/time64.h>
8 
9 #include <linux/dsa/ocelot.h>
10 #include <linux/ptp_classify.h>
11 #include <soc/mscc/ocelot_ptp.h>
12 #include <soc/mscc/ocelot_sys.h>
13 #include <soc/mscc/ocelot_vcap.h>
14 #include <soc/mscc/ocelot.h>
15 #include "ocelot.h"
16 
ocelot_ptp_gettime64(struct ptp_clock_info * ptp,struct timespec64 * ts)17 int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
18 {
19 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
20 	unsigned long flags;
21 	time64_t s;
22 	u32 val;
23 	s64 ns;
24 
25 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
26 
27 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
28 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
29 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
30 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
31 
32 	s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff;
33 	s <<= 32;
34 	s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
35 	ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
36 
37 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
38 
39 	/* Deal with negative values */
40 	if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
41 		s--;
42 		ns &= 0xf;
43 		ns += 999999984;
44 	}
45 
46 	set_normalized_timespec64(ts, s, ns);
47 	return 0;
48 }
49 EXPORT_SYMBOL(ocelot_ptp_gettime64);
50 
ocelot_ptp_settime64(struct ptp_clock_info * ptp,const struct timespec64 * ts)51 int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
52 			 const struct timespec64 *ts)
53 {
54 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
55 	unsigned long flags;
56 	u32 val;
57 
58 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
59 
60 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
61 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
62 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
63 
64 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
65 
66 	ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB,
67 			 TOD_ACC_PIN);
68 	ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB,
69 			 TOD_ACC_PIN);
70 	ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
71 
72 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
73 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
74 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
75 
76 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
77 
78 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
79 
80 	if (ocelot->ops->tas_clock_adjust)
81 		ocelot->ops->tas_clock_adjust(ocelot);
82 
83 	return 0;
84 }
85 EXPORT_SYMBOL(ocelot_ptp_settime64);
86 
ocelot_ptp_adjtime(struct ptp_clock_info * ptp,s64 delta)87 int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
88 {
89 	if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
90 		struct ocelot *ocelot = container_of(ptp, struct ocelot,
91 						     ptp_info);
92 		unsigned long flags;
93 		u32 val;
94 
95 		spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
96 
97 		val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
98 		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
99 			 PTP_PIN_CFG_DOM);
100 		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
101 
102 		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
103 
104 		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
105 		ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN);
106 		ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
107 
108 		val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
109 		val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
110 			 PTP_PIN_CFG_DOM);
111 		val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
112 
113 		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
114 
115 		spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
116 
117 		if (ocelot->ops->tas_clock_adjust)
118 			ocelot->ops->tas_clock_adjust(ocelot);
119 	} else {
120 		/* Fall back using ocelot_ptp_settime64 which is not exact. */
121 		struct timespec64 ts;
122 		u64 now;
123 
124 		ocelot_ptp_gettime64(ptp, &ts);
125 
126 		now = ktime_to_ns(timespec64_to_ktime(ts));
127 		ts = ns_to_timespec64(now + delta);
128 
129 		ocelot_ptp_settime64(ptp, &ts);
130 	}
131 
132 	return 0;
133 }
134 EXPORT_SYMBOL(ocelot_ptp_adjtime);
135 
ocelot_ptp_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)136 int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
137 {
138 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
139 	u32 unit = 0, direction = 0;
140 	unsigned long flags;
141 	u64 adj = 0;
142 
143 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
144 
145 	if (!scaled_ppm)
146 		goto disable_adj;
147 
148 	if (scaled_ppm < 0) {
149 		direction = PTP_CFG_CLK_ADJ_CFG_DIR;
150 		scaled_ppm = -scaled_ppm;
151 	}
152 
153 	adj = PSEC_PER_SEC << 16;
154 	do_div(adj, scaled_ppm);
155 	do_div(adj, 1000);
156 
157 	/* If the adjustment value is too large, use ns instead */
158 	if (adj >= (1L << 30)) {
159 		unit = PTP_CFG_CLK_ADJ_FREQ_NS;
160 		do_div(adj, 1000);
161 	}
162 
163 	/* Still too big */
164 	if (adj >= (1L << 30))
165 		goto disable_adj;
166 
167 	ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
168 	ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
169 		     PTP_CLK_CFG_ADJ_CFG);
170 
171 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
172 	return 0;
173 
174 disable_adj:
175 	ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
176 
177 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
178 	return 0;
179 }
180 EXPORT_SYMBOL(ocelot_ptp_adjfine);
181 
ocelot_ptp_verify(struct ptp_clock_info * ptp,unsigned int pin,enum ptp_pin_function func,unsigned int chan)182 int ocelot_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
183 		      enum ptp_pin_function func, unsigned int chan)
184 {
185 	switch (func) {
186 	case PTP_PF_NONE:
187 	case PTP_PF_PEROUT:
188 		break;
189 	case PTP_PF_EXTTS:
190 	case PTP_PF_PHYSYNC:
191 		return -1;
192 	}
193 	return 0;
194 }
195 EXPORT_SYMBOL(ocelot_ptp_verify);
196 
ocelot_ptp_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)197 int ocelot_ptp_enable(struct ptp_clock_info *ptp,
198 		      struct ptp_clock_request *rq, int on)
199 {
200 	struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
201 	struct timespec64 ts_phase, ts_period;
202 	enum ocelot_ptp_pins ptp_pin;
203 	unsigned long flags;
204 	bool pps = false;
205 	int pin = -1;
206 	s64 wf_high;
207 	s64 wf_low;
208 	u32 val;
209 
210 	switch (rq->type) {
211 	case PTP_CLK_REQ_PEROUT:
212 		/* Reject requests with unsupported flags */
213 		if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE |
214 					 PTP_PEROUT_PHASE))
215 			return -EOPNOTSUPP;
216 
217 		pin = ptp_find_pin(ocelot->ptp_clock, PTP_PF_PEROUT,
218 				   rq->perout.index);
219 		if (pin == 0)
220 			ptp_pin = PTP_PIN_0;
221 		else if (pin == 1)
222 			ptp_pin = PTP_PIN_1;
223 		else if (pin == 2)
224 			ptp_pin = PTP_PIN_2;
225 		else if (pin == 3)
226 			ptp_pin = PTP_PIN_3;
227 		else
228 			return -EBUSY;
229 
230 		ts_period.tv_sec = rq->perout.period.sec;
231 		ts_period.tv_nsec = rq->perout.period.nsec;
232 
233 		if (ts_period.tv_sec == 1 && ts_period.tv_nsec == 0)
234 			pps = true;
235 
236 		/* Handle turning off */
237 		if (!on) {
238 			spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
239 			val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
240 			ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin);
241 			spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
242 			break;
243 		}
244 
245 		if (rq->perout.flags & PTP_PEROUT_PHASE) {
246 			ts_phase.tv_sec = rq->perout.phase.sec;
247 			ts_phase.tv_nsec = rq->perout.phase.nsec;
248 		} else {
249 			/* Compatibility */
250 			ts_phase.tv_sec = rq->perout.start.sec;
251 			ts_phase.tv_nsec = rq->perout.start.nsec;
252 		}
253 		if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) {
254 			dev_warn(ocelot->dev,
255 				 "Absolute start time not supported!\n");
256 			dev_warn(ocelot->dev,
257 				 "Accept nsec for PPS phase adjustment, otherwise start time should be 0 0.\n");
258 			return -EINVAL;
259 		}
260 
261 		/* Calculate waveform high and low times */
262 		if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) {
263 			struct timespec64 ts_on;
264 
265 			ts_on.tv_sec = rq->perout.on.sec;
266 			ts_on.tv_nsec = rq->perout.on.nsec;
267 
268 			wf_high = timespec64_to_ns(&ts_on);
269 		} else {
270 			if (pps) {
271 				wf_high = 1000;
272 			} else {
273 				wf_high = timespec64_to_ns(&ts_period);
274 				wf_high = div_s64(wf_high, 2);
275 			}
276 		}
277 
278 		wf_low = timespec64_to_ns(&ts_period);
279 		wf_low -= wf_high;
280 
281 		/* Handle PPS request */
282 		if (pps) {
283 			spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
284 			ocelot_write_rix(ocelot, ts_phase.tv_nsec,
285 					 PTP_PIN_WF_LOW_PERIOD, ptp_pin);
286 			ocelot_write_rix(ocelot, wf_high,
287 					 PTP_PIN_WF_HIGH_PERIOD, ptp_pin);
288 			val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK);
289 			val |= PTP_PIN_CFG_SYNC;
290 			ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin);
291 			spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
292 			break;
293 		}
294 
295 		/* Handle periodic clock */
296 		if (wf_high > 0x3fffffff || wf_high <= 0x6)
297 			return -EINVAL;
298 		if (wf_low > 0x3fffffff || wf_low <= 0x6)
299 			return -EINVAL;
300 
301 		spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
302 		ocelot_write_rix(ocelot, wf_low, PTP_PIN_WF_LOW_PERIOD,
303 				 ptp_pin);
304 		ocelot_write_rix(ocelot, wf_high, PTP_PIN_WF_HIGH_PERIOD,
305 				 ptp_pin);
306 		val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK);
307 		ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin);
308 		spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
309 		break;
310 	default:
311 		return -EOPNOTSUPP;
312 	}
313 	return 0;
314 }
315 EXPORT_SYMBOL(ocelot_ptp_enable);
316 
ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter * trap)317 static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap)
318 {
319 	trap->key_type = OCELOT_VCAP_KEY_ETYPE;
320 	*(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588);
321 	*(__be16 *)trap->key.etype.etype.mask = htons(0xffff);
322 }
323 
324 static void
ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter * trap)325 ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
326 {
327 	trap->key_type = OCELOT_VCAP_KEY_IPV4;
328 	trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
329 	trap->key.ipv4.proto.mask[0] = 0xff;
330 	trap->key.ipv4.dport.value = PTP_EV_PORT;
331 	trap->key.ipv4.dport.mask = 0xffff;
332 }
333 
334 static void
ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter * trap)335 ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
336 {
337 	trap->key_type = OCELOT_VCAP_KEY_IPV6;
338 	trap->key.ipv6.proto.value[0] = IPPROTO_UDP;
339 	trap->key.ipv6.proto.mask[0] = 0xff;
340 	trap->key.ipv6.dport.value = PTP_EV_PORT;
341 	trap->key.ipv6.dport.mask = 0xffff;
342 }
343 
344 static void
ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter * trap)345 ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
346 {
347 	trap->key_type = OCELOT_VCAP_KEY_IPV4;
348 	trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
349 	trap->key.ipv4.proto.mask[0] = 0xff;
350 	trap->key.ipv4.dport.value = PTP_GEN_PORT;
351 	trap->key.ipv4.dport.mask = 0xffff;
352 }
353 
354 static void
ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter * trap)355 ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
356 {
357 	trap->key_type = OCELOT_VCAP_KEY_IPV6;
358 	trap->key.ipv6.proto.value[0] = IPPROTO_UDP;
359 	trap->key.ipv6.proto.mask[0] = 0xff;
360 	trap->key.ipv6.dport.value = PTP_GEN_PORT;
361 	trap->key.ipv6.dport.mask = 0xffff;
362 }
363 
ocelot_l2_ptp_trap_add(struct ocelot * ocelot,int port)364 static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port)
365 {
366 	unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
367 
368 	return ocelot_trap_add(ocelot, port, l2_cookie, true,
369 			       ocelot_populate_l2_ptp_trap_key);
370 }
371 
ocelot_l2_ptp_trap_del(struct ocelot * ocelot,int port)372 static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port)
373 {
374 	unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
375 
376 	return ocelot_trap_del(ocelot, port, l2_cookie);
377 }
378 
ocelot_ipv4_ptp_trap_add(struct ocelot * ocelot,int port)379 static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port)
380 {
381 	unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
382 	unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
383 	int err;
384 
385 	err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true,
386 			      ocelot_populate_ipv4_ptp_event_trap_key);
387 	if (err)
388 		return err;
389 
390 	err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false,
391 			      ocelot_populate_ipv4_ptp_general_trap_key);
392 	if (err)
393 		ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
394 
395 	return err;
396 }
397 
ocelot_ipv4_ptp_trap_del(struct ocelot * ocelot,int port)398 static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port)
399 {
400 	unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
401 	unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
402 	int err;
403 
404 	err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
405 	err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie);
406 	return err;
407 }
408 
ocelot_ipv6_ptp_trap_add(struct ocelot * ocelot,int port)409 static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port)
410 {
411 	unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
412 	unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
413 	int err;
414 
415 	err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true,
416 			      ocelot_populate_ipv6_ptp_event_trap_key);
417 	if (err)
418 		return err;
419 
420 	err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false,
421 			      ocelot_populate_ipv6_ptp_general_trap_key);
422 	if (err)
423 		ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
424 
425 	return err;
426 }
427 
ocelot_ipv6_ptp_trap_del(struct ocelot * ocelot,int port)428 static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port)
429 {
430 	unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
431 	unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
432 	int err;
433 
434 	err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
435 	err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie);
436 	return err;
437 }
438 
ocelot_setup_ptp_traps(struct ocelot * ocelot,int port,bool l2,bool l4)439 static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
440 				  bool l2, bool l4)
441 {
442 	struct ocelot_port *ocelot_port = ocelot->ports[port];
443 	int err;
444 
445 	ocelot_port->trap_proto &= ~(OCELOT_PROTO_PTP_L2 |
446 				     OCELOT_PROTO_PTP_L4);
447 
448 	if (l2)
449 		err = ocelot_l2_ptp_trap_add(ocelot, port);
450 	else
451 		err = ocelot_l2_ptp_trap_del(ocelot, port);
452 	if (err)
453 		return err;
454 
455 	if (l4) {
456 		err = ocelot_ipv4_ptp_trap_add(ocelot, port);
457 		if (err)
458 			goto err_ipv4;
459 
460 		err = ocelot_ipv6_ptp_trap_add(ocelot, port);
461 		if (err)
462 			goto err_ipv6;
463 	} else {
464 		err = ocelot_ipv4_ptp_trap_del(ocelot, port);
465 
466 		err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
467 	}
468 	if (err)
469 		return err;
470 
471 	if (l2)
472 		ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L2;
473 	if (l4)
474 		ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L4;
475 
476 	return 0;
477 
478 err_ipv6:
479 	ocelot_ipv4_ptp_trap_del(ocelot, port);
480 err_ipv4:
481 	if (l2)
482 		ocelot_l2_ptp_trap_del(ocelot, port);
483 	return err;
484 }
485 
ocelot_traps_to_ptp_rx_filter(unsigned int proto)486 static int ocelot_traps_to_ptp_rx_filter(unsigned int proto)
487 {
488 	if ((proto & OCELOT_PROTO_PTP_L2) && (proto & OCELOT_PROTO_PTP_L4))
489 		return HWTSTAMP_FILTER_PTP_V2_EVENT;
490 	else if (proto & OCELOT_PROTO_PTP_L2)
491 		return HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
492 	else if (proto & OCELOT_PROTO_PTP_L4)
493 		return HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
494 
495 	return HWTSTAMP_FILTER_NONE;
496 }
497 
ocelot_hwstamp_get(struct ocelot * ocelot,int port,struct ifreq * ifr)498 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
499 {
500 	struct ocelot_port *ocelot_port = ocelot->ports[port];
501 	struct hwtstamp_config cfg = {};
502 
503 	switch (ocelot_port->ptp_cmd) {
504 	case IFH_REW_OP_TWO_STEP_PTP:
505 		cfg.tx_type = HWTSTAMP_TX_ON;
506 		break;
507 	case IFH_REW_OP_ORIGIN_PTP:
508 		cfg.tx_type = HWTSTAMP_TX_ONESTEP_SYNC;
509 		break;
510 	default:
511 		cfg.tx_type = HWTSTAMP_TX_OFF;
512 		break;
513 	}
514 
515 	cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto);
516 
517 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
518 }
519 EXPORT_SYMBOL(ocelot_hwstamp_get);
520 
ocelot_hwstamp_set(struct ocelot * ocelot,int port,struct ifreq * ifr)521 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
522 {
523 	struct ocelot_port *ocelot_port = ocelot->ports[port];
524 	bool l2 = false, l4 = false;
525 	struct hwtstamp_config cfg;
526 	int err;
527 
528 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
529 		return -EFAULT;
530 
531 	/* Tx type sanity check */
532 	switch (cfg.tx_type) {
533 	case HWTSTAMP_TX_ON:
534 		ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
535 		break;
536 	case HWTSTAMP_TX_ONESTEP_SYNC:
537 		/* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
538 		 * need to update the origin time.
539 		 */
540 		ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
541 		break;
542 	case HWTSTAMP_TX_OFF:
543 		ocelot_port->ptp_cmd = 0;
544 		break;
545 	default:
546 		return -ERANGE;
547 	}
548 
549 	switch (cfg.rx_filter) {
550 	case HWTSTAMP_FILTER_NONE:
551 		break;
552 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
553 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
554 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
555 		l4 = true;
556 		break;
557 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
558 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
559 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
560 		l2 = true;
561 		break;
562 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
563 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
564 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
565 		l2 = true;
566 		l4 = true;
567 		break;
568 	default:
569 		return -ERANGE;
570 	}
571 
572 	err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
573 	if (err)
574 		return err;
575 
576 	cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto);
577 
578 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
579 }
580 EXPORT_SYMBOL(ocelot_hwstamp_set);
581 
ocelot_get_ts_info(struct ocelot * ocelot,int port,struct ethtool_ts_info * info)582 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
583 		       struct ethtool_ts_info *info)
584 {
585 	info->phc_index = ocelot->ptp_clock ?
586 			  ptp_clock_index(ocelot->ptp_clock) : -1;
587 	if (info->phc_index == -1) {
588 		info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
589 					 SOF_TIMESTAMPING_RX_SOFTWARE |
590 					 SOF_TIMESTAMPING_SOFTWARE;
591 		return 0;
592 	}
593 	info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
594 				 SOF_TIMESTAMPING_RX_SOFTWARE |
595 				 SOF_TIMESTAMPING_SOFTWARE |
596 				 SOF_TIMESTAMPING_TX_HARDWARE |
597 				 SOF_TIMESTAMPING_RX_HARDWARE |
598 				 SOF_TIMESTAMPING_RAW_HARDWARE;
599 	info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
600 			 BIT(HWTSTAMP_TX_ONESTEP_SYNC);
601 	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
602 			   BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
603 			   BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
604 			   BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
605 
606 	return 0;
607 }
608 EXPORT_SYMBOL(ocelot_get_ts_info);
609 
ocelot_port_add_txtstamp_skb(struct ocelot * ocelot,int port,struct sk_buff * clone)610 static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
611 					struct sk_buff *clone)
612 {
613 	struct ocelot_port *ocelot_port = ocelot->ports[port];
614 	unsigned long flags;
615 
616 	spin_lock_irqsave(&ocelot->ts_id_lock, flags);
617 
618 	if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
619 	    ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
620 		spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
621 		return -EBUSY;
622 	}
623 
624 	skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
625 	/* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
626 	OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
627 
628 	ocelot_port->ts_id++;
629 	if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
630 		ocelot_port->ts_id = 0;
631 
632 	ocelot_port->ptp_skbs_in_flight++;
633 	ocelot->ptp_skbs_in_flight++;
634 
635 	skb_queue_tail(&ocelot_port->tx_skbs, clone);
636 
637 	spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
638 
639 	return 0;
640 }
641 
ocelot_ptp_is_onestep_sync(struct sk_buff * skb,unsigned int ptp_class)642 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
643 				       unsigned int ptp_class)
644 {
645 	struct ptp_header *hdr;
646 	u8 msgtype, twostep;
647 
648 	hdr = ptp_parse_header(skb, ptp_class);
649 	if (!hdr)
650 		return false;
651 
652 	msgtype = ptp_get_msgtype(hdr, ptp_class);
653 	twostep = hdr->flag_field[0] & 0x2;
654 
655 	if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
656 		return true;
657 
658 	return false;
659 }
660 
ocelot_port_txtstamp_request(struct ocelot * ocelot,int port,struct sk_buff * skb,struct sk_buff ** clone)661 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
662 				 struct sk_buff *skb,
663 				 struct sk_buff **clone)
664 {
665 	struct ocelot_port *ocelot_port = ocelot->ports[port];
666 	u8 ptp_cmd = ocelot_port->ptp_cmd;
667 	unsigned int ptp_class;
668 	int err;
669 
670 	/* Don't do anything if PTP timestamping not enabled */
671 	if (!ptp_cmd)
672 		return 0;
673 
674 	ptp_class = ptp_classify_raw(skb);
675 	if (ptp_class == PTP_CLASS_NONE)
676 		return -EINVAL;
677 
678 	/* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
679 	if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
680 		if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
681 			OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
682 			return 0;
683 		}
684 
685 		/* Fall back to two-step timestamping */
686 		ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
687 	}
688 
689 	if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
690 		*clone = skb_clone_sk(skb);
691 		if (!(*clone))
692 			return -ENOMEM;
693 
694 		err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
695 		if (err)
696 			return err;
697 
698 		OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
699 		OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
700 	}
701 
702 	return 0;
703 }
704 EXPORT_SYMBOL(ocelot_port_txtstamp_request);
705 
ocelot_get_hwtimestamp(struct ocelot * ocelot,struct timespec64 * ts)706 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
707 				   struct timespec64 *ts)
708 {
709 	unsigned long flags;
710 	u32 val;
711 
712 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
713 
714 	/* Read current PTP time to get seconds */
715 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
716 
717 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
718 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
719 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
720 	ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
721 
722 	/* Read packet HW timestamp from FIFO */
723 	val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
724 	ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
725 
726 	/* Sec has incremented since the ts was registered */
727 	if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
728 		ts->tv_sec--;
729 
730 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
731 }
732 
ocelot_validate_ptp_skb(struct sk_buff * clone,u16 seqid)733 static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
734 {
735 	struct ptp_header *hdr;
736 
737 	hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
738 	if (WARN_ON(!hdr))
739 		return false;
740 
741 	return seqid == ntohs(hdr->sequence_id);
742 }
743 
ocelot_get_txtstamp(struct ocelot * ocelot)744 void ocelot_get_txtstamp(struct ocelot *ocelot)
745 {
746 	int budget = OCELOT_PTP_QUEUE_SZ;
747 
748 	while (budget--) {
749 		struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
750 		struct skb_shared_hwtstamps shhwtstamps;
751 		u32 val, id, seqid, txport;
752 		struct ocelot_port *port;
753 		struct timespec64 ts;
754 		unsigned long flags;
755 
756 		val = ocelot_read(ocelot, SYS_PTP_STATUS);
757 
758 		/* Check if a timestamp can be retrieved */
759 		if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
760 			break;
761 
762 		WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
763 
764 		/* Retrieve the ts ID and Tx port */
765 		id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
766 		txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
767 		seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
768 
769 		port = ocelot->ports[txport];
770 
771 		spin_lock(&ocelot->ts_id_lock);
772 		port->ptp_skbs_in_flight--;
773 		ocelot->ptp_skbs_in_flight--;
774 		spin_unlock(&ocelot->ts_id_lock);
775 
776 		/* Retrieve its associated skb */
777 try_again:
778 		spin_lock_irqsave(&port->tx_skbs.lock, flags);
779 
780 		skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
781 			if (OCELOT_SKB_CB(skb)->ts_id != id)
782 				continue;
783 			__skb_unlink(skb, &port->tx_skbs);
784 			skb_match = skb;
785 			break;
786 		}
787 
788 		spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
789 
790 		if (WARN_ON(!skb_match))
791 			continue;
792 
793 		if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
794 			dev_err_ratelimited(ocelot->dev,
795 					    "port %d received stale TX timestamp for seqid %d, discarding\n",
796 					    txport, seqid);
797 			dev_kfree_skb_any(skb);
798 			goto try_again;
799 		}
800 
801 		/* Get the h/w timestamp */
802 		ocelot_get_hwtimestamp(ocelot, &ts);
803 
804 		/* Set the timestamp into the skb */
805 		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
806 		shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
807 		skb_complete_tx_timestamp(skb_match, &shhwtstamps);
808 
809 		/* Next ts */
810 		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
811 	}
812 }
813 EXPORT_SYMBOL(ocelot_get_txtstamp);
814 
ocelot_init_timestamp(struct ocelot * ocelot,const struct ptp_clock_info * info)815 int ocelot_init_timestamp(struct ocelot *ocelot,
816 			  const struct ptp_clock_info *info)
817 {
818 	struct ptp_clock *ptp_clock;
819 	int i;
820 
821 	ocelot->ptp_info = *info;
822 
823 	for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) {
824 		struct ptp_pin_desc *p = &ocelot->ptp_pins[i];
825 
826 		snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i);
827 		p->index = i;
828 		p->func = PTP_PF_NONE;
829 	}
830 
831 	ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0];
832 
833 	ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
834 	if (IS_ERR(ptp_clock))
835 		return PTR_ERR(ptp_clock);
836 	/* Check if PHC support is missing at the configuration level */
837 	if (!ptp_clock)
838 		return 0;
839 
840 	ocelot->ptp_clock = ptp_clock;
841 
842 	ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
843 	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
844 	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
845 
846 	ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
847 
848 	return 0;
849 }
850 EXPORT_SYMBOL(ocelot_init_timestamp);
851 
ocelot_deinit_timestamp(struct ocelot * ocelot)852 int ocelot_deinit_timestamp(struct ocelot *ocelot)
853 {
854 	if (ocelot->ptp_clock)
855 		ptp_clock_unregister(ocelot->ptp_clock);
856 	return 0;
857 }
858 EXPORT_SYMBOL(ocelot_deinit_timestamp);
859