hwtstamp.c

  1// SPDX-License-Identifier: GPL-2.0-or-later
  2/*
  3 * Marvell 88E6xxx Switch hardware timestamping support
  4 *
  5 * Copyright (c) 2008 Marvell Semiconductor
  6 *
  7 * Copyright (c) 2017 National Instruments
  8 *      Erik Hons <erik.hons@ni.com>
  9 *      Brandon Streiff <brandon.streiff@ni.com>
 10 *      Dane Wagner <dane.wagner@ni.com>
 11 */
 12
 13#include "chip.h"
 14#include "global2.h"
 15#include "hwtstamp.h"
 16#include "ptp.h"
 17#include <linux/ptp_classify.h>
 18
 19#define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
 20
 21static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port,
 22				   int addr, u16 *data, int len)
 23{
 24	if (!chip->info->ops->avb_ops->port_ptp_read)
 25		return -EOPNOTSUPP;
 26
 27	return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr,
 28						       data, len);
 29}
 30
 31static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port,
 32				    int addr, u16 data)
 33{
 34	if (!chip->info->ops->avb_ops->port_ptp_write)
 35		return -EOPNOTSUPP;
 36
 37	return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr,
 38							data);
 39}
 40
 41static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr,
 42			       u16 data)
 43{
 44	if (!chip->info->ops->avb_ops->ptp_write)
 45		return -EOPNOTSUPP;
 46
 47	return chip->info->ops->avb_ops->ptp_write(chip, addr, data);
 48}
 49
 50static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr,
 51			      u16 *data)
 52{
 53	if (!chip->info->ops->avb_ops->ptp_read)
 54		return -EOPNOTSUPP;
 55
 56	return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1);
 57}
 58
 59/* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX
 60 * timestamp. When working properly, hardware will produce a timestamp
 61 * within 1ms. Software may enounter delays due to MDIO contention, so
 62 * the timeout is set accordingly.
 63 */
 64#define TX_TSTAMP_TIMEOUT	msecs_to_jiffies(40)
 65
 66int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
 67			  struct kernel_ethtool_ts_info *info)
 68{
 69	const struct mv88e6xxx_ptp_ops *ptp_ops;
 70	struct mv88e6xxx_chip *chip;
 71
 72	chip = ds->priv;
 73	ptp_ops = chip->info->ops->ptp_ops;
 74
 75	if (!chip->info->ptp_support)
 76		return -EOPNOTSUPP;
 77
 78	info->so_timestamping =
 79		SOF_TIMESTAMPING_TX_HARDWARE |
 80		SOF_TIMESTAMPING_RX_HARDWARE |
 81		SOF_TIMESTAMPING_RAW_HARDWARE;
 82	info->phc_index = ptp_clock_index(chip->ptp_clock);
 83	info->tx_types =
 84		(1 << HWTSTAMP_TX_OFF) |
 85		(1 << HWTSTAMP_TX_ON);
 86	info->rx_filters = ptp_ops->rx_filters;
 87
 88	return 0;
 89}
 90
 91static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port,
 92					 struct kernel_hwtstamp_config *config)
 93{
 94	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
 95	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
 96	bool tstamp_enable = false;
 97
 98	/* Prevent the TX/RX paths from trying to interact with the
 99	 * timestamp hardware while we reconfigure it.
100	 */
101	clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
102
103	switch (config->tx_type) {
104	case HWTSTAMP_TX_OFF:
105		tstamp_enable = false;
106		break;
107	case HWTSTAMP_TX_ON:
108		tstamp_enable = true;
109		break;
110	default:
111		return -ERANGE;
112	}
113
114	/* The switch supports timestamping both L2 and L4; one cannot be
115	 * disabled independently of the other.
116	 */
117
118	if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) {
119		config->rx_filter = HWTSTAMP_FILTER_NONE;
120		dev_dbg(chip->dev, "Unsupported rx_filter %d\n",
121			config->rx_filter);
122		return -ERANGE;
123	}
124
125	switch (config->rx_filter) {
126	case HWTSTAMP_FILTER_NONE:
127		tstamp_enable = false;
128		break;
129	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
130	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
131	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
132	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
133	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
134	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
135	case HWTSTAMP_FILTER_PTP_V2_EVENT:
136	case HWTSTAMP_FILTER_PTP_V2_SYNC:
137	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
138		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
139		break;
140	case HWTSTAMP_FILTER_ALL:
141	default:
142		config->rx_filter = HWTSTAMP_FILTER_NONE;
143		return -ERANGE;
144	}
145
146	mv88e6xxx_reg_lock(chip);
147	if (tstamp_enable) {
148		chip->enable_count += 1;
149		if (chip->enable_count == 1 && ptp_ops->global_enable)
150			ptp_ops->global_enable(chip);
151		if (ptp_ops->port_enable)
152			ptp_ops->port_enable(chip, port);
153	} else {
154		if (ptp_ops->port_disable)
155			ptp_ops->port_disable(chip, port);
156		chip->enable_count -= 1;
157		if (chip->enable_count == 0 && ptp_ops->global_disable)
158			ptp_ops->global_disable(chip);
159	}
160	mv88e6xxx_reg_unlock(chip);
161
162	/* Once hardware has been configured, enable timestamp checks
163	 * in the RX/TX paths.
164	 */
165	if (tstamp_enable)
166		set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
167
168	return 0;
169}
170
171int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port,
172				struct kernel_hwtstamp_config *config,
173				struct netlink_ext_ack *extack)
174{
175	struct mv88e6xxx_chip *chip = ds->priv;
176	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
177	int err;
178
179	if (!chip->info->ptp_support)
180		return -EOPNOTSUPP;
181
182	err = mv88e6xxx_set_hwtstamp_config(chip, port, config);
183	if (err)
184		return err;
185
186	/* Save the chosen configuration to be returned later. */
187	ps->tstamp_config = *config;
188
189	return 0;
190}
191
192int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port,
193				struct kernel_hwtstamp_config *config)
194{
195	struct mv88e6xxx_chip *chip = ds->priv;
196	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
197
198	if (!chip->info->ptp_support)
199		return -EOPNOTSUPP;
200
201	*config = ps->tstamp_config;
202
203	return 0;
204}
205
206/* Returns a pointer to the PTP header if the caller should time stamp,
207 * or NULL if the caller should not.
208 */
209static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip,
210						  int port, struct sk_buff *skb,
211						  unsigned int type)
212{
213	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
214	struct ptp_header *hdr;
215
216	if (!chip->info->ptp_support)
217		return NULL;
218
219	hdr = ptp_parse_header(skb, type);
220	if (!hdr)
221		return NULL;
222
223	if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
224		return NULL;
225
226	return hdr;
227}
228
229static int mv88e6xxx_ts_valid(u16 status)
230{
231	if (!(status & MV88E6XXX_PTP_TS_VALID))
232		return 0;
233	if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
234		return 0;
235	return 1;
236}
237
238static int seq_match(struct sk_buff *skb, u16 ts_seqid)
239{
240	unsigned int type = SKB_PTP_TYPE(skb);
241	struct ptp_header *hdr;
242
243	hdr = ptp_parse_header(skb, type);
244
245	return ts_seqid == ntohs(hdr->sequence_id);
246}
247
248static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
249			       struct mv88e6xxx_port_hwtstamp *ps,
250			       struct sk_buff *skb, u16 reg,
251			       struct sk_buff_head *rxq)
252{
253	u16 buf[4] = { 0 }, status, seq_id;
254	struct skb_shared_hwtstamps *shwt;
255	struct sk_buff_head received;
256	u64 ns, timelo, timehi;
257	unsigned long flags;
258	int err;
259
260	/* The latched timestamp belongs to one of the received frames. */
261	__skb_queue_head_init(&received);
262	spin_lock_irqsave(&rxq->lock, flags);
263	skb_queue_splice_tail_init(rxq, &received);
264	spin_unlock_irqrestore(&rxq->lock, flags);
265
266	mv88e6xxx_reg_lock(chip);
267	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
268				      reg, buf, ARRAY_SIZE(buf));
269	mv88e6xxx_reg_unlock(chip);
270	if (err)
271		pr_err("failed to get the receive time stamp\n");
272
273	status = buf[0];
274	timelo = buf[1];
275	timehi = buf[2];
276	seq_id = buf[3];
277
278	if (status & MV88E6XXX_PTP_TS_VALID) {
279		mv88e6xxx_reg_lock(chip);
280		err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
281		mv88e6xxx_reg_unlock(chip);
282		if (err)
283			pr_err("failed to clear the receive status\n");
284	}
285	/* Since the device can only handle one time stamp at a time,
286	 * we purge any extra frames from the queue.
287	 */
288	for ( ; skb; skb = __skb_dequeue(&received)) {
289		if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
290			ns = timehi << 16 | timelo;
291
292			mv88e6xxx_reg_lock(chip);
293			ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
294			mv88e6xxx_reg_unlock(chip);
295			shwt = skb_hwtstamps(skb);
296			memset(shwt, 0, sizeof(*shwt));
297			shwt->hwtstamp = ns_to_ktime(ns);
298			status &= ~MV88E6XXX_PTP_TS_VALID;
299		}
300		netif_rx(skb);
301	}
302}
303
304static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
305				    struct mv88e6xxx_port_hwtstamp *ps)
306{
307	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
308	struct sk_buff *skb;
309
310	skb = skb_dequeue(&ps->rx_queue);
311
312	if (skb)
313		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg,
314				   &ps->rx_queue);
315
316	skb = skb_dequeue(&ps->rx_queue2);
317	if (skb)
318		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg,
319				   &ps->rx_queue2);
320}
321
322static int is_pdelay_resp(const struct ptp_header *hdr)
323{
324	return (hdr->tsmt & 0xf) == 3;
325}
326
327bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
328			     struct sk_buff *skb, unsigned int type)
329{
330	struct mv88e6xxx_port_hwtstamp *ps;
331	struct mv88e6xxx_chip *chip;
332	struct ptp_header *hdr;
333
334	chip = ds->priv;
335	ps = &chip->port_hwtstamp[port];
336
337	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
338		return false;
339
340	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
341	if (!hdr)
342		return false;
343
344	SKB_PTP_TYPE(skb) = type;
345
346	if (is_pdelay_resp(hdr))
347		skb_queue_tail(&ps->rx_queue2, skb);
348	else
349		skb_queue_tail(&ps->rx_queue, skb);
350
351	ptp_schedule_worker(chip->ptp_clock, 0);
352
353	return true;
354}
355
356static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
357				   struct mv88e6xxx_port_hwtstamp *ps)
358{
359	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
360	struct skb_shared_hwtstamps shhwtstamps;
361	u16 departure_block[4], status;
362	struct sk_buff *tmp_skb;
363	u32 time_raw;
364	int err;
365	u64 ns;
366
367	if (!ps->tx_skb)
368		return 0;
369
370	mv88e6xxx_reg_lock(chip);
371	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
372				      ptp_ops->dep_sts_reg,
373				      departure_block,
374				      ARRAY_SIZE(departure_block));
375	mv88e6xxx_reg_unlock(chip);
376
377	if (err)
378		goto free_and_clear_skb;
379
380	if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
381		if (time_is_before_jiffies(ps->tx_tstamp_start +
382					   TX_TSTAMP_TIMEOUT)) {
383			dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
384				 ps->port_id);
385			goto free_and_clear_skb;
386		}
387		/* The timestamp should be available quickly, while getting it
388		 * is high priority and time bounded to only 10ms. A poll is
389		 * warranted so restart the work.
390		 */
391		return 1;
392	}
393
394	/* We have the timestamp; go ahead and clear valid now */
395	mv88e6xxx_reg_lock(chip);
396	mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
397	mv88e6xxx_reg_unlock(chip);
398
399	status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
400	if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
401		dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
402		goto free_and_clear_skb;
403	}
404
405	if (departure_block[3] != ps->tx_seq_id) {
406		dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
407		goto free_and_clear_skb;
408	}
409
410	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
411	time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
412	mv88e6xxx_reg_lock(chip);
413	ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
414	mv88e6xxx_reg_unlock(chip);
415	shhwtstamps.hwtstamp = ns_to_ktime(ns);
416
417	dev_dbg(chip->dev,
418		"p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
419		ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
420		departure_block[0], ps->tx_seq_id, departure_block[3]);
421
422	/* skb_complete_tx_timestamp() will free up the client to make
423	 * another timestamp-able transmit. We have to be ready for it
424	 * -- by clearing the ps->tx_skb "flag" -- beforehand.
425	 */
426
427	tmp_skb = ps->tx_skb;
428	ps->tx_skb = NULL;
429	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
430	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
431
432	return 0;
433
434free_and_clear_skb:
435	dev_kfree_skb_any(ps->tx_skb);
436	ps->tx_skb = NULL;
437	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
438
439	return 0;
440}
441
442long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
443{
444	struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
445	struct dsa_switch *ds = chip->ds;
446	struct mv88e6xxx_port_hwtstamp *ps;
447	int i, restart = 0;
448
449	for (i = 0; i < ds->num_ports; i++) {
450		if (!dsa_is_user_port(ds, i))
451			continue;
452
453		ps = &chip->port_hwtstamp[i];
454		if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
455			restart |= mv88e6xxx_txtstamp_work(chip, ps);
456
457		mv88e6xxx_rxtstamp_work(chip, ps);
458	}
459
460	return restart ? 1 : -1;
461}
462
463void mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
464			     struct sk_buff *skb)
465{
466	struct mv88e6xxx_chip *chip = ds->priv;
467	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
468	struct ptp_header *hdr;
469	struct sk_buff *clone;
470	unsigned int type;
471
472	type = ptp_classify_raw(skb);
473	if (type == PTP_CLASS_NONE)
474		return;
475
476	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
477	if (!hdr)
478		return;
479
480	clone = skb_clone_sk(skb);
481	if (!clone)
482		return;
483
484	if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
485				  &ps->state)) {
486		kfree_skb(clone);
487		return;
488	}
489
490	ps->tx_skb = clone;
491	ps->tx_tstamp_start = jiffies;
492	ps->tx_seq_id = be16_to_cpu(hdr->sequence_id);
493
494	ptp_schedule_worker(chip->ptp_clock, 0);
495}
496
497int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
498{
499	u16 val;
500	int err;
501
502	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
503	if (err)
504		return err;
505	val |= MV88E6165_PTP_CFG_DISABLE_PTP;
506
507	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
508}
509
510int mv88e6165_global_enable(struct mv88e6xxx_chip *chip)
511{
512	u16 val;
513	int err;
514
515	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
516	if (err)
517		return err;
518
519	val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK);
520
521	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
522}
523
524int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port)
525{
526	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
527					MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
528}
529
530int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port)
531{
532	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
533					MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH);
534}
535
536static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
537{
538	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
539	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
540
541	ps->port_id = port;
542
543	skb_queue_head_init(&ps->rx_queue);
544	skb_queue_head_init(&ps->rx_queue2);
545
546	if (ptp_ops->port_disable)
547		return ptp_ops->port_disable(chip, port);
548
549	return 0;
550}
551
552int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
553{
554	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
555	int err;
556	int i;
557
558	/* Disable timestamping on all ports. */
559	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
560		err = mv88e6xxx_hwtstamp_port_setup(chip, i);
561		if (err)
562			return err;
563	}
564
565	/* Disable PTP globally */
566	if (ptp_ops->global_disable) {
567		err = ptp_ops->global_disable(chip);
568		if (err)
569			return err;
570	}
571
572	/* Set the ethertype of L2 PTP messages */
573	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_ETHERTYPE, ETH_P_1588);
574	if (err)
575		return err;
576
577	/* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
578	 * timestamp. This affects all ports that have timestamping enabled,
579	 * but the timestamp config is per-port; thus we configure all events
580	 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
581	 */
582	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
583				  MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
584	if (err)
585		return err;
586
587	/* Use ARRIVAL1 for peer delay response messages. */
588	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
589				  MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
590	if (err)
591		return err;
592
593	/* 88E6341 devices default to timestamping at the PHY, but this has
594	 * a hardware issue that results in unreliable timestamps. Force
595	 * these devices to timestamp at the MAC.
596	 */
597	if (chip->info->family == MV88E6XXX_FAMILY_6341) {
598		u16 val = MV88E6341_PTP_CFG_UPDATE |
599			  MV88E6341_PTP_CFG_MODE_IDX |
600			  MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
601		err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
602		if (err)
603			return err;
604	}
605
606	return 0;
607}
608
609void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
610{
611}