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 
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 
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 
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 
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 
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 
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 
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
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
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.ipv4.proto.value[0] = IPPROTO_UDP;
339 	trap->key.ipv4.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
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
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.ipv4.proto.value[0] = IPPROTO_UDP;
359 	trap->key.ipv4.proto.mask[0] = 0xff;
360 	trap->key.ipv6.dport.value = PTP_GEN_PORT;
361 	trap->key.ipv6.dport.mask = 0xffff;
362 }
363 
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 
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 
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 
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 
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 
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 
439 static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
440 				  bool l2, bool l4)
441 {
442 	int err;
443 
444 	if (l2)
445 		err = ocelot_l2_ptp_trap_add(ocelot, port);
446 	else
447 		err = ocelot_l2_ptp_trap_del(ocelot, port);
448 	if (err)
449 		return err;
450 
451 	if (l4) {
452 		err = ocelot_ipv4_ptp_trap_add(ocelot, port);
453 		if (err)
454 			goto err_ipv4;
455 
456 		err = ocelot_ipv6_ptp_trap_add(ocelot, port);
457 		if (err)
458 			goto err_ipv6;
459 	} else {
460 		err = ocelot_ipv4_ptp_trap_del(ocelot, port);
461 
462 		err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
463 	}
464 	if (err)
465 		return err;
466 
467 	return 0;
468 
469 err_ipv6:
470 	ocelot_ipv4_ptp_trap_del(ocelot, port);
471 err_ipv4:
472 	if (l2)
473 		ocelot_l2_ptp_trap_del(ocelot, port);
474 	return err;
475 }
476 
477 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
478 {
479 	return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
480 			    sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
481 }
482 EXPORT_SYMBOL(ocelot_hwstamp_get);
483 
484 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
485 {
486 	struct ocelot_port *ocelot_port = ocelot->ports[port];
487 	bool l2 = false, l4 = false;
488 	struct hwtstamp_config cfg;
489 	int err;
490 
491 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
492 		return -EFAULT;
493 
494 	/* Tx type sanity check */
495 	switch (cfg.tx_type) {
496 	case HWTSTAMP_TX_ON:
497 		ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
498 		break;
499 	case HWTSTAMP_TX_ONESTEP_SYNC:
500 		/* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
501 		 * need to update the origin time.
502 		 */
503 		ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
504 		break;
505 	case HWTSTAMP_TX_OFF:
506 		ocelot_port->ptp_cmd = 0;
507 		break;
508 	default:
509 		return -ERANGE;
510 	}
511 
512 	mutex_lock(&ocelot->ptp_lock);
513 
514 	switch (cfg.rx_filter) {
515 	case HWTSTAMP_FILTER_NONE:
516 		break;
517 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
518 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
519 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
520 		l4 = true;
521 		break;
522 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
523 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
524 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
525 		l2 = true;
526 		break;
527 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
528 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
529 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
530 		l2 = true;
531 		l4 = true;
532 		break;
533 	default:
534 		mutex_unlock(&ocelot->ptp_lock);
535 		return -ERANGE;
536 	}
537 
538 	err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
539 	if (err) {
540 		mutex_unlock(&ocelot->ptp_lock);
541 		return err;
542 	}
543 
544 	if (l2 && l4)
545 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
546 	else if (l2)
547 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
548 	else if (l4)
549 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
550 	else
551 		cfg.rx_filter = HWTSTAMP_FILTER_NONE;
552 
553 	/* Commit back the result & save it */
554 	memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
555 	mutex_unlock(&ocelot->ptp_lock);
556 
557 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
558 }
559 EXPORT_SYMBOL(ocelot_hwstamp_set);
560 
561 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
562 		       struct ethtool_ts_info *info)
563 {
564 	info->phc_index = ocelot->ptp_clock ?
565 			  ptp_clock_index(ocelot->ptp_clock) : -1;
566 	if (info->phc_index == -1) {
567 		info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
568 					 SOF_TIMESTAMPING_RX_SOFTWARE |
569 					 SOF_TIMESTAMPING_SOFTWARE;
570 		return 0;
571 	}
572 	info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
573 				 SOF_TIMESTAMPING_RX_SOFTWARE |
574 				 SOF_TIMESTAMPING_SOFTWARE |
575 				 SOF_TIMESTAMPING_TX_HARDWARE |
576 				 SOF_TIMESTAMPING_RX_HARDWARE |
577 				 SOF_TIMESTAMPING_RAW_HARDWARE;
578 	info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
579 			 BIT(HWTSTAMP_TX_ONESTEP_SYNC);
580 	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
581 			   BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
582 			   BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
583 			   BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
584 
585 	return 0;
586 }
587 EXPORT_SYMBOL(ocelot_get_ts_info);
588 
589 static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
590 					struct sk_buff *clone)
591 {
592 	struct ocelot_port *ocelot_port = ocelot->ports[port];
593 	unsigned long flags;
594 
595 	spin_lock_irqsave(&ocelot->ts_id_lock, flags);
596 
597 	if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
598 	    ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
599 		spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
600 		return -EBUSY;
601 	}
602 
603 	skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
604 	/* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
605 	OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
606 
607 	ocelot_port->ts_id++;
608 	if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
609 		ocelot_port->ts_id = 0;
610 
611 	ocelot_port->ptp_skbs_in_flight++;
612 	ocelot->ptp_skbs_in_flight++;
613 
614 	skb_queue_tail(&ocelot_port->tx_skbs, clone);
615 
616 	spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
617 
618 	return 0;
619 }
620 
621 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
622 				       unsigned int ptp_class)
623 {
624 	struct ptp_header *hdr;
625 	u8 msgtype, twostep;
626 
627 	hdr = ptp_parse_header(skb, ptp_class);
628 	if (!hdr)
629 		return false;
630 
631 	msgtype = ptp_get_msgtype(hdr, ptp_class);
632 	twostep = hdr->flag_field[0] & 0x2;
633 
634 	if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
635 		return true;
636 
637 	return false;
638 }
639 
640 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
641 				 struct sk_buff *skb,
642 				 struct sk_buff **clone)
643 {
644 	struct ocelot_port *ocelot_port = ocelot->ports[port];
645 	u8 ptp_cmd = ocelot_port->ptp_cmd;
646 	unsigned int ptp_class;
647 	int err;
648 
649 	/* Don't do anything if PTP timestamping not enabled */
650 	if (!ptp_cmd)
651 		return 0;
652 
653 	ptp_class = ptp_classify_raw(skb);
654 	if (ptp_class == PTP_CLASS_NONE)
655 		return -EINVAL;
656 
657 	/* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
658 	if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
659 		if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
660 			OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
661 			return 0;
662 		}
663 
664 		/* Fall back to two-step timestamping */
665 		ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
666 	}
667 
668 	if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
669 		*clone = skb_clone_sk(skb);
670 		if (!(*clone))
671 			return -ENOMEM;
672 
673 		err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
674 		if (err)
675 			return err;
676 
677 		OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
678 		OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
679 	}
680 
681 	return 0;
682 }
683 EXPORT_SYMBOL(ocelot_port_txtstamp_request);
684 
685 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
686 				   struct timespec64 *ts)
687 {
688 	unsigned long flags;
689 	u32 val;
690 
691 	spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
692 
693 	/* Read current PTP time to get seconds */
694 	val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
695 
696 	val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
697 	val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
698 	ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
699 	ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
700 
701 	/* Read packet HW timestamp from FIFO */
702 	val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
703 	ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
704 
705 	/* Sec has incremented since the ts was registered */
706 	if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
707 		ts->tv_sec--;
708 
709 	spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
710 }
711 
712 static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
713 {
714 	struct ptp_header *hdr;
715 
716 	hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
717 	if (WARN_ON(!hdr))
718 		return false;
719 
720 	return seqid == ntohs(hdr->sequence_id);
721 }
722 
723 void ocelot_get_txtstamp(struct ocelot *ocelot)
724 {
725 	int budget = OCELOT_PTP_QUEUE_SZ;
726 
727 	while (budget--) {
728 		struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
729 		struct skb_shared_hwtstamps shhwtstamps;
730 		u32 val, id, seqid, txport;
731 		struct ocelot_port *port;
732 		struct timespec64 ts;
733 		unsigned long flags;
734 
735 		val = ocelot_read(ocelot, SYS_PTP_STATUS);
736 
737 		/* Check if a timestamp can be retrieved */
738 		if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
739 			break;
740 
741 		WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
742 
743 		/* Retrieve the ts ID and Tx port */
744 		id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
745 		txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
746 		seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
747 
748 		port = ocelot->ports[txport];
749 
750 		spin_lock(&ocelot->ts_id_lock);
751 		port->ptp_skbs_in_flight--;
752 		ocelot->ptp_skbs_in_flight--;
753 		spin_unlock(&ocelot->ts_id_lock);
754 
755 		/* Retrieve its associated skb */
756 try_again:
757 		spin_lock_irqsave(&port->tx_skbs.lock, flags);
758 
759 		skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
760 			if (OCELOT_SKB_CB(skb)->ts_id != id)
761 				continue;
762 			__skb_unlink(skb, &port->tx_skbs);
763 			skb_match = skb;
764 			break;
765 		}
766 
767 		spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
768 
769 		if (WARN_ON(!skb_match))
770 			continue;
771 
772 		if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
773 			dev_err_ratelimited(ocelot->dev,
774 					    "port %d received stale TX timestamp for seqid %d, discarding\n",
775 					    txport, seqid);
776 			dev_kfree_skb_any(skb);
777 			goto try_again;
778 		}
779 
780 		/* Get the h/w timestamp */
781 		ocelot_get_hwtimestamp(ocelot, &ts);
782 
783 		/* Set the timestamp into the skb */
784 		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
785 		shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
786 		skb_complete_tx_timestamp(skb_match, &shhwtstamps);
787 
788 		/* Next ts */
789 		ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
790 	}
791 }
792 EXPORT_SYMBOL(ocelot_get_txtstamp);
793 
794 int ocelot_init_timestamp(struct ocelot *ocelot,
795 			  const struct ptp_clock_info *info)
796 {
797 	struct ptp_clock *ptp_clock;
798 	int i;
799 
800 	ocelot->ptp_info = *info;
801 
802 	for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) {
803 		struct ptp_pin_desc *p = &ocelot->ptp_pins[i];
804 
805 		snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i);
806 		p->index = i;
807 		p->func = PTP_PF_NONE;
808 	}
809 
810 	ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0];
811 
812 	ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
813 	if (IS_ERR(ptp_clock))
814 		return PTR_ERR(ptp_clock);
815 	/* Check if PHC support is missing at the configuration level */
816 	if (!ptp_clock)
817 		return 0;
818 
819 	ocelot->ptp_clock = ptp_clock;
820 
821 	ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
822 	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
823 	ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
824 
825 	ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
826 
827 	/* There is no device reconfiguration, PTP Rx stamping is always
828 	 * enabled.
829 	 */
830 	ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
831 
832 	return 0;
833 }
834 EXPORT_SYMBOL(ocelot_init_timestamp);
835 
836 int ocelot_deinit_timestamp(struct ocelot *ocelot)
837 {
838 	if (ocelot->ptp_clock)
839 		ptp_clock_unregister(ocelot->ptp_clock);
840 	return 0;
841 }
842 EXPORT_SYMBOL(ocelot_deinit_timestamp);
843