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