drivers/net/dsa/mv88e6xxx/hwtstamp.c

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