2 * Marvell 88E6xxx Switch hardware timestamping support
4 * Copyright (c) 2008 Marvell Semiconductor
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>
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.
21 #include <linux/ptp_classify.h>
23 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
25 static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port,
26 int addr, u16 *data, int len)
28 if (!chip->info->ops->avb_ops->port_ptp_read)
31 return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr,
35 static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port,
38 if (!chip->info->ops->avb_ops->port_ptp_write)
41 return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr,
45 static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr,
48 if (!chip->info->ops->avb_ops->ptp_write)
51 return chip->info->ops->avb_ops->ptp_write(chip, addr, data);
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.
59 #define TX_TSTAMP_TIMEOUT msecs_to_jiffies(20)
61 int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
62 struct ethtool_ts_info *info)
64 struct mv88e6xxx_chip *chip = ds->priv;
66 if (!chip->info->ptp_support)
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);
75 (1 << HWTSTAMP_TX_OFF) |
76 (1 << HWTSTAMP_TX_ON);
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);
92 static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port,
93 struct hwtstamp_config *config)
95 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
96 bool tstamp_enable = false;
100 /* Prevent the TX/RX paths from trying to interact with the
101 * timestamp hardware while we reconfigure it.
103 clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
105 /* reserved for future extensions */
109 switch (config->tx_type) {
110 case HWTSTAMP_TX_OFF:
111 tstamp_enable = false;
114 tstamp_enable = true;
120 /* The switch supports timestamping both L2 and L4; one cannot be
121 * disabled independently of the other.
123 switch (config->rx_filter) {
124 case HWTSTAMP_FILTER_NONE:
125 tstamp_enable = false;
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;
138 case HWTSTAMP_FILTER_ALL:
140 config->rx_filter = HWTSTAMP_FILTER_NONE;
145 /* Disable transportSpecific value matching, so that packets
146 * with either 1588 (0) and 802.1AS (1) will be timestamped.
148 port_config0 = MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH;
150 /* Disable PTP. This disables both RX and TX timestamping. */
151 port_config0 = MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP;
154 mutex_lock(&chip->reg_lock);
155 err = mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
157 mutex_unlock(&chip->reg_lock);
162 /* Once hardware has been configured, enable timestamp checks
163 * in the RX/TX paths.
166 set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
171 int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port,
174 struct mv88e6xxx_chip *chip = ds->priv;
175 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
176 struct hwtstamp_config config;
179 if (!chip->info->ptp_support)
182 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
185 err = mv88e6xxx_set_hwtstamp_config(chip, port, &config);
189 /* Save the chosen configuration to be returned later. */
190 memcpy(&ps->tstamp_config, &config, sizeof(config));
192 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
196 int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port,
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;
203 if (!chip->info->ptp_support)
206 return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
210 /* Get the start of the PTP header in this skb */
211 static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type)
213 u8 *data = skb_mac_header(skb);
214 unsigned int offset = 0;
216 if (type & PTP_CLASS_VLAN)
219 switch (type & PTP_CLASS_PMASK) {
221 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
224 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
233 /* Ensure that the entire header is present in this packet. */
234 if (skb->len + ETH_HLEN < offset + 34)
237 return data + offset;
240 /* Returns a pointer to the PTP header if the caller should time stamp,
241 * or NULL if the caller should not.
243 static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port,
244 struct sk_buff *skb, unsigned int type)
246 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
249 if (!chip->info->ptp_support)
252 hdr = parse_ptp_header(skb, type);
256 if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
262 static int mv88e6xxx_ts_valid(u16 status)
264 if (!(status & MV88E6XXX_PTP_TS_VALID))
266 if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
271 static int seq_match(struct sk_buff *skb, u16 ts_seqid)
273 unsigned int type = SKB_PTP_TYPE(skb);
274 u8 *hdr = parse_ptp_header(skb, type);
277 seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
279 return ts_seqid == ntohs(*seqid);
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)
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;
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);
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);
305 pr_err("failed to get the receive time stamp\n");
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);
317 pr_err("failed to clear the receive status\n");
319 /* Since the device can only handle one time stamp at a time,
320 * we purge any extra frames from the queue.
322 for ( ; skb; skb = __skb_dequeue(&received)) {
323 if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
324 ns = timehi << 16 | timelo;
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;
338 static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
339 struct mv88e6xxx_port_hwtstamp *ps)
343 skb = skb_dequeue(&ps->rx_queue);
346 mv88e6xxx_get_rxts(chip, ps, skb, MV88E6XXX_PORT_PTP_ARR0_STS,
349 skb = skb_dequeue(&ps->rx_queue2);
351 mv88e6xxx_get_rxts(chip, ps, skb, MV88E6XXX_PORT_PTP_ARR1_STS,
355 static int is_pdelay_resp(u8 *msgtype)
357 return (*msgtype & 0xf) == 3;
360 bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
361 struct sk_buff *skb, unsigned int type)
363 struct mv88e6xxx_port_hwtstamp *ps;
364 struct mv88e6xxx_chip *chip;
368 ps = &chip->port_hwtstamp[port];
370 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
373 hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
377 SKB_PTP_TYPE(skb) = type;
379 if (is_pdelay_resp(hdr))
380 skb_queue_tail(&ps->rx_queue2, skb);
382 skb_queue_tail(&ps->rx_queue, skb);
384 ptp_schedule_worker(chip->ptp_clock, 0);
389 static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
390 struct mv88e6xxx_port_hwtstamp *ps)
392 struct skb_shared_hwtstamps shhwtstamps;
393 u16 departure_block[4], status;
394 struct sk_buff *tmp_skb;
402 mutex_lock(&chip->reg_lock);
403 err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
404 MV88E6XXX_PORT_PTP_DEP_STS,
406 ARRAY_SIZE(departure_block));
407 mutex_unlock(&chip->reg_lock);
410 goto free_and_clear_skb;
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",
417 goto free_and_clear_skb;
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.
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);
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;
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;
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);
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]);
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.
460 tmp_skb = ps->tx_skb;
462 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
463 skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
468 dev_kfree_skb_any(ps->tx_skb);
470 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
475 long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
477 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
478 struct dsa_switch *ds = chip->ds;
479 struct mv88e6xxx_port_hwtstamp *ps;
482 for (i = 0; i < ds->num_ports; i++) {
483 if (!dsa_is_user_port(ds, i))
486 ps = &chip->port_hwtstamp[i];
487 if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
488 restart |= mv88e6xxx_txtstamp_work(chip, ps);
490 mv88e6xxx_rxtstamp_work(chip, ps);
493 return restart ? 1 : -1;
496 bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
497 struct sk_buff *clone, unsigned int type)
499 struct mv88e6xxx_chip *chip = ds->priv;
500 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
504 if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
507 hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
511 seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
513 if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
518 ps->tx_tstamp_start = jiffies;
519 ps->tx_seq_id = be16_to_cpup(seq_ptr);
521 ptp_schedule_worker(chip->ptp_clock, 0);
525 static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
527 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
531 skb_queue_head_init(&ps->rx_queue);
532 skb_queue_head_init(&ps->rx_queue2);
534 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
535 MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
538 int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
543 /* Disable timestamping on all ports. */
544 for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
545 err = mv88e6xxx_hwtstamp_port_setup(chip, i);
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.
555 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
556 MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
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);
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.
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);
582 void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)