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