ocelot_ptp.c

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