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