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