Loading...
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
25static 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
35static 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
45static 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
61int 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
92static 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
171int 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
196int 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 */
211static 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 */
243static 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
262static 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
271static 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
282static 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 struct skb_shared_hwtstamps *shwt;
289 struct sk_buff_head received;
290 u64 ns, timelo, timehi;
291 unsigned long flags;
292 int err;
293
294 /* The latched timestamp belongs to one of the received frames. */
295 __skb_queue_head_init(&received);
296 spin_lock_irqsave(&rxq->lock, flags);
297 skb_queue_splice_tail_init(rxq, &received);
298 spin_unlock_irqrestore(&rxq->lock, flags);
299
300 mutex_lock(&chip->reg_lock);
301 err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
302 reg, buf, ARRAY_SIZE(buf));
303 mutex_unlock(&chip->reg_lock);
304 if (err)
305 pr_err("failed to get the receive time stamp\n");
306
307 status = buf[0];
308 timelo = buf[1];
309 timehi = buf[2];
310 seq_id = buf[3];
311
312 if (status & MV88E6XXX_PTP_TS_VALID) {
313 mutex_lock(&chip->reg_lock);
314 err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
315 mutex_unlock(&chip->reg_lock);
316 if (err)
317 pr_err("failed to clear the receive status\n");
318 }
319 /* Since the device can only handle one time stamp at a time,
320 * we purge any extra frames from the queue.
321 */
322 for ( ; skb; skb = __skb_dequeue(&received)) {
323 if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
324 ns = timehi << 16 | timelo;
325
326 mutex_lock(&chip->reg_lock);
327 ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
328 mutex_unlock(&chip->reg_lock);
329 shwt = skb_hwtstamps(skb);
330 memset(shwt, 0, sizeof(*shwt));
331 shwt->hwtstamp = ns_to_ktime(ns);
332 status &= ~MV88E6XXX_PTP_TS_VALID;
333 }
334 netif_rx_ni(skb);
335 }
336}
337
338static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
339 struct mv88e6xxx_port_hwtstamp *ps)
340{
341 struct sk_buff *skb;
342
343 skb = skb_dequeue(&ps->rx_queue);
344
345 if (skb)
346 mv88e6xxx_get_rxts(chip, ps, skb, MV88E6XXX_PORT_PTP_ARR0_STS,
347 &ps->rx_queue);
348
349 skb = skb_dequeue(&ps->rx_queue2);
350 if (skb)
351 mv88e6xxx_get_rxts(chip, ps, skb, MV88E6XXX_PORT_PTP_ARR1_STS,
352 &ps->rx_queue2);
353}
354
355static int is_pdelay_resp(u8 *msgtype)
356{
357 return (*msgtype & 0xf) == 3;
358}
359
360bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
361 struct sk_buff *skb, unsigned int type)
362{
363 struct mv88e6xxx_port_hwtstamp *ps;
364 struct mv88e6xxx_chip *chip;
365 u8 *hdr;
366
367 chip = ds->priv;
368 ps = &chip->port_hwtstamp[port];
369
370 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
371 return false;
372
373 hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
374 if (!hdr)
375 return false;
376
377 SKB_PTP_TYPE(skb) = type;
378
379 if (is_pdelay_resp(hdr))
380 skb_queue_tail(&ps->rx_queue2, skb);
381 else
382 skb_queue_tail(&ps->rx_queue, skb);
383
384 ptp_schedule_worker(chip->ptp_clock, 0);
385
386 return true;
387}
388
389static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
390 struct mv88e6xxx_port_hwtstamp *ps)
391{
392 struct skb_shared_hwtstamps shhwtstamps;
393 u16 departure_block[4], status;
394 struct sk_buff *tmp_skb;
395 u32 time_raw;
396 int err;
397 u64 ns;
398
399 if (!ps->tx_skb)
400 return 0;
401
402 mutex_lock(&chip->reg_lock);
403 err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
404 MV88E6XXX_PORT_PTP_DEP_STS,
405 departure_block,
406 ARRAY_SIZE(departure_block));
407 mutex_unlock(&chip->reg_lock);
408
409 if (err)
410 goto free_and_clear_skb;
411
412 if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
413 if (time_is_before_jiffies(ps->tx_tstamp_start +
414 TX_TSTAMP_TIMEOUT)) {
415 dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
416 ps->port_id);
417 goto free_and_clear_skb;
418 }
419 /* The timestamp should be available quickly, while getting it
420 * is high priority and time bounded to only 10ms. A poll is
421 * warranted so restart the work.
422 */
423 return 1;
424 }
425
426 /* We have the timestamp; go ahead and clear valid now */
427 mutex_lock(&chip->reg_lock);
428 mv88e6xxx_port_ptp_write(chip, ps->port_id,
429 MV88E6XXX_PORT_PTP_DEP_STS, 0);
430 mutex_unlock(&chip->reg_lock);
431
432 status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
433 if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
434 dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
435 goto free_and_clear_skb;
436 }
437
438 if (departure_block[3] != ps->tx_seq_id) {
439 dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
440 goto free_and_clear_skb;
441 }
442
443 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
444 time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
445 mutex_lock(&chip->reg_lock);
446 ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
447 mutex_unlock(&chip->reg_lock);
448 shhwtstamps.hwtstamp = ns_to_ktime(ns);
449
450 dev_dbg(chip->dev,
451 "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
452 ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
453 departure_block[0], ps->tx_seq_id, departure_block[3]);
454
455 /* skb_complete_tx_timestamp() will free up the client to make
456 * another timestamp-able transmit. We have to be ready for it
457 * -- by clearing the ps->tx_skb "flag" -- beforehand.
458 */
459
460 tmp_skb = ps->tx_skb;
461 ps->tx_skb = NULL;
462 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
463 skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
464
465 return 0;
466
467free_and_clear_skb:
468 dev_kfree_skb_any(ps->tx_skb);
469 ps->tx_skb = NULL;
470 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
471
472 return 0;
473}
474
475long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
476{
477 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
478 struct dsa_switch *ds = chip->ds;
479 struct mv88e6xxx_port_hwtstamp *ps;
480 int i, restart = 0;
481
482 for (i = 0; i < ds->num_ports; i++) {
483 if (!dsa_is_user_port(ds, i))
484 continue;
485
486 ps = &chip->port_hwtstamp[i];
487 if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
488 restart |= mv88e6xxx_txtstamp_work(chip, ps);
489
490 mv88e6xxx_rxtstamp_work(chip, ps);
491 }
492
493 return restart ? 1 : -1;
494}
495
496bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
497 struct sk_buff *clone, unsigned int type)
498{
499 struct mv88e6xxx_chip *chip = ds->priv;
500 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
501 __be16 *seq_ptr;
502 u8 *hdr;
503
504 if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
505 return false;
506
507 hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
508 if (!hdr)
509 return false;
510
511 seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
512
513 if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
514 &ps->state))
515 return false;
516
517 ps->tx_skb = clone;
518 ps->tx_tstamp_start = jiffies;
519 ps->tx_seq_id = be16_to_cpup(seq_ptr);
520
521 ptp_schedule_worker(chip->ptp_clock, 0);
522 return true;
523}
524
525static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
526{
527 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
528
529 ps->port_id = port;
530
531 skb_queue_head_init(&ps->rx_queue);
532 skb_queue_head_init(&ps->rx_queue2);
533
534 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
535 MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
536}
537
538int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
539{
540 int err;
541 int i;
542
543 /* Disable timestamping on all ports. */
544 for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
545 err = mv88e6xxx_hwtstamp_port_setup(chip, i);
546 if (err)
547 return err;
548 }
549
550 /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
551 * timestamp. This affects all ports that have timestamping enabled,
552 * but the timestamp config is per-port; thus we configure all events
553 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
554 */
555 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
556 MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
557 if (err)
558 return err;
559
560 /* Use ARRIVAL1 for peer delay response messages. */
561 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
562 MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
563 if (err)
564 return err;
565
566 /* 88E6341 devices default to timestamping at the PHY, but this has
567 * a hardware issue that results in unreliable timestamps. Force
568 * these devices to timestamp at the MAC.
569 */
570 if (chip->info->family == MV88E6XXX_FAMILY_6341) {
571 u16 val = MV88E6341_PTP_CFG_UPDATE |
572 MV88E6341_PTP_CFG_MODE_IDX |
573 MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
574 err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
575 if (err)
576 return err;
577 }
578
579 return 0;
580}
581
582void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
583{
584}
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 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 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 /* reserved for future extensions */
104 if (config->flags)
105 return -EINVAL;
106
107 switch (config->tx_type) {
108 case HWTSTAMP_TX_OFF:
109 tstamp_enable = false;
110 break;
111 case HWTSTAMP_TX_ON:
112 tstamp_enable = true;
113 break;
114 default:
115 return -ERANGE;
116 }
117
118 /* The switch supports timestamping both L2 and L4; one cannot be
119 * disabled independently of the other.
120 */
121
122 if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) {
123 config->rx_filter = HWTSTAMP_FILTER_NONE;
124 dev_dbg(chip->dev, "Unsupported rx_filter %d\n",
125 config->rx_filter);
126 return -ERANGE;
127 }
128
129 switch (config->rx_filter) {
130 case HWTSTAMP_FILTER_NONE:
131 tstamp_enable = false;
132 break;
133 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
134 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
135 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
136 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
137 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
138 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
139 case HWTSTAMP_FILTER_PTP_V2_EVENT:
140 case HWTSTAMP_FILTER_PTP_V2_SYNC:
141 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
142 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
143 break;
144 case HWTSTAMP_FILTER_ALL:
145 default:
146 config->rx_filter = HWTSTAMP_FILTER_NONE;
147 return -ERANGE;
148 }
149
150 mv88e6xxx_reg_lock(chip);
151 if (tstamp_enable) {
152 chip->enable_count += 1;
153 if (chip->enable_count == 1 && ptp_ops->global_enable)
154 ptp_ops->global_enable(chip);
155 if (ptp_ops->port_enable)
156 ptp_ops->port_enable(chip, port);
157 } else {
158 if (ptp_ops->port_disable)
159 ptp_ops->port_disable(chip, port);
160 chip->enable_count -= 1;
161 if (chip->enable_count == 0 && ptp_ops->global_disable)
162 ptp_ops->global_disable(chip);
163 }
164 mv88e6xxx_reg_unlock(chip);
165
166 /* Once hardware has been configured, enable timestamp checks
167 * in the RX/TX paths.
168 */
169 if (tstamp_enable)
170 set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
171
172 return 0;
173}
174
175int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port,
176 struct ifreq *ifr)
177{
178 struct mv88e6xxx_chip *chip = ds->priv;
179 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
180 struct hwtstamp_config config;
181 int err;
182
183 if (!chip->info->ptp_support)
184 return -EOPNOTSUPP;
185
186 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
187 return -EFAULT;
188
189 err = mv88e6xxx_set_hwtstamp_config(chip, port, &config);
190 if (err)
191 return err;
192
193 /* Save the chosen configuration to be returned later. */
194 memcpy(&ps->tstamp_config, &config, sizeof(config));
195
196 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
197 -EFAULT : 0;
198}
199
200int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port,
201 struct ifreq *ifr)
202{
203 struct mv88e6xxx_chip *chip = ds->priv;
204 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
205 struct hwtstamp_config *config = &ps->tstamp_config;
206
207 if (!chip->info->ptp_support)
208 return -EOPNOTSUPP;
209
210 return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
211 -EFAULT : 0;
212}
213
214/* Get the start of the PTP header in this skb */
215static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type)
216{
217 u8 *data = skb_mac_header(skb);
218 unsigned int offset = 0;
219
220 if (type & PTP_CLASS_VLAN)
221 offset += VLAN_HLEN;
222
223 switch (type & PTP_CLASS_PMASK) {
224 case PTP_CLASS_IPV4:
225 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
226 break;
227 case PTP_CLASS_IPV6:
228 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
229 break;
230 case PTP_CLASS_L2:
231 offset += ETH_HLEN;
232 break;
233 default:
234 return NULL;
235 }
236
237 /* Ensure that the entire header is present in this packet. */
238 if (skb->len + ETH_HLEN < offset + 34)
239 return NULL;
240
241 return data + offset;
242}
243
244/* Returns a pointer to the PTP header if the caller should time stamp,
245 * or NULL if the caller should not.
246 */
247static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port,
248 struct sk_buff *skb, unsigned int type)
249{
250 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
251 u8 *hdr;
252
253 if (!chip->info->ptp_support)
254 return NULL;
255
256 hdr = parse_ptp_header(skb, type);
257 if (!hdr)
258 return NULL;
259
260 if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
261 return NULL;
262
263 return hdr;
264}
265
266static int mv88e6xxx_ts_valid(u16 status)
267{
268 if (!(status & MV88E6XXX_PTP_TS_VALID))
269 return 0;
270 if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
271 return 0;
272 return 1;
273}
274
275static int seq_match(struct sk_buff *skb, u16 ts_seqid)
276{
277 unsigned int type = SKB_PTP_TYPE(skb);
278 u8 *hdr = parse_ptp_header(skb, type);
279 __be16 *seqid;
280
281 seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
282
283 return ts_seqid == ntohs(*seqid);
284}
285
286static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
287 struct mv88e6xxx_port_hwtstamp *ps,
288 struct sk_buff *skb, u16 reg,
289 struct sk_buff_head *rxq)
290{
291 u16 buf[4] = { 0 }, status, seq_id;
292 struct skb_shared_hwtstamps *shwt;
293 struct sk_buff_head received;
294 u64 ns, timelo, timehi;
295 unsigned long flags;
296 int err;
297
298 /* The latched timestamp belongs to one of the received frames. */
299 __skb_queue_head_init(&received);
300 spin_lock_irqsave(&rxq->lock, flags);
301 skb_queue_splice_tail_init(rxq, &received);
302 spin_unlock_irqrestore(&rxq->lock, flags);
303
304 mv88e6xxx_reg_lock(chip);
305 err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
306 reg, buf, ARRAY_SIZE(buf));
307 mv88e6xxx_reg_unlock(chip);
308 if (err)
309 pr_err("failed to get the receive time stamp\n");
310
311 status = buf[0];
312 timelo = buf[1];
313 timehi = buf[2];
314 seq_id = buf[3];
315
316 if (status & MV88E6XXX_PTP_TS_VALID) {
317 mv88e6xxx_reg_lock(chip);
318 err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
319 mv88e6xxx_reg_unlock(chip);
320 if (err)
321 pr_err("failed to clear the receive status\n");
322 }
323 /* Since the device can only handle one time stamp at a time,
324 * we purge any extra frames from the queue.
325 */
326 for ( ; skb; skb = __skb_dequeue(&received)) {
327 if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
328 ns = timehi << 16 | timelo;
329
330 mv88e6xxx_reg_lock(chip);
331 ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
332 mv88e6xxx_reg_unlock(chip);
333 shwt = skb_hwtstamps(skb);
334 memset(shwt, 0, sizeof(*shwt));
335 shwt->hwtstamp = ns_to_ktime(ns);
336 status &= ~MV88E6XXX_PTP_TS_VALID;
337 }
338 netif_rx_ni(skb);
339 }
340}
341
342static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
343 struct mv88e6xxx_port_hwtstamp *ps)
344{
345 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
346 struct sk_buff *skb;
347
348 skb = skb_dequeue(&ps->rx_queue);
349
350 if (skb)
351 mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg,
352 &ps->rx_queue);
353
354 skb = skb_dequeue(&ps->rx_queue2);
355 if (skb)
356 mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg,
357 &ps->rx_queue2);
358}
359
360static int is_pdelay_resp(u8 *msgtype)
361{
362 return (*msgtype & 0xf) == 3;
363}
364
365bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
366 struct sk_buff *skb, unsigned int type)
367{
368 struct mv88e6xxx_port_hwtstamp *ps;
369 struct mv88e6xxx_chip *chip;
370 u8 *hdr;
371
372 chip = ds->priv;
373 ps = &chip->port_hwtstamp[port];
374
375 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
376 return false;
377
378 hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
379 if (!hdr)
380 return false;
381
382 SKB_PTP_TYPE(skb) = type;
383
384 if (is_pdelay_resp(hdr))
385 skb_queue_tail(&ps->rx_queue2, skb);
386 else
387 skb_queue_tail(&ps->rx_queue, skb);
388
389 ptp_schedule_worker(chip->ptp_clock, 0);
390
391 return true;
392}
393
394static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
395 struct mv88e6xxx_port_hwtstamp *ps)
396{
397 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
398 struct skb_shared_hwtstamps shhwtstamps;
399 u16 departure_block[4], status;
400 struct sk_buff *tmp_skb;
401 u32 time_raw;
402 int err;
403 u64 ns;
404
405 if (!ps->tx_skb)
406 return 0;
407
408 mv88e6xxx_reg_lock(chip);
409 err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
410 ptp_ops->dep_sts_reg,
411 departure_block,
412 ARRAY_SIZE(departure_block));
413 mv88e6xxx_reg_unlock(chip);
414
415 if (err)
416 goto free_and_clear_skb;
417
418 if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
419 if (time_is_before_jiffies(ps->tx_tstamp_start +
420 TX_TSTAMP_TIMEOUT)) {
421 dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
422 ps->port_id);
423 goto free_and_clear_skb;
424 }
425 /* The timestamp should be available quickly, while getting it
426 * is high priority and time bounded to only 10ms. A poll is
427 * warranted so restart the work.
428 */
429 return 1;
430 }
431
432 /* We have the timestamp; go ahead and clear valid now */
433 mv88e6xxx_reg_lock(chip);
434 mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
435 mv88e6xxx_reg_unlock(chip);
436
437 status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
438 if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
439 dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
440 goto free_and_clear_skb;
441 }
442
443 if (departure_block[3] != ps->tx_seq_id) {
444 dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
445 goto free_and_clear_skb;
446 }
447
448 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
449 time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
450 mv88e6xxx_reg_lock(chip);
451 ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
452 mv88e6xxx_reg_unlock(chip);
453 shhwtstamps.hwtstamp = ns_to_ktime(ns);
454
455 dev_dbg(chip->dev,
456 "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
457 ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
458 departure_block[0], ps->tx_seq_id, departure_block[3]);
459
460 /* skb_complete_tx_timestamp() will free up the client to make
461 * another timestamp-able transmit. We have to be ready for it
462 * -- by clearing the ps->tx_skb "flag" -- beforehand.
463 */
464
465 tmp_skb = ps->tx_skb;
466 ps->tx_skb = NULL;
467 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
468 skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
469
470 return 0;
471
472free_and_clear_skb:
473 dev_kfree_skb_any(ps->tx_skb);
474 ps->tx_skb = NULL;
475 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
476
477 return 0;
478}
479
480long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
481{
482 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
483 struct dsa_switch *ds = chip->ds;
484 struct mv88e6xxx_port_hwtstamp *ps;
485 int i, restart = 0;
486
487 for (i = 0; i < ds->num_ports; i++) {
488 if (!dsa_is_user_port(ds, i))
489 continue;
490
491 ps = &chip->port_hwtstamp[i];
492 if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
493 restart |= mv88e6xxx_txtstamp_work(chip, ps);
494
495 mv88e6xxx_rxtstamp_work(chip, ps);
496 }
497
498 return restart ? 1 : -1;
499}
500
501bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
502 struct sk_buff *clone, unsigned int type)
503{
504 struct mv88e6xxx_chip *chip = ds->priv;
505 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
506 __be16 *seq_ptr;
507 u8 *hdr;
508
509 if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
510 return false;
511
512 hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
513 if (!hdr)
514 return false;
515
516 seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
517
518 if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
519 &ps->state))
520 return false;
521
522 ps->tx_skb = clone;
523 ps->tx_tstamp_start = jiffies;
524 ps->tx_seq_id = be16_to_cpup(seq_ptr);
525
526 ptp_schedule_worker(chip->ptp_clock, 0);
527 return true;
528}
529
530int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
531{
532 u16 val;
533 int err;
534
535 err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
536 if (err)
537 return err;
538 val |= MV88E6165_PTP_CFG_DISABLE_PTP;
539
540 return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
541}
542
543int mv88e6165_global_enable(struct mv88e6xxx_chip *chip)
544{
545 u16 val;
546 int err;
547
548 err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
549 if (err)
550 return err;
551
552 val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK);
553
554 return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
555}
556
557int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port)
558{
559 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
560 MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
561}
562
563int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port)
564{
565 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
566 MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH);
567}
568
569static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
570{
571 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
572 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
573
574 ps->port_id = port;
575
576 skb_queue_head_init(&ps->rx_queue);
577 skb_queue_head_init(&ps->rx_queue2);
578
579 if (ptp_ops->port_disable)
580 return ptp_ops->port_disable(chip, port);
581
582 return 0;
583}
584
585int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
586{
587 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
588 int err;
589 int i;
590
591 /* Disable timestamping on all ports. */
592 for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
593 err = mv88e6xxx_hwtstamp_port_setup(chip, i);
594 if (err)
595 return err;
596 }
597
598 /* Disable PTP globally */
599 if (ptp_ops->global_disable) {
600 err = ptp_ops->global_disable(chip);
601 if (err)
602 return err;
603 }
604
605 /* Set the ethertype of L2 PTP messages */
606 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588);
607 if (err)
608 return err;
609
610 /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
611 * timestamp. This affects all ports that have timestamping enabled,
612 * but the timestamp config is per-port; thus we configure all events
613 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
614 */
615 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
616 MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
617 if (err)
618 return err;
619
620 /* Use ARRIVAL1 for peer delay response messages. */
621 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
622 MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
623 if (err)
624 return err;
625
626 /* 88E6341 devices default to timestamping at the PHY, but this has
627 * a hardware issue that results in unreliable timestamps. Force
628 * these devices to timestamp at the MAC.
629 */
630 if (chip->info->family == MV88E6XXX_FAMILY_6341) {
631 u16 val = MV88E6341_PTP_CFG_UPDATE |
632 MV88E6341_PTP_CFG_MODE_IDX |
633 MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
634 err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
635 if (err)
636 return err;
637 }
638
639 return 0;
640}
641
642void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
643{
644}