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