12874c5fdSThomas Gleixner // SPDX-License-Identifier: GPL-2.0-or-later 2c6fe0ad2SBrandon Streiff /* 3c6fe0ad2SBrandon Streiff * Marvell 88E6xxx Switch hardware timestamping support 4c6fe0ad2SBrandon Streiff * 5c6fe0ad2SBrandon Streiff * Copyright (c) 2008 Marvell Semiconductor 6c6fe0ad2SBrandon Streiff * 7c6fe0ad2SBrandon Streiff * Copyright (c) 2017 National Instruments 8c6fe0ad2SBrandon Streiff * Erik Hons <erik.hons@ni.com> 9c6fe0ad2SBrandon Streiff * Brandon Streiff <brandon.streiff@ni.com> 10c6fe0ad2SBrandon Streiff * Dane Wagner <dane.wagner@ni.com> 11c6fe0ad2SBrandon Streiff */ 12c6fe0ad2SBrandon Streiff 13c6fe0ad2SBrandon Streiff #include "chip.h" 14c6fe0ad2SBrandon Streiff #include "global2.h" 15c6fe0ad2SBrandon Streiff #include "hwtstamp.h" 16c6fe0ad2SBrandon Streiff #include "ptp.h" 17c6fe0ad2SBrandon Streiff #include <linux/ptp_classify.h> 18c6fe0ad2SBrandon Streiff 19c6fe0ad2SBrandon Streiff #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb)) 20c6fe0ad2SBrandon Streiff 21c6fe0ad2SBrandon Streiff static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port, 22c6fe0ad2SBrandon Streiff int addr, u16 *data, int len) 23c6fe0ad2SBrandon Streiff { 24c6fe0ad2SBrandon Streiff if (!chip->info->ops->avb_ops->port_ptp_read) 25c6fe0ad2SBrandon Streiff return -EOPNOTSUPP; 26c6fe0ad2SBrandon Streiff 27c6fe0ad2SBrandon Streiff return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr, 28c6fe0ad2SBrandon Streiff data, len); 29c6fe0ad2SBrandon Streiff } 30c6fe0ad2SBrandon Streiff 31c6fe0ad2SBrandon Streiff static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port, 32c6fe0ad2SBrandon Streiff int addr, u16 data) 33c6fe0ad2SBrandon Streiff { 34c6fe0ad2SBrandon Streiff if (!chip->info->ops->avb_ops->port_ptp_write) 35c6fe0ad2SBrandon Streiff return -EOPNOTSUPP; 36c6fe0ad2SBrandon Streiff 37c6fe0ad2SBrandon Streiff return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr, 38c6fe0ad2SBrandon Streiff data); 39c6fe0ad2SBrandon Streiff } 40c6fe0ad2SBrandon Streiff 41c6fe0ad2SBrandon Streiff static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr, 42c6fe0ad2SBrandon Streiff u16 data) 43c6fe0ad2SBrandon Streiff { 44c6fe0ad2SBrandon Streiff if (!chip->info->ops->avb_ops->ptp_write) 45c6fe0ad2SBrandon Streiff return -EOPNOTSUPP; 46c6fe0ad2SBrandon Streiff 47c6fe0ad2SBrandon Streiff return chip->info->ops->avb_ops->ptp_write(chip, addr, data); 48c6fe0ad2SBrandon Streiff } 49c6fe0ad2SBrandon Streiff 50e2294a8bSAndrew Lunn static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr, 51e2294a8bSAndrew Lunn u16 *data) 52e2294a8bSAndrew Lunn { 53e2294a8bSAndrew Lunn if (!chip->info->ops->avb_ops->ptp_read) 54e2294a8bSAndrew Lunn return -EOPNOTSUPP; 55e2294a8bSAndrew Lunn 56e2294a8bSAndrew Lunn return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1); 57e2294a8bSAndrew Lunn } 58e2294a8bSAndrew Lunn 59c6fe0ad2SBrandon Streiff /* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX 60c6fe0ad2SBrandon Streiff * timestamp. When working properly, hardware will produce a timestamp 61c6fe0ad2SBrandon Streiff * within 1ms. Software may enounter delays due to MDIO contention, so 62c6fe0ad2SBrandon Streiff * the timeout is set accordingly. 63c6fe0ad2SBrandon Streiff */ 64df31b74cSAndrew Lunn #define TX_TSTAMP_TIMEOUT msecs_to_jiffies(40) 65c6fe0ad2SBrandon Streiff 66c6fe0ad2SBrandon Streiff int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, 67c6fe0ad2SBrandon Streiff struct ethtool_ts_info *info) 68c6fe0ad2SBrandon Streiff { 6948cb5e03SAndrew Lunn const struct mv88e6xxx_ptp_ops *ptp_ops; 7048cb5e03SAndrew Lunn struct mv88e6xxx_chip *chip; 7148cb5e03SAndrew Lunn 7248cb5e03SAndrew Lunn chip = ds->priv; 7348cb5e03SAndrew Lunn ptp_ops = chip->info->ops->ptp_ops; 74c6fe0ad2SBrandon Streiff 75c6fe0ad2SBrandon Streiff if (!chip->info->ptp_support) 76c6fe0ad2SBrandon Streiff return -EOPNOTSUPP; 77c6fe0ad2SBrandon Streiff 78c6fe0ad2SBrandon Streiff info->so_timestamping = 79c6fe0ad2SBrandon Streiff SOF_TIMESTAMPING_TX_HARDWARE | 80c6fe0ad2SBrandon Streiff SOF_TIMESTAMPING_RX_HARDWARE | 81c6fe0ad2SBrandon Streiff SOF_TIMESTAMPING_RAW_HARDWARE; 82c6fe0ad2SBrandon Streiff info->phc_index = ptp_clock_index(chip->ptp_clock); 83c6fe0ad2SBrandon Streiff info->tx_types = 84c6fe0ad2SBrandon Streiff (1 << HWTSTAMP_TX_OFF) | 85c6fe0ad2SBrandon Streiff (1 << HWTSTAMP_TX_ON); 8648cb5e03SAndrew Lunn info->rx_filters = ptp_ops->rx_filters; 87c6fe0ad2SBrandon Streiff 88c6fe0ad2SBrandon Streiff return 0; 89c6fe0ad2SBrandon Streiff } 90c6fe0ad2SBrandon Streiff 91c6fe0ad2SBrandon Streiff static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port, 92c6fe0ad2SBrandon Streiff struct hwtstamp_config *config) 93c6fe0ad2SBrandon Streiff { 94ffc705deSAndrew Lunn const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 95c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 96c6fe0ad2SBrandon Streiff bool tstamp_enable = false; 97c6fe0ad2SBrandon Streiff 98c6fe0ad2SBrandon Streiff /* Prevent the TX/RX paths from trying to interact with the 99c6fe0ad2SBrandon Streiff * timestamp hardware while we reconfigure it. 100c6fe0ad2SBrandon Streiff */ 101c6fe0ad2SBrandon Streiff clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); 102c6fe0ad2SBrandon Streiff 103c6fe0ad2SBrandon Streiff /* reserved for future extensions */ 104c6fe0ad2SBrandon Streiff if (config->flags) 105c6fe0ad2SBrandon Streiff return -EINVAL; 106c6fe0ad2SBrandon Streiff 107c6fe0ad2SBrandon Streiff switch (config->tx_type) { 108c6fe0ad2SBrandon Streiff case HWTSTAMP_TX_OFF: 109c6fe0ad2SBrandon Streiff tstamp_enable = false; 110c6fe0ad2SBrandon Streiff break; 111c6fe0ad2SBrandon Streiff case HWTSTAMP_TX_ON: 112c6fe0ad2SBrandon Streiff tstamp_enable = true; 113c6fe0ad2SBrandon Streiff break; 114c6fe0ad2SBrandon Streiff default: 115c6fe0ad2SBrandon Streiff return -ERANGE; 116c6fe0ad2SBrandon Streiff } 117c6fe0ad2SBrandon Streiff 118c6fe0ad2SBrandon Streiff /* The switch supports timestamping both L2 and L4; one cannot be 119c6fe0ad2SBrandon Streiff * disabled independently of the other. 120c6fe0ad2SBrandon Streiff */ 12148cb5e03SAndrew Lunn 12248cb5e03SAndrew Lunn if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) { 12348cb5e03SAndrew Lunn config->rx_filter = HWTSTAMP_FILTER_NONE; 12448cb5e03SAndrew Lunn dev_dbg(chip->dev, "Unsupported rx_filter %d\n", 12548cb5e03SAndrew Lunn config->rx_filter); 12648cb5e03SAndrew Lunn return -ERANGE; 12748cb5e03SAndrew Lunn } 12848cb5e03SAndrew Lunn 129c6fe0ad2SBrandon Streiff switch (config->rx_filter) { 130c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_NONE: 131c6fe0ad2SBrandon Streiff tstamp_enable = false; 132c6fe0ad2SBrandon Streiff break; 133c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 134c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 135c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 136c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 137c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 138c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 139c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_EVENT: 140c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_SYNC: 141c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 142c6fe0ad2SBrandon Streiff config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 143c6fe0ad2SBrandon Streiff break; 144c6fe0ad2SBrandon Streiff case HWTSTAMP_FILTER_ALL: 145c6fe0ad2SBrandon Streiff default: 146c6fe0ad2SBrandon Streiff config->rx_filter = HWTSTAMP_FILTER_NONE; 147c6fe0ad2SBrandon Streiff return -ERANGE; 148c6fe0ad2SBrandon Streiff } 149c6fe0ad2SBrandon Streiff 150c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 151ffc705deSAndrew Lunn if (tstamp_enable) { 152e2294a8bSAndrew Lunn chip->enable_count += 1; 153e2294a8bSAndrew Lunn if (chip->enable_count == 1 && ptp_ops->global_enable) 154e2294a8bSAndrew Lunn ptp_ops->global_enable(chip); 155ffc705deSAndrew Lunn if (ptp_ops->port_enable) 156ffc705deSAndrew Lunn ptp_ops->port_enable(chip, port); 157ffc705deSAndrew Lunn } else { 158ffc705deSAndrew Lunn if (ptp_ops->port_disable) 159ffc705deSAndrew Lunn ptp_ops->port_disable(chip, port); 160e2294a8bSAndrew Lunn chip->enable_count -= 1; 161e2294a8bSAndrew Lunn if (chip->enable_count == 0 && ptp_ops->global_disable) 162e2294a8bSAndrew Lunn ptp_ops->global_disable(chip); 163ffc705deSAndrew Lunn } 164c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 165c6fe0ad2SBrandon Streiff 166c6fe0ad2SBrandon Streiff /* Once hardware has been configured, enable timestamp checks 167c6fe0ad2SBrandon Streiff * in the RX/TX paths. 168c6fe0ad2SBrandon Streiff */ 169c6fe0ad2SBrandon Streiff if (tstamp_enable) 170c6fe0ad2SBrandon Streiff set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); 171c6fe0ad2SBrandon Streiff 172c6fe0ad2SBrandon Streiff return 0; 173c6fe0ad2SBrandon Streiff } 174c6fe0ad2SBrandon Streiff 175c6fe0ad2SBrandon Streiff int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port, 176c6fe0ad2SBrandon Streiff struct ifreq *ifr) 177c6fe0ad2SBrandon Streiff { 178c6fe0ad2SBrandon Streiff struct mv88e6xxx_chip *chip = ds->priv; 179c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 180c6fe0ad2SBrandon Streiff struct hwtstamp_config config; 181c6fe0ad2SBrandon Streiff int err; 182c6fe0ad2SBrandon Streiff 183c6fe0ad2SBrandon Streiff if (!chip->info->ptp_support) 184c6fe0ad2SBrandon Streiff return -EOPNOTSUPP; 185c6fe0ad2SBrandon Streiff 186c6fe0ad2SBrandon Streiff if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 187c6fe0ad2SBrandon Streiff return -EFAULT; 188c6fe0ad2SBrandon Streiff 189c6fe0ad2SBrandon Streiff err = mv88e6xxx_set_hwtstamp_config(chip, port, &config); 190c6fe0ad2SBrandon Streiff if (err) 191c6fe0ad2SBrandon Streiff return err; 192c6fe0ad2SBrandon Streiff 193c6fe0ad2SBrandon Streiff /* Save the chosen configuration to be returned later. */ 194c6fe0ad2SBrandon Streiff memcpy(&ps->tstamp_config, &config, sizeof(config)); 195c6fe0ad2SBrandon Streiff 196c6fe0ad2SBrandon Streiff return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? 197c6fe0ad2SBrandon Streiff -EFAULT : 0; 198c6fe0ad2SBrandon Streiff } 199c6fe0ad2SBrandon Streiff 200c6fe0ad2SBrandon Streiff int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port, 201c6fe0ad2SBrandon Streiff struct ifreq *ifr) 202c6fe0ad2SBrandon Streiff { 203c6fe0ad2SBrandon Streiff struct mv88e6xxx_chip *chip = ds->priv; 204c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 205c6fe0ad2SBrandon Streiff struct hwtstamp_config *config = &ps->tstamp_config; 206c6fe0ad2SBrandon Streiff 207c6fe0ad2SBrandon Streiff if (!chip->info->ptp_support) 208c6fe0ad2SBrandon Streiff return -EOPNOTSUPP; 209c6fe0ad2SBrandon Streiff 210c6fe0ad2SBrandon Streiff return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ? 211c6fe0ad2SBrandon Streiff -EFAULT : 0; 212c6fe0ad2SBrandon Streiff } 213c6fe0ad2SBrandon Streiff 214c6fe0ad2SBrandon Streiff /* Returns a pointer to the PTP header if the caller should time stamp, 215c6fe0ad2SBrandon Streiff * or NULL if the caller should not. 216c6fe0ad2SBrandon Streiff */ 21728fba67fSKurt Kanzenbach static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, 21828fba67fSKurt Kanzenbach int port, struct sk_buff *skb, 21928fba67fSKurt Kanzenbach unsigned int type) 220c6fe0ad2SBrandon Streiff { 221c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 22228fba67fSKurt Kanzenbach struct ptp_header *hdr; 223c6fe0ad2SBrandon Streiff 224c6fe0ad2SBrandon Streiff if (!chip->info->ptp_support) 225c6fe0ad2SBrandon Streiff return NULL; 226c6fe0ad2SBrandon Streiff 22728fba67fSKurt Kanzenbach hdr = ptp_parse_header(skb, type); 228c6fe0ad2SBrandon Streiff if (!hdr) 229c6fe0ad2SBrandon Streiff return NULL; 230c6fe0ad2SBrandon Streiff 231c6fe0ad2SBrandon Streiff if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state)) 232c6fe0ad2SBrandon Streiff return NULL; 233c6fe0ad2SBrandon Streiff 234c6fe0ad2SBrandon Streiff return hdr; 235c6fe0ad2SBrandon Streiff } 236c6fe0ad2SBrandon Streiff 237c6fe0ad2SBrandon Streiff static int mv88e6xxx_ts_valid(u16 status) 238c6fe0ad2SBrandon Streiff { 239c6fe0ad2SBrandon Streiff if (!(status & MV88E6XXX_PTP_TS_VALID)) 240c6fe0ad2SBrandon Streiff return 0; 241c6fe0ad2SBrandon Streiff if (status & MV88E6XXX_PTP_TS_STATUS_MASK) 242c6fe0ad2SBrandon Streiff return 0; 243c6fe0ad2SBrandon Streiff return 1; 244c6fe0ad2SBrandon Streiff } 245c6fe0ad2SBrandon Streiff 246c6fe0ad2SBrandon Streiff static int seq_match(struct sk_buff *skb, u16 ts_seqid) 247c6fe0ad2SBrandon Streiff { 248c6fe0ad2SBrandon Streiff unsigned int type = SKB_PTP_TYPE(skb); 24928fba67fSKurt Kanzenbach struct ptp_header *hdr; 250c6fe0ad2SBrandon Streiff 25128fba67fSKurt Kanzenbach hdr = ptp_parse_header(skb, type); 252c6fe0ad2SBrandon Streiff 25328fba67fSKurt Kanzenbach return ts_seqid == ntohs(hdr->sequence_id); 254c6fe0ad2SBrandon Streiff } 255c6fe0ad2SBrandon Streiff 256c6fe0ad2SBrandon Streiff static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip, 257c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps, 258c6fe0ad2SBrandon Streiff struct sk_buff *skb, u16 reg, 259c6fe0ad2SBrandon Streiff struct sk_buff_head *rxq) 260c6fe0ad2SBrandon Streiff { 261b2d12101SColin Ian King u16 buf[4] = { 0 }, status, seq_id; 262c6fe0ad2SBrandon Streiff struct skb_shared_hwtstamps *shwt; 26322904823SRichard Cochran struct sk_buff_head received; 26422904823SRichard Cochran u64 ns, timelo, timehi; 26522904823SRichard Cochran unsigned long flags; 266c6fe0ad2SBrandon Streiff int err; 267c6fe0ad2SBrandon Streiff 26822904823SRichard Cochran /* The latched timestamp belongs to one of the received frames. */ 26922904823SRichard Cochran __skb_queue_head_init(&received); 27022904823SRichard Cochran spin_lock_irqsave(&rxq->lock, flags); 27122904823SRichard Cochran skb_queue_splice_tail_init(rxq, &received); 27222904823SRichard Cochran spin_unlock_irqrestore(&rxq->lock, flags); 27322904823SRichard Cochran 274c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 275c6fe0ad2SBrandon Streiff err = mv88e6xxx_port_ptp_read(chip, ps->port_id, 276c6fe0ad2SBrandon Streiff reg, buf, ARRAY_SIZE(buf)); 277c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 278c6fe0ad2SBrandon Streiff if (err) 279c6fe0ad2SBrandon Streiff pr_err("failed to get the receive time stamp\n"); 280c6fe0ad2SBrandon Streiff 281c6fe0ad2SBrandon Streiff status = buf[0]; 282c6fe0ad2SBrandon Streiff timelo = buf[1]; 283c6fe0ad2SBrandon Streiff timehi = buf[2]; 284c6fe0ad2SBrandon Streiff seq_id = buf[3]; 285c6fe0ad2SBrandon Streiff 286c6fe0ad2SBrandon Streiff if (status & MV88E6XXX_PTP_TS_VALID) { 287c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 288c6fe0ad2SBrandon Streiff err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0); 289c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 290c6fe0ad2SBrandon Streiff if (err) 291c6fe0ad2SBrandon Streiff pr_err("failed to clear the receive status\n"); 292c6fe0ad2SBrandon Streiff } 293c6fe0ad2SBrandon Streiff /* Since the device can only handle one time stamp at a time, 294c6fe0ad2SBrandon Streiff * we purge any extra frames from the queue. 295c6fe0ad2SBrandon Streiff */ 29622904823SRichard Cochran for ( ; skb; skb = __skb_dequeue(&received)) { 297c6fe0ad2SBrandon Streiff if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) { 298b2d12101SColin Ian King ns = timehi << 16 | timelo; 299c6fe0ad2SBrandon Streiff 300c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 301c6fe0ad2SBrandon Streiff ns = timecounter_cyc2time(&chip->tstamp_tc, ns); 302c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 303c6fe0ad2SBrandon Streiff shwt = skb_hwtstamps(skb); 304c6fe0ad2SBrandon Streiff memset(shwt, 0, sizeof(*shwt)); 305c6fe0ad2SBrandon Streiff shwt->hwtstamp = ns_to_ktime(ns); 306c6fe0ad2SBrandon Streiff status &= ~MV88E6XXX_PTP_TS_VALID; 307c6fe0ad2SBrandon Streiff } 308c6fe0ad2SBrandon Streiff netif_rx_ni(skb); 309c6fe0ad2SBrandon Streiff } 310c6fe0ad2SBrandon Streiff } 311c6fe0ad2SBrandon Streiff 312c6fe0ad2SBrandon Streiff static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip, 313c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps) 314c6fe0ad2SBrandon Streiff { 315ffc705deSAndrew Lunn const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 316c6fe0ad2SBrandon Streiff struct sk_buff *skb; 317c6fe0ad2SBrandon Streiff 318c6fe0ad2SBrandon Streiff skb = skb_dequeue(&ps->rx_queue); 319c6fe0ad2SBrandon Streiff 320c6fe0ad2SBrandon Streiff if (skb) 321ffc705deSAndrew Lunn mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg, 322c6fe0ad2SBrandon Streiff &ps->rx_queue); 323c6fe0ad2SBrandon Streiff 324c6fe0ad2SBrandon Streiff skb = skb_dequeue(&ps->rx_queue2); 325c6fe0ad2SBrandon Streiff if (skb) 326ffc705deSAndrew Lunn mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg, 327c6fe0ad2SBrandon Streiff &ps->rx_queue2); 328c6fe0ad2SBrandon Streiff } 329c6fe0ad2SBrandon Streiff 33028fba67fSKurt Kanzenbach static int is_pdelay_resp(const struct ptp_header *hdr) 331c6fe0ad2SBrandon Streiff { 33228fba67fSKurt Kanzenbach return (hdr->tsmt & 0xf) == 3; 333c6fe0ad2SBrandon Streiff } 334c6fe0ad2SBrandon Streiff 335c6fe0ad2SBrandon Streiff bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, 336c6fe0ad2SBrandon Streiff struct sk_buff *skb, unsigned int type) 337c6fe0ad2SBrandon Streiff { 338c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps; 339c6fe0ad2SBrandon Streiff struct mv88e6xxx_chip *chip; 34028fba67fSKurt Kanzenbach struct ptp_header *hdr; 341c6fe0ad2SBrandon Streiff 342c6fe0ad2SBrandon Streiff chip = ds->priv; 343c6fe0ad2SBrandon Streiff ps = &chip->port_hwtstamp[port]; 344c6fe0ad2SBrandon Streiff 345c6fe0ad2SBrandon Streiff if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) 346c6fe0ad2SBrandon Streiff return false; 347c6fe0ad2SBrandon Streiff 348c6fe0ad2SBrandon Streiff hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); 349c6fe0ad2SBrandon Streiff if (!hdr) 350c6fe0ad2SBrandon Streiff return false; 351c6fe0ad2SBrandon Streiff 352c6fe0ad2SBrandon Streiff SKB_PTP_TYPE(skb) = type; 353c6fe0ad2SBrandon Streiff 354c6fe0ad2SBrandon Streiff if (is_pdelay_resp(hdr)) 355c6fe0ad2SBrandon Streiff skb_queue_tail(&ps->rx_queue2, skb); 356c6fe0ad2SBrandon Streiff else 357c6fe0ad2SBrandon Streiff skb_queue_tail(&ps->rx_queue, skb); 358c6fe0ad2SBrandon Streiff 359c6fe0ad2SBrandon Streiff ptp_schedule_worker(chip->ptp_clock, 0); 360c6fe0ad2SBrandon Streiff 361c6fe0ad2SBrandon Streiff return true; 362c6fe0ad2SBrandon Streiff } 363c6fe0ad2SBrandon Streiff 364c6fe0ad2SBrandon Streiff static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip, 365c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps) 366c6fe0ad2SBrandon Streiff { 367ffc705deSAndrew Lunn const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 368c6fe0ad2SBrandon Streiff struct skb_shared_hwtstamps shhwtstamps; 369c6fe0ad2SBrandon Streiff u16 departure_block[4], status; 370c6fe0ad2SBrandon Streiff struct sk_buff *tmp_skb; 371c6fe0ad2SBrandon Streiff u32 time_raw; 372c6fe0ad2SBrandon Streiff int err; 373c6fe0ad2SBrandon Streiff u64 ns; 374c6fe0ad2SBrandon Streiff 375c6fe0ad2SBrandon Streiff if (!ps->tx_skb) 376c6fe0ad2SBrandon Streiff return 0; 377c6fe0ad2SBrandon Streiff 378c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 379c6fe0ad2SBrandon Streiff err = mv88e6xxx_port_ptp_read(chip, ps->port_id, 380ffc705deSAndrew Lunn ptp_ops->dep_sts_reg, 381c6fe0ad2SBrandon Streiff departure_block, 382c6fe0ad2SBrandon Streiff ARRAY_SIZE(departure_block)); 383c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 384c6fe0ad2SBrandon Streiff 385c6fe0ad2SBrandon Streiff if (err) 386c6fe0ad2SBrandon Streiff goto free_and_clear_skb; 387c6fe0ad2SBrandon Streiff 388c6fe0ad2SBrandon Streiff if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) { 389c6fe0ad2SBrandon Streiff if (time_is_before_jiffies(ps->tx_tstamp_start + 390c6fe0ad2SBrandon Streiff TX_TSTAMP_TIMEOUT)) { 391c6fe0ad2SBrandon Streiff dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n", 392c6fe0ad2SBrandon Streiff ps->port_id); 393c6fe0ad2SBrandon Streiff goto free_and_clear_skb; 394c6fe0ad2SBrandon Streiff } 395c6fe0ad2SBrandon Streiff /* The timestamp should be available quickly, while getting it 396c6fe0ad2SBrandon Streiff * is high priority and time bounded to only 10ms. A poll is 397c6fe0ad2SBrandon Streiff * warranted so restart the work. 398c6fe0ad2SBrandon Streiff */ 399c6fe0ad2SBrandon Streiff return 1; 400c6fe0ad2SBrandon Streiff } 401c6fe0ad2SBrandon Streiff 402c6fe0ad2SBrandon Streiff /* We have the timestamp; go ahead and clear valid now */ 403c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 404ffc705deSAndrew Lunn mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0); 405c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 406c6fe0ad2SBrandon Streiff 407c6fe0ad2SBrandon Streiff status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK; 408c6fe0ad2SBrandon Streiff if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) { 409c6fe0ad2SBrandon Streiff dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id); 410c6fe0ad2SBrandon Streiff goto free_and_clear_skb; 411c6fe0ad2SBrandon Streiff } 412c6fe0ad2SBrandon Streiff 413c6fe0ad2SBrandon Streiff if (departure_block[3] != ps->tx_seq_id) { 414c6fe0ad2SBrandon Streiff dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id); 415c6fe0ad2SBrandon Streiff goto free_and_clear_skb; 416c6fe0ad2SBrandon Streiff } 417c6fe0ad2SBrandon Streiff 418c6fe0ad2SBrandon Streiff memset(&shhwtstamps, 0, sizeof(shhwtstamps)); 419c6fe0ad2SBrandon Streiff time_raw = ((u32)departure_block[2] << 16) | departure_block[1]; 420c9acece0SRasmus Villemoes mv88e6xxx_reg_lock(chip); 421c6fe0ad2SBrandon Streiff ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw); 422c9acece0SRasmus Villemoes mv88e6xxx_reg_unlock(chip); 423c6fe0ad2SBrandon Streiff shhwtstamps.hwtstamp = ns_to_ktime(ns); 424c6fe0ad2SBrandon Streiff 425c6fe0ad2SBrandon Streiff dev_dbg(chip->dev, 426c6fe0ad2SBrandon Streiff "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n", 427c6fe0ad2SBrandon Streiff ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp), 428c6fe0ad2SBrandon Streiff departure_block[0], ps->tx_seq_id, departure_block[3]); 429c6fe0ad2SBrandon Streiff 430c6fe0ad2SBrandon Streiff /* skb_complete_tx_timestamp() will free up the client to make 431c6fe0ad2SBrandon Streiff * another timestamp-able transmit. We have to be ready for it 432c6fe0ad2SBrandon Streiff * -- by clearing the ps->tx_skb "flag" -- beforehand. 433c6fe0ad2SBrandon Streiff */ 434c6fe0ad2SBrandon Streiff 435c6fe0ad2SBrandon Streiff tmp_skb = ps->tx_skb; 436c6fe0ad2SBrandon Streiff ps->tx_skb = NULL; 437c6fe0ad2SBrandon Streiff clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 438c6fe0ad2SBrandon Streiff skb_complete_tx_timestamp(tmp_skb, &shhwtstamps); 439c6fe0ad2SBrandon Streiff 440c6fe0ad2SBrandon Streiff return 0; 441c6fe0ad2SBrandon Streiff 442c6fe0ad2SBrandon Streiff free_and_clear_skb: 443c6fe0ad2SBrandon Streiff dev_kfree_skb_any(ps->tx_skb); 444c6fe0ad2SBrandon Streiff ps->tx_skb = NULL; 445c6fe0ad2SBrandon Streiff clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 446c6fe0ad2SBrandon Streiff 447c6fe0ad2SBrandon Streiff return 0; 448c6fe0ad2SBrandon Streiff } 449c6fe0ad2SBrandon Streiff 450c6fe0ad2SBrandon Streiff long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp) 451c6fe0ad2SBrandon Streiff { 452c6fe0ad2SBrandon Streiff struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 453c6fe0ad2SBrandon Streiff struct dsa_switch *ds = chip->ds; 454c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps; 455c6fe0ad2SBrandon Streiff int i, restart = 0; 456c6fe0ad2SBrandon Streiff 457c6fe0ad2SBrandon Streiff for (i = 0; i < ds->num_ports; i++) { 458c6fe0ad2SBrandon Streiff if (!dsa_is_user_port(ds, i)) 459c6fe0ad2SBrandon Streiff continue; 460c6fe0ad2SBrandon Streiff 461c6fe0ad2SBrandon Streiff ps = &chip->port_hwtstamp[i]; 462c6fe0ad2SBrandon Streiff if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) 463c6fe0ad2SBrandon Streiff restart |= mv88e6xxx_txtstamp_work(chip, ps); 464c6fe0ad2SBrandon Streiff 465c6fe0ad2SBrandon Streiff mv88e6xxx_rxtstamp_work(chip, ps); 466c6fe0ad2SBrandon Streiff } 467c6fe0ad2SBrandon Streiff 468c6fe0ad2SBrandon Streiff return restart ? 1 : -1; 469c6fe0ad2SBrandon Streiff } 470c6fe0ad2SBrandon Streiff 471c6fe0ad2SBrandon Streiff bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, 472c6fe0ad2SBrandon Streiff struct sk_buff *clone, unsigned int type) 473c6fe0ad2SBrandon Streiff { 474c6fe0ad2SBrandon Streiff struct mv88e6xxx_chip *chip = ds->priv; 475c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 47628fba67fSKurt Kanzenbach struct ptp_header *hdr; 477c6fe0ad2SBrandon Streiff 478c6fe0ad2SBrandon Streiff if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP)) 479c6fe0ad2SBrandon Streiff return false; 480c6fe0ad2SBrandon Streiff 481c6fe0ad2SBrandon Streiff hdr = mv88e6xxx_should_tstamp(chip, port, clone, type); 482c6fe0ad2SBrandon Streiff if (!hdr) 483c6fe0ad2SBrandon Streiff return false; 484c6fe0ad2SBrandon Streiff 485c6fe0ad2SBrandon Streiff if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, 486c6fe0ad2SBrandon Streiff &ps->state)) 487c6fe0ad2SBrandon Streiff return false; 488c6fe0ad2SBrandon Streiff 489c6fe0ad2SBrandon Streiff ps->tx_skb = clone; 490c6fe0ad2SBrandon Streiff ps->tx_tstamp_start = jiffies; 49128fba67fSKurt Kanzenbach ps->tx_seq_id = be16_to_cpu(hdr->sequence_id); 492c6fe0ad2SBrandon Streiff 493c6fe0ad2SBrandon Streiff ptp_schedule_worker(chip->ptp_clock, 0); 494c6fe0ad2SBrandon Streiff return true; 495c6fe0ad2SBrandon Streiff } 496c6fe0ad2SBrandon Streiff 497e2294a8bSAndrew Lunn int mv88e6165_global_disable(struct mv88e6xxx_chip *chip) 498e2294a8bSAndrew Lunn { 499e2294a8bSAndrew Lunn u16 val; 500e2294a8bSAndrew Lunn int err; 501e2294a8bSAndrew Lunn 502e2294a8bSAndrew Lunn err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val); 503e2294a8bSAndrew Lunn if (err) 504e2294a8bSAndrew Lunn return err; 505e2294a8bSAndrew Lunn val |= MV88E6165_PTP_CFG_DISABLE_PTP; 506e2294a8bSAndrew Lunn 507e2294a8bSAndrew Lunn return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val); 508e2294a8bSAndrew Lunn } 509e2294a8bSAndrew Lunn 510e2294a8bSAndrew Lunn int mv88e6165_global_enable(struct mv88e6xxx_chip *chip) 511e2294a8bSAndrew Lunn { 512e2294a8bSAndrew Lunn u16 val; 513e2294a8bSAndrew Lunn int err; 514e2294a8bSAndrew Lunn 515e2294a8bSAndrew Lunn err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val); 516e2294a8bSAndrew Lunn if (err) 517e2294a8bSAndrew Lunn return err; 518e2294a8bSAndrew Lunn 519e2294a8bSAndrew Lunn val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK); 520e2294a8bSAndrew Lunn 521e2294a8bSAndrew Lunn return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val); 522e2294a8bSAndrew Lunn } 523e2294a8bSAndrew Lunn 524ffc705deSAndrew Lunn int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port) 525ffc705deSAndrew Lunn { 526ffc705deSAndrew Lunn return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, 527ffc705deSAndrew Lunn MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP); 528ffc705deSAndrew Lunn } 529ffc705deSAndrew Lunn 530ffc705deSAndrew Lunn int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port) 531ffc705deSAndrew Lunn { 532ffc705deSAndrew Lunn return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, 533ffc705deSAndrew Lunn MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH); 534ffc705deSAndrew Lunn } 535ffc705deSAndrew Lunn 536c6fe0ad2SBrandon Streiff static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port) 537c6fe0ad2SBrandon Streiff { 538ffc705deSAndrew Lunn const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 539c6fe0ad2SBrandon Streiff struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 540c6fe0ad2SBrandon Streiff 541c6fe0ad2SBrandon Streiff ps->port_id = port; 542c6fe0ad2SBrandon Streiff 543c6fe0ad2SBrandon Streiff skb_queue_head_init(&ps->rx_queue); 544c6fe0ad2SBrandon Streiff skb_queue_head_init(&ps->rx_queue2); 545c6fe0ad2SBrandon Streiff 546ffc705deSAndrew Lunn if (ptp_ops->port_disable) 547ffc705deSAndrew Lunn return ptp_ops->port_disable(chip, port); 548ffc705deSAndrew Lunn 549ffc705deSAndrew Lunn return 0; 550c6fe0ad2SBrandon Streiff } 551c6fe0ad2SBrandon Streiff 552c6fe0ad2SBrandon Streiff int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) 553c6fe0ad2SBrandon Streiff { 554e2294a8bSAndrew Lunn const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 555c6fe0ad2SBrandon Streiff int err; 556c6fe0ad2SBrandon Streiff int i; 557c6fe0ad2SBrandon Streiff 558c6fe0ad2SBrandon Streiff /* Disable timestamping on all ports. */ 559c6fe0ad2SBrandon Streiff for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) { 560c6fe0ad2SBrandon Streiff err = mv88e6xxx_hwtstamp_port_setup(chip, i); 561c6fe0ad2SBrandon Streiff if (err) 562c6fe0ad2SBrandon Streiff return err; 563c6fe0ad2SBrandon Streiff } 564c6fe0ad2SBrandon Streiff 565e2294a8bSAndrew Lunn /* Disable PTP globally */ 566e2294a8bSAndrew Lunn if (ptp_ops->global_disable) { 567e2294a8bSAndrew Lunn err = ptp_ops->global_disable(chip); 568e2294a8bSAndrew Lunn if (err) 569e2294a8bSAndrew Lunn return err; 570e2294a8bSAndrew Lunn } 571e2294a8bSAndrew Lunn 5722dbed245SAndrew Lunn /* Set the ethertype of L2 PTP messages */ 5732dbed245SAndrew Lunn err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588); 5742dbed245SAndrew Lunn if (err) 5752dbed245SAndrew Lunn return err; 5762dbed245SAndrew Lunn 577c6fe0ad2SBrandon Streiff /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to 578c6fe0ad2SBrandon Streiff * timestamp. This affects all ports that have timestamping enabled, 579c6fe0ad2SBrandon Streiff * but the timestamp config is per-port; thus we configure all events 580c6fe0ad2SBrandon Streiff * here and only support the HWTSTAMP_FILTER_*_EVENT filter types. 581c6fe0ad2SBrandon Streiff */ 582c6fe0ad2SBrandon Streiff err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE, 583c6fe0ad2SBrandon Streiff MV88E6XXX_PTP_MSGTYPE_ALL_EVENT); 584c6fe0ad2SBrandon Streiff if (err) 585c6fe0ad2SBrandon Streiff return err; 586c6fe0ad2SBrandon Streiff 587c6fe0ad2SBrandon Streiff /* Use ARRIVAL1 for peer delay response messages. */ 588c6fe0ad2SBrandon Streiff err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR, 589c6fe0ad2SBrandon Streiff MV88E6XXX_PTP_MSGTYPE_PDLAY_RES); 590c6fe0ad2SBrandon Streiff if (err) 591c6fe0ad2SBrandon Streiff return err; 592c6fe0ad2SBrandon Streiff 593a2e47134SBrandon Streiff /* 88E6341 devices default to timestamping at the PHY, but this has 594a2e47134SBrandon Streiff * a hardware issue that results in unreliable timestamps. Force 595a2e47134SBrandon Streiff * these devices to timestamp at the MAC. 596a2e47134SBrandon Streiff */ 597a2e47134SBrandon Streiff if (chip->info->family == MV88E6XXX_FAMILY_6341) { 598a2e47134SBrandon Streiff u16 val = MV88E6341_PTP_CFG_UPDATE | 599a2e47134SBrandon Streiff MV88E6341_PTP_CFG_MODE_IDX | 600a2e47134SBrandon Streiff MV88E6341_PTP_CFG_MODE_TS_AT_MAC; 601a2e47134SBrandon Streiff err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val); 602a2e47134SBrandon Streiff if (err) 603a2e47134SBrandon Streiff return err; 604a2e47134SBrandon Streiff } 605a2e47134SBrandon Streiff 606c6fe0ad2SBrandon Streiff return 0; 607c6fe0ad2SBrandon Streiff } 608c6fe0ad2SBrandon Streiff 609c6fe0ad2SBrandon Streiff void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) 610c6fe0ad2SBrandon Streiff { 611c6fe0ad2SBrandon Streiff } 612