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