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 
mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip * chip,int port,int addr,u16 * data,int len)21 static 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 
mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip * chip,int port,int addr,u16 data)31 static 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 
mv88e6xxx_ptp_write(struct mv88e6xxx_chip * chip,int addr,u16 data)41 static 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 
mv88e6xxx_ptp_read(struct mv88e6xxx_chip * chip,int addr,u16 * data)50 static 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 
mv88e6xxx_get_ts_info(struct dsa_switch * ds,int port,struct ethtool_ts_info * info)66 int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
67 			  struct 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 
mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip * chip,int port,struct hwtstamp_config * config)91 static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port,
92 					 struct 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 
mv88e6xxx_port_hwtstamp_set(struct dsa_switch * ds,int port,struct ifreq * ifr)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 
mv88e6xxx_port_hwtstamp_get(struct dsa_switch * ds,int port,struct ifreq * ifr)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 /* Returns a pointer to the PTP header if the caller should time stamp,
211  * or NULL if the caller should not.
212  */
mv88e6xxx_should_tstamp(struct mv88e6xxx_chip * chip,int port,struct sk_buff * skb,unsigned int type)213 static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip,
214 						  int port, struct sk_buff *skb,
215 						  unsigned int type)
216 {
217 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
218 	struct ptp_header *hdr;
219 
220 	if (!chip->info->ptp_support)
221 		return NULL;
222 
223 	hdr = ptp_parse_header(skb, type);
224 	if (!hdr)
225 		return NULL;
226 
227 	if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
228 		return NULL;
229 
230 	return hdr;
231 }
232 
mv88e6xxx_ts_valid(u16 status)233 static int mv88e6xxx_ts_valid(u16 status)
234 {
235 	if (!(status & MV88E6XXX_PTP_TS_VALID))
236 		return 0;
237 	if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
238 		return 0;
239 	return 1;
240 }
241 
seq_match(struct sk_buff * skb,u16 ts_seqid)242 static int seq_match(struct sk_buff *skb, u16 ts_seqid)
243 {
244 	unsigned int type = SKB_PTP_TYPE(skb);
245 	struct ptp_header *hdr;
246 
247 	hdr = ptp_parse_header(skb, type);
248 
249 	return ts_seqid == ntohs(hdr->sequence_id);
250 }
251 
mv88e6xxx_get_rxts(struct mv88e6xxx_chip * chip,struct mv88e6xxx_port_hwtstamp * ps,struct sk_buff * skb,u16 reg,struct sk_buff_head * rxq)252 static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
253 			       struct mv88e6xxx_port_hwtstamp *ps,
254 			       struct sk_buff *skb, u16 reg,
255 			       struct sk_buff_head *rxq)
256 {
257 	u16 buf[4] = { 0 }, status, seq_id;
258 	struct skb_shared_hwtstamps *shwt;
259 	struct sk_buff_head received;
260 	u64 ns, timelo, timehi;
261 	unsigned long flags;
262 	int err;
263 
264 	/* The latched timestamp belongs to one of the received frames. */
265 	__skb_queue_head_init(&received);
266 	spin_lock_irqsave(&rxq->lock, flags);
267 	skb_queue_splice_tail_init(rxq, &received);
268 	spin_unlock_irqrestore(&rxq->lock, flags);
269 
270 	mv88e6xxx_reg_lock(chip);
271 	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
272 				      reg, buf, ARRAY_SIZE(buf));
273 	mv88e6xxx_reg_unlock(chip);
274 	if (err)
275 		pr_err("failed to get the receive time stamp\n");
276 
277 	status = buf[0];
278 	timelo = buf[1];
279 	timehi = buf[2];
280 	seq_id = buf[3];
281 
282 	if (status & MV88E6XXX_PTP_TS_VALID) {
283 		mv88e6xxx_reg_lock(chip);
284 		err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
285 		mv88e6xxx_reg_unlock(chip);
286 		if (err)
287 			pr_err("failed to clear the receive status\n");
288 	}
289 	/* Since the device can only handle one time stamp at a time,
290 	 * we purge any extra frames from the queue.
291 	 */
292 	for ( ; skb; skb = __skb_dequeue(&received)) {
293 		if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
294 			ns = timehi << 16 | timelo;
295 
296 			mv88e6xxx_reg_lock(chip);
297 			ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
298 			mv88e6xxx_reg_unlock(chip);
299 			shwt = skb_hwtstamps(skb);
300 			memset(shwt, 0, sizeof(*shwt));
301 			shwt->hwtstamp = ns_to_ktime(ns);
302 			status &= ~MV88E6XXX_PTP_TS_VALID;
303 		}
304 		netif_rx(skb);
305 	}
306 }
307 
mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip * chip,struct mv88e6xxx_port_hwtstamp * ps)308 static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
309 				    struct mv88e6xxx_port_hwtstamp *ps)
310 {
311 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
312 	struct sk_buff *skb;
313 
314 	skb = skb_dequeue(&ps->rx_queue);
315 
316 	if (skb)
317 		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg,
318 				   &ps->rx_queue);
319 
320 	skb = skb_dequeue(&ps->rx_queue2);
321 	if (skb)
322 		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg,
323 				   &ps->rx_queue2);
324 }
325 
is_pdelay_resp(const struct ptp_header * hdr)326 static int is_pdelay_resp(const struct ptp_header *hdr)
327 {
328 	return (hdr->tsmt & 0xf) == 3;
329 }
330 
mv88e6xxx_port_rxtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb,unsigned int type)331 bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
332 			     struct sk_buff *skb, unsigned int type)
333 {
334 	struct mv88e6xxx_port_hwtstamp *ps;
335 	struct mv88e6xxx_chip *chip;
336 	struct ptp_header *hdr;
337 
338 	chip = ds->priv;
339 	ps = &chip->port_hwtstamp[port];
340 
341 	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
342 		return false;
343 
344 	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
345 	if (!hdr)
346 		return false;
347 
348 	SKB_PTP_TYPE(skb) = type;
349 
350 	if (is_pdelay_resp(hdr))
351 		skb_queue_tail(&ps->rx_queue2, skb);
352 	else
353 		skb_queue_tail(&ps->rx_queue, skb);
354 
355 	ptp_schedule_worker(chip->ptp_clock, 0);
356 
357 	return true;
358 }
359 
mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip * chip,struct mv88e6xxx_port_hwtstamp * ps)360 static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
361 				   struct mv88e6xxx_port_hwtstamp *ps)
362 {
363 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
364 	struct skb_shared_hwtstamps shhwtstamps;
365 	u16 departure_block[4], status;
366 	struct sk_buff *tmp_skb;
367 	u32 time_raw;
368 	int err;
369 	u64 ns;
370 
371 	if (!ps->tx_skb)
372 		return 0;
373 
374 	mv88e6xxx_reg_lock(chip);
375 	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
376 				      ptp_ops->dep_sts_reg,
377 				      departure_block,
378 				      ARRAY_SIZE(departure_block));
379 	mv88e6xxx_reg_unlock(chip);
380 
381 	if (err)
382 		goto free_and_clear_skb;
383 
384 	if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
385 		if (time_is_before_jiffies(ps->tx_tstamp_start +
386 					   TX_TSTAMP_TIMEOUT)) {
387 			dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
388 				 ps->port_id);
389 			goto free_and_clear_skb;
390 		}
391 		/* The timestamp should be available quickly, while getting it
392 		 * is high priority and time bounded to only 10ms. A poll is
393 		 * warranted so restart the work.
394 		 */
395 		return 1;
396 	}
397 
398 	/* We have the timestamp; go ahead and clear valid now */
399 	mv88e6xxx_reg_lock(chip);
400 	mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
401 	mv88e6xxx_reg_unlock(chip);
402 
403 	status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
404 	if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
405 		dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
406 		goto free_and_clear_skb;
407 	}
408 
409 	if (departure_block[3] != ps->tx_seq_id) {
410 		dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
411 		goto free_and_clear_skb;
412 	}
413 
414 	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
415 	time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
416 	mv88e6xxx_reg_lock(chip);
417 	ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
418 	mv88e6xxx_reg_unlock(chip);
419 	shhwtstamps.hwtstamp = ns_to_ktime(ns);
420 
421 	dev_dbg(chip->dev,
422 		"p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
423 		ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
424 		departure_block[0], ps->tx_seq_id, departure_block[3]);
425 
426 	/* skb_complete_tx_timestamp() will free up the client to make
427 	 * another timestamp-able transmit. We have to be ready for it
428 	 * -- by clearing the ps->tx_skb "flag" -- beforehand.
429 	 */
430 
431 	tmp_skb = ps->tx_skb;
432 	ps->tx_skb = NULL;
433 	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
434 	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
435 
436 	return 0;
437 
438 free_and_clear_skb:
439 	dev_kfree_skb_any(ps->tx_skb);
440 	ps->tx_skb = NULL;
441 	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
442 
443 	return 0;
444 }
445 
mv88e6xxx_hwtstamp_work(struct ptp_clock_info * ptp)446 long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
447 {
448 	struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
449 	struct dsa_switch *ds = chip->ds;
450 	struct mv88e6xxx_port_hwtstamp *ps;
451 	int i, restart = 0;
452 
453 	for (i = 0; i < ds->num_ports; i++) {
454 		if (!dsa_is_user_port(ds, i))
455 			continue;
456 
457 		ps = &chip->port_hwtstamp[i];
458 		if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
459 			restart |= mv88e6xxx_txtstamp_work(chip, ps);
460 
461 		mv88e6xxx_rxtstamp_work(chip, ps);
462 	}
463 
464 	return restart ? 1 : -1;
465 }
466 
mv88e6xxx_port_txtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)467 void mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
468 			     struct sk_buff *skb)
469 {
470 	struct mv88e6xxx_chip *chip = ds->priv;
471 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
472 	struct ptp_header *hdr;
473 	struct sk_buff *clone;
474 	unsigned int type;
475 
476 	type = ptp_classify_raw(skb);
477 	if (type == PTP_CLASS_NONE)
478 		return;
479 
480 	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
481 	if (!hdr)
482 		return;
483 
484 	clone = skb_clone_sk(skb);
485 	if (!clone)
486 		return;
487 
488 	if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
489 				  &ps->state)) {
490 		kfree_skb(clone);
491 		return;
492 	}
493 
494 	ps->tx_skb = clone;
495 	ps->tx_tstamp_start = jiffies;
496 	ps->tx_seq_id = be16_to_cpu(hdr->sequence_id);
497 
498 	ptp_schedule_worker(chip->ptp_clock, 0);
499 }
500 
mv88e6165_global_disable(struct mv88e6xxx_chip * chip)501 int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
502 {
503 	u16 val;
504 	int err;
505 
506 	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
507 	if (err)
508 		return err;
509 	val |= MV88E6165_PTP_CFG_DISABLE_PTP;
510 
511 	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
512 }
513 
mv88e6165_global_enable(struct mv88e6xxx_chip * chip)514 int mv88e6165_global_enable(struct mv88e6xxx_chip *chip)
515 {
516 	u16 val;
517 	int err;
518 
519 	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
520 	if (err)
521 		return err;
522 
523 	val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK);
524 
525 	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
526 }
527 
mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip * chip,int port)528 int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port)
529 {
530 	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
531 					MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
532 }
533 
mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip * chip,int port)534 int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port)
535 {
536 	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
537 					MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH);
538 }
539 
mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip * chip,int port)540 static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
541 {
542 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
543 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
544 
545 	ps->port_id = port;
546 
547 	skb_queue_head_init(&ps->rx_queue);
548 	skb_queue_head_init(&ps->rx_queue2);
549 
550 	if (ptp_ops->port_disable)
551 		return ptp_ops->port_disable(chip, port);
552 
553 	return 0;
554 }
555 
mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip * chip)556 int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
557 {
558 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
559 	int err;
560 	int i;
561 
562 	/* Disable timestamping on all ports. */
563 	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
564 		err = mv88e6xxx_hwtstamp_port_setup(chip, i);
565 		if (err)
566 			return err;
567 	}
568 
569 	/* Disable PTP globally */
570 	if (ptp_ops->global_disable) {
571 		err = ptp_ops->global_disable(chip);
572 		if (err)
573 			return err;
574 	}
575 
576 	/* Set the ethertype of L2 PTP messages */
577 	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588);
578 	if (err)
579 		return err;
580 
581 	/* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
582 	 * timestamp. This affects all ports that have timestamping enabled,
583 	 * but the timestamp config is per-port; thus we configure all events
584 	 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
585 	 */
586 	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
587 				  MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
588 	if (err)
589 		return err;
590 
591 	/* Use ARRIVAL1 for peer delay response messages. */
592 	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
593 				  MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
594 	if (err)
595 		return err;
596 
597 	/* 88E6341 devices default to timestamping at the PHY, but this has
598 	 * a hardware issue that results in unreliable timestamps. Force
599 	 * these devices to timestamp at the MAC.
600 	 */
601 	if (chip->info->family == MV88E6XXX_FAMILY_6341) {
602 		u16 val = MV88E6341_PTP_CFG_UPDATE |
603 			  MV88E6341_PTP_CFG_MODE_IDX |
604 			  MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
605 		err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
606 		if (err)
607 			return err;
608 	}
609 
610 	return 0;
611 }
612 
mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip * chip)613 void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
614 {
615 }
616