drivers/net/dsa/mv88e6xxx/hwtstamp.c

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