616 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Marvell 88E6xxx Switch hardware timestamping support4 *5 * Copyright (c) 2008 Marvell Semiconductor6 *7 * Copyright (c) 2017 National Instruments8 * 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 TX60 * timestamp. When working properly, hardware will produce a timestamp61 * within 1ms. Software may enounter delays due to MDIO contention, so62 * 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 kernel_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 the99 * 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 be115 * 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 checks163 * 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/* Returns a pointer to the PTP header if the caller should time stamp,211 * or NULL if the caller should not.212 */213static 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 233static 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 242static 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 252static 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 308static 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 326static int is_pdelay_resp(const struct ptp_header *hdr)327{328 return (hdr->tsmt & 0xf) == 3;329}330 331bool 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 else353 skb_queue_tail(&ps->rx_queue, skb);354 355 ptp_schedule_worker(chip->ptp_clock, 0);356 357 return true;358}359 360static 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 it392 * is high priority and time bounded to only 10ms. A poll is393 * 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 make427 * another timestamp-able transmit. We have to be ready for it428 * -- 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 438free_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 446long 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 467void 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 501int 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 514int 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 528int 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 534int 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 540static 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 556int 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 to582 * timestamp. This affects all ports that have timestamping enabled,583 * but the timestamp config is per-port; thus we configure all events584 * 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 has598 * a hardware issue that results in unreliable timestamps. Force599 * 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 613void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)614{615}616