drivers/net/phy/bcm-phy-ptp.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2022 Meta Platforms Inc.
   4  * Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com>
   5  */
   6
   7 #include <asm/unaligned.h>
   8 #include <linux/mii.h>
   9 #include <linux/phy.h>
  10 #include <linux/ptp_classify.h>
  11 #include <linux/ptp_clock_kernel.h>
  12 #include <linux/net_tstamp.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/workqueue.h>
  15
  16 #include "bcm-phy-lib.h"
  17
  18 /* IEEE 1588 Expansion registers */
  19 #define SLICE_CTRL              0x0810
  20 #define  SLICE_TX_EN                    BIT(0)
  21 #define  SLICE_RX_EN                    BIT(8)
  22 #define TX_EVENT_MODE           0x0811
  23 #define  MODE_TX_UPDATE_CF              BIT(0)
  24 #define  MODE_TX_REPLACE_TS_CF          BIT(1)
  25 #define  MODE_TX_REPLACE_TS             GENMASK(1, 0)
  26 #define RX_EVENT_MODE           0x0819
  27 #define  MODE_RX_UPDATE_CF              BIT(0)
  28 #define  MODE_RX_INSERT_TS_48           BIT(1)
  29 #define  MODE_RX_INSERT_TS_64           GENMASK(1, 0)
  30
  31 #define MODE_EVT_SHIFT_SYNC             0
  32 #define MODE_EVT_SHIFT_DELAY_REQ        2
  33 #define MODE_EVT_SHIFT_PDELAY_REQ       4
  34 #define MODE_EVT_SHIFT_PDELAY_RESP      6
  35
  36 #define MODE_SEL_SHIFT_PORT             0
  37 #define MODE_SEL_SHIFT_CPU              8
  38
  39 #define RX_MODE_SEL(sel, evt, act) \
  40         (((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
  41
  42 #define TX_MODE_SEL(sel, evt, act) \
  43         (((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
  44
  45 /* needs global TS capture first */
  46 #define TX_TS_CAPTURE           0x0821
  47 #define  TX_TS_CAP_EN                   BIT(0)
  48 #define RX_TS_CAPTURE           0x0822
  49 #define  RX_TS_CAP_EN                   BIT(0)
  50
  51 #define TIME_CODE_0             0x0854
  52 #define TIME_CODE_1             0x0855
  53 #define TIME_CODE_2             0x0856
  54 #define TIME_CODE_3             0x0857
  55 #define TIME_CODE_4             0x0858
  56
  57 #define DPLL_SELECT             0x085b
  58 #define  DPLL_HB_MODE2                  BIT(6)
  59
  60 #define SHADOW_CTRL             0x085c
  61 #define SHADOW_LOAD             0x085d
  62 #define  TIME_CODE_LOAD                 BIT(10)
  63 #define  SYNC_OUT_LOAD                  BIT(9)
  64 #define  NCO_TIME_LOAD                  BIT(7)
  65 #define  FREQ_LOAD                      BIT(6)
  66 #define INTR_MASK               0x085e
  67 #define INTR_STATUS             0x085f
  68 #define  INTC_FSYNC                     BIT(0)
  69 #define  INTC_SOP                       BIT(1)
  70
  71 #define NCO_FREQ_LSB            0x0873
  72 #define NCO_FREQ_MSB            0x0874
  73
  74 #define NCO_TIME_0              0x0875
  75 #define NCO_TIME_1              0x0876
  76 #define NCO_TIME_2_CTRL         0x0877
  77 #define  FREQ_MDIO_SEL                  BIT(14)
  78
  79 #define SYNC_OUT_0              0x0878
  80 #define SYNC_OUT_1              0x0879
  81 #define SYNC_OUT_2              0x087a
  82
  83 #define SYNC_IN_DIVIDER         0x087b
  84
  85 #define SYNOUT_TS_0             0x087c
  86 #define SYNOUT_TS_1             0x087d
  87 #define SYNOUT_TS_2             0x087e
  88
  89 #define NSE_CTRL                0x087f
  90 #define  NSE_GMODE_EN                   GENMASK(15, 14)
  91 #define  NSE_CAPTURE_EN                 BIT(13)
  92 #define  NSE_INIT                       BIT(12)
  93 #define  NSE_CPU_FRAMESYNC              BIT(5)
  94 #define  NSE_SYNC1_FRAMESYNC            BIT(3)
  95 #define  NSE_FRAMESYNC_MASK             GENMASK(5, 2)
  96 #define  NSE_PEROUT_EN                  BIT(1)
  97 #define  NSE_ONESHOT_EN                 BIT(0)
  98 #define  NSE_SYNC_OUT_MASK              GENMASK(1, 0)
  99
 100 #define TS_READ_CTRL            0x0885
 101 #define  TS_READ_START                  BIT(0)
 102 #define  TS_READ_END                    BIT(1)
 103
 104 #define HB_REG_0                0x0886
 105 #define HB_REG_1                0x0887
 106 #define HB_REG_2                0x0888
 107 #define HB_REG_3                0x08ec
 108 #define HB_REG_4                0x08ed
 109 #define HB_STAT_CTRL            0x088e
 110 #define  HB_READ_START                  BIT(10)
 111 #define  HB_READ_END                    BIT(11)
 112 #define  HB_READ_MASK                   GENMASK(11, 10)
 113
 114 #define TS_REG_0                0x0889
 115 #define TS_REG_1                0x088a
 116 #define TS_REG_2                0x088b
 117 #define TS_REG_3                0x08c4
 118
 119 #define TS_INFO_0               0x088c
 120 #define TS_INFO_1               0x088d
 121
 122 #define TIMECODE_CTRL           0x08c3
 123 #define  TX_TIMECODE_SEL                GENMASK(7, 0)
 124 #define  RX_TIMECODE_SEL                GENMASK(15, 8)
 125
 126 #define TIME_SYNC               0x0ff5
 127 #define  TIME_SYNC_EN                   BIT(0)
 128
 129 struct bcm_ptp_private {
 130         struct phy_device *phydev;
 131         struct mii_timestamper mii_ts;
 132         struct ptp_clock *ptp_clock;
 133         struct ptp_clock_info ptp_info;
 134         struct ptp_pin_desc pin;
 135         struct mutex mutex;
 136         struct sk_buff_head tx_queue;
 137         int tx_type;
 138         bool hwts_rx;
 139         u16 nse_ctrl;
 140         bool pin_active;
 141         struct delayed_work pin_work;
 142 };
 143
 144 struct bcm_ptp_skb_cb {
 145         unsigned long timeout;
 146         u16 seq_id;
 147         u8 msgtype;
 148         bool discard;
 149 };
 150
 151 struct bcm_ptp_capture {
 152         ktime_t hwtstamp;
 153         u16 seq_id;
 154         u8 msgtype;
 155         bool tx_dir;
 156 };
 157
 158 #define BCM_SKB_CB(skb)         ((struct bcm_ptp_skb_cb *)(skb)->cb)
 159 #define SKB_TS_TIMEOUT          10                      /* jiffies */
 160
 161 #define BCM_MAX_PULSE_8NS       ((1U << 9) - 1)
 162 #define BCM_MAX_PERIOD_8NS      ((1U << 30) - 1)
 163
 164 #define BRCM_PHY_MODEL(phydev) \
 165         ((phydev)->drv->phy_id & (phydev)->drv->phy_id_mask)
 166
 167 static struct bcm_ptp_private *mii2priv(struct mii_timestamper *mii_ts)
 168 {
 169         return container_of(mii_ts, struct bcm_ptp_private, mii_ts);
 170 }
 171
 172 static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info)
 173 {
 174         return container_of(info, struct bcm_ptp_private, ptp_info);
 175 }
 176
 177 static void bcm_ptp_get_framesync_ts(struct phy_device *phydev,
 178                                      struct timespec64 *ts)
 179 {
 180         u16 hb[4];
 181
 182         bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_START);
 183
 184         hb[0] = bcm_phy_read_exp(phydev, HB_REG_0);
 185         hb[1] = bcm_phy_read_exp(phydev, HB_REG_1);
 186         hb[2] = bcm_phy_read_exp(phydev, HB_REG_2);
 187         hb[3] = bcm_phy_read_exp(phydev, HB_REG_3);
 188
 189         bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_END);
 190         bcm_phy_write_exp(phydev, HB_STAT_CTRL, 0);
 191
 192         ts->tv_sec = (hb[3] << 16) | hb[2];
 193         ts->tv_nsec = (hb[1] << 16) | hb[0];
 194 }
 195
 196 static u16 bcm_ptp_framesync_disable(struct phy_device *phydev, u16 orig_ctrl)
 197 {
 198         u16 ctrl = orig_ctrl & ~(NSE_FRAMESYNC_MASK | NSE_CAPTURE_EN);
 199
 200         bcm_phy_write_exp(phydev, NSE_CTRL, ctrl);
 201
 202         return ctrl;
 203 }
 204
 205 static void bcm_ptp_framesync_restore(struct phy_device *phydev, u16 orig_ctrl)
 206 {
 207         if (orig_ctrl & NSE_FRAMESYNC_MASK)
 208                 bcm_phy_write_exp(phydev, NSE_CTRL, orig_ctrl);
 209 }
 210
 211 static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl)
 212 {
 213         /* trigger framesync - must have 0->1 transition. */
 214         bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC);
 215 }
 216
 217 static int bcm_ptp_framesync_ts(struct phy_device *phydev,
 218                                 struct ptp_system_timestamp *sts,
 219                                 struct timespec64 *ts,
 220                                 u16 orig_ctrl)
 221 {
 222         u16 ctrl, reg;
 223         int i;
 224
 225         ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl);
 226
 227         ptp_read_system_prets(sts);
 228
 229         /* trigger framesync + capture */
 230         bcm_ptp_framesync(phydev, ctrl | NSE_CAPTURE_EN);
 231
 232         ptp_read_system_postts(sts);
 233
 234         /* poll for FSYNC interrupt from TS capture */
 235         for (i = 0; i < 10; i++) {
 236                 reg = bcm_phy_read_exp(phydev, INTR_STATUS);
 237                 if (reg & INTC_FSYNC) {
 238                         bcm_ptp_get_framesync_ts(phydev, ts);
 239                         break;
 240                 }
 241         }
 242
 243         bcm_ptp_framesync_restore(phydev, orig_ctrl);
 244
 245         return reg & INTC_FSYNC ? 0 : -ETIMEDOUT;
 246 }
 247
 248 static int bcm_ptp_gettimex(struct ptp_clock_info *info,
 249                             struct timespec64 *ts,
 250                             struct ptp_system_timestamp *sts)
 251 {
 252         struct bcm_ptp_private *priv = ptp2priv(info);
 253         int err;
 254
 255         mutex_lock(&priv->mutex);
 256         err = bcm_ptp_framesync_ts(priv->phydev, sts, ts, priv->nse_ctrl);
 257         mutex_unlock(&priv->mutex);
 258
 259         return err;
 260 }
 261
 262 static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv,
 263                                   const struct timespec64 *ts)
 264 {
 265         struct phy_device *phydev = priv->phydev;
 266         u16 ctrl;
 267         u64 ns;
 268
 269         ctrl = bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
 270
 271         /* set up time code */
 272         bcm_phy_write_exp(phydev, TIME_CODE_0, ts->tv_nsec);
 273         bcm_phy_write_exp(phydev, TIME_CODE_1, ts->tv_nsec >> 16);
 274         bcm_phy_write_exp(phydev, TIME_CODE_2, ts->tv_sec);
 275         bcm_phy_write_exp(phydev, TIME_CODE_3, ts->tv_sec >> 16);
 276         bcm_phy_write_exp(phydev, TIME_CODE_4, ts->tv_sec >> 32);
 277
 278         /* set NCO counter to match */
 279         ns = timespec64_to_ns(ts);
 280         bcm_phy_write_exp(phydev, NCO_TIME_0, ns >> 4);
 281         bcm_phy_write_exp(phydev, NCO_TIME_1, ns >> 20);
 282         bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, (ns >> 36) & 0xfff);
 283
 284         /* set up load on next frame sync (auto-clears due to NSE_INIT) */
 285         bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD);
 286
 287         /* must have NSE_INIT in order to write time code */
 288         bcm_ptp_framesync(phydev, ctrl | NSE_INIT);
 289
 290         bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
 291
 292         return 0;
 293 }
 294
 295 static int bcm_ptp_settime(struct ptp_clock_info *info,
 296                            const struct timespec64 *ts)
 297 {
 298         struct bcm_ptp_private *priv = ptp2priv(info);
 299         int err;
 300
 301         mutex_lock(&priv->mutex);
 302         err = bcm_ptp_settime_locked(priv, ts);
 303         mutex_unlock(&priv->mutex);
 304
 305         return err;
 306 }
 307
 308 static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv,
 309                                   s64 delta_ns)
 310 {
 311         struct timespec64 ts;
 312         int err;
 313         s64 ns;
 314
 315         err = bcm_ptp_framesync_ts(priv->phydev, NULL, &ts, priv->nse_ctrl);
 316         if (!err) {
 317                 ns = timespec64_to_ns(&ts) + delta_ns;
 318                 ts = ns_to_timespec64(ns);
 319                 err = bcm_ptp_settime_locked(priv, &ts);
 320         }
 321         return err;
 322 }
 323
 324 static int bcm_ptp_adjtime(struct ptp_clock_info *info, s64 delta_ns)
 325 {
 326         struct bcm_ptp_private *priv = ptp2priv(info);
 327         int err;
 328
 329         mutex_lock(&priv->mutex);
 330         err = bcm_ptp_adjtime_locked(priv, delta_ns);
 331         mutex_unlock(&priv->mutex);
 332
 333         return err;
 334 }
 335
 336 /* A 125Mhz clock should adjust 8ns per pulse.
 337  * The frequency adjustment base is 0x8000 0000, or 8*2^28.
 338  *
 339  * Frequency adjustment is
 340  * adj = scaled_ppm * 8*2^28 / (10^6 * 2^16)
 341  *   which simplifies to:
 342  * adj = scaled_ppm * 2^9 / 5^6
 343  */
 344 static int bcm_ptp_adjfine(struct ptp_clock_info *info, long scaled_ppm)
 345 {
 346         struct bcm_ptp_private *priv = ptp2priv(info);
 347         int neg_adj = 0;
 348         u32 diff, freq;
 349         u16 ctrl;
 350         u64 adj;
 351
 352         if (scaled_ppm < 0) {
 353                 neg_adj = 1;
 354                 scaled_ppm = -scaled_ppm;
 355         }
 356
 357         adj = scaled_ppm << 9;
 358         diff = div_u64(adj, 15625);
 359         freq = (8 << 28) + (neg_adj ? -diff : diff);
 360
 361         mutex_lock(&priv->mutex);
 362
 363         ctrl = bcm_ptp_framesync_disable(priv->phydev, priv->nse_ctrl);
 364
 365         bcm_phy_write_exp(priv->phydev, NCO_FREQ_LSB, freq);
 366         bcm_phy_write_exp(priv->phydev, NCO_FREQ_MSB, freq >> 16);
 367
 368         bcm_phy_write_exp(priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL);
 369
 370         /* load on next framesync */
 371         bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, FREQ_LOAD);
 372
 373         bcm_ptp_framesync(priv->phydev, ctrl);
 374
 375         /* clear load */
 376         bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0);
 377
 378         bcm_ptp_framesync_restore(priv->phydev, priv->nse_ctrl);
 379
 380         mutex_unlock(&priv->mutex);
 381
 382         return 0;
 383 }
 384
 385 static bool bcm_ptp_rxtstamp(struct mii_timestamper *mii_ts,
 386                              struct sk_buff *skb, int type)
 387 {
 388         struct bcm_ptp_private *priv = mii2priv(mii_ts);
 389         struct skb_shared_hwtstamps *hwts;
 390         struct ptp_header *header;
 391         u32 sec, nsec;
 392         u8 *data;
 393         int off;
 394
 395         if (!priv->hwts_rx)
 396                 return false;
 397
 398         header = ptp_parse_header(skb, type);
 399         if (!header)
 400                 return false;
 401
 402         data = (u8 *)(header + 1);
 403         sec = get_unaligned_be32(data);
 404         nsec = get_unaligned_be32(data + 4);
 405
 406         hwts = skb_hwtstamps(skb);
 407         hwts->hwtstamp = ktime_set(sec, nsec);
 408
 409         off = data - skb->data + 8;
 410         if (off < skb->len) {
 411                 memmove(data, data + 8, skb->len - off);
 412                 __pskb_trim(skb, skb->len - 8);
 413         }
 414
 415         return false;
 416 }
 417
 418 static bool bcm_ptp_get_tstamp(struct bcm_ptp_private *priv,
 419                                struct bcm_ptp_capture *capts)
 420 {
 421         struct phy_device *phydev = priv->phydev;
 422         u16 ts[4], reg;
 423         u32 sec, nsec;
 424
 425         mutex_lock(&priv->mutex);
 426
 427         reg = bcm_phy_read_exp(phydev, INTR_STATUS);
 428         if ((reg & INTC_SOP) == 0) {
 429                 mutex_unlock(&priv->mutex);
 430                 return false;
 431         }
 432
 433         bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_START);
 434
 435         ts[0] = bcm_phy_read_exp(phydev, TS_REG_0);
 436         ts[1] = bcm_phy_read_exp(phydev, TS_REG_1);
 437         ts[2] = bcm_phy_read_exp(phydev, TS_REG_2);
 438         ts[3] = bcm_phy_read_exp(phydev, TS_REG_3);
 439
 440         /* not in be32 format for some reason */
 441         capts->seq_id = bcm_phy_read_exp(priv->phydev, TS_INFO_0);
 442
 443         reg = bcm_phy_read_exp(phydev, TS_INFO_1);
 444         capts->msgtype = reg >> 12;
 445         capts->tx_dir = !!(reg & BIT(11));
 446
 447         bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_END);
 448         bcm_phy_write_exp(phydev, TS_READ_CTRL, 0);
 449
 450         mutex_unlock(&priv->mutex);
 451
 452         sec = (ts[3] << 16) | ts[2];
 453         nsec = (ts[1] << 16) | ts[0];
 454         capts->hwtstamp = ktime_set(sec, nsec);
 455
 456         return true;
 457 }
 458
 459 static void bcm_ptp_match_tstamp(struct bcm_ptp_private *priv,
 460                                  struct bcm_ptp_capture *capts)
 461 {
 462         struct skb_shared_hwtstamps hwts;
 463         struct sk_buff *skb, *ts_skb;
 464         unsigned long flags;
 465         bool first = false;
 466
 467         ts_skb = NULL;
 468         spin_lock_irqsave(&priv->tx_queue.lock, flags);
 469         skb_queue_walk(&priv->tx_queue, skb) {
 470                 if (BCM_SKB_CB(skb)->seq_id == capts->seq_id &&
 471                     BCM_SKB_CB(skb)->msgtype == capts->msgtype) {
 472                         first = skb_queue_is_first(&priv->tx_queue, skb);
 473                         __skb_unlink(skb, &priv->tx_queue);
 474                         ts_skb = skb;
 475                         break;
 476                 }
 477         }
 478         spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
 479
 480         /* TX captures one-step packets, discard them if needed. */
 481         if (ts_skb) {
 482                 if (BCM_SKB_CB(ts_skb)->discard) {
 483                         kfree_skb(ts_skb);
 484                 } else {
 485                         memset(&hwts, 0, sizeof(hwts));
 486                         hwts.hwtstamp = capts->hwtstamp;
 487                         skb_complete_tx_timestamp(ts_skb, &hwts);
 488                 }
 489         }
 490
 491         /* not first match, try and expire entries */
 492         if (!first) {
 493                 while ((skb = skb_dequeue(&priv->tx_queue))) {
 494                         if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) {
 495                                 skb_queue_head(&priv->tx_queue, skb);
 496                                 break;
 497                         }
 498                         kfree_skb(skb);
 499                 }
 500         }
 501 }
 502
 503 static long bcm_ptp_do_aux_work(struct ptp_clock_info *info)
 504 {
 505         struct bcm_ptp_private *priv = ptp2priv(info);
 506         struct bcm_ptp_capture capts;
 507         bool reschedule = false;
 508
 509         while (!skb_queue_empty_lockless(&priv->tx_queue)) {
 510                 if (!bcm_ptp_get_tstamp(priv, &capts)) {
 511                         reschedule = true;
 512                         break;
 513                 }
 514                 bcm_ptp_match_tstamp(priv, &capts);
 515         }
 516
 517         return reschedule ? 1 : -1;
 518 }
 519
 520 static int bcm_ptp_cancel_func(struct bcm_ptp_private *priv)
 521 {
 522         if (!priv->pin_active)
 523                 return 0;
 524
 525         priv->pin_active = false;
 526
 527         priv->nse_ctrl &= ~(NSE_SYNC_OUT_MASK | NSE_SYNC1_FRAMESYNC |
 528                             NSE_CAPTURE_EN);
 529         bcm_phy_write_exp(priv->phydev, NSE_CTRL, priv->nse_ctrl);
 530
 531         cancel_delayed_work_sync(&priv->pin_work);
 532
 533         return 0;
 534 }
 535
 536 static void bcm_ptp_perout_work(struct work_struct *pin_work)
 537 {
 538         struct bcm_ptp_private *priv =
 539                 container_of(pin_work, struct bcm_ptp_private, pin_work.work);
 540         struct phy_device *phydev = priv->phydev;
 541         struct timespec64 ts;
 542         u64 ns, next;
 543         u16 ctrl;
 544
 545         mutex_lock(&priv->mutex);
 546
 547         /* no longer running */
 548         if (!priv->pin_active) {
 549                 mutex_unlock(&priv->mutex);
 550                 return;
 551         }
 552
 553         bcm_ptp_framesync_ts(phydev, NULL, &ts, priv->nse_ctrl);
 554
 555         /* this is 1PPS only */
 556         next = NSEC_PER_SEC - ts.tv_nsec;
 557         ts.tv_sec += next < NSEC_PER_MSEC ? 2 : 1;
 558         ts.tv_nsec = 0;
 559
 560         ns = timespec64_to_ns(&ts);
 561
 562         /* force 0->1 transition for ONESHOT */
 563         ctrl = bcm_ptp_framesync_disable(phydev,
 564                                          priv->nse_ctrl & ~NSE_ONESHOT_EN);
 565
 566         bcm_phy_write_exp(phydev, SYNOUT_TS_0, ns & 0xfff0);
 567         bcm_phy_write_exp(phydev, SYNOUT_TS_1, ns >> 16);
 568         bcm_phy_write_exp(phydev, SYNOUT_TS_2, ns >> 32);
 569
 570         /* load values on next framesync */
 571         bcm_phy_write_exp(phydev, SHADOW_LOAD, SYNC_OUT_LOAD);
 572
 573         bcm_ptp_framesync(phydev, ctrl | NSE_ONESHOT_EN | NSE_INIT);
 574
 575         priv->nse_ctrl |= NSE_ONESHOT_EN;
 576         bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
 577
 578         mutex_unlock(&priv->mutex);
 579
 580         next = next + NSEC_PER_MSEC;
 581         schedule_delayed_work(&priv->pin_work, nsecs_to_jiffies(next));
 582 }
 583
 584 static int bcm_ptp_perout_locked(struct bcm_ptp_private *priv,
 585                                  struct ptp_perout_request *req, int on)
 586 {
 587         struct phy_device *phydev = priv->phydev;
 588         u64 period, pulse;
 589         u16 val;
 590
 591         if (!on)
 592                 return bcm_ptp_cancel_func(priv);
 593
 594         /* 1PPS */
 595         if (req->period.sec != 1 || req->period.nsec != 0)
 596                 return -EINVAL;
 597
 598         period = BCM_MAX_PERIOD_8NS;    /* write nonzero value */
 599
 600         if (req->flags & PTP_PEROUT_PHASE)
 601                 return -EOPNOTSUPP;
 602
 603         if (req->flags & PTP_PEROUT_DUTY_CYCLE)
 604                 pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
 605         else
 606                 pulse = (u64)BCM_MAX_PULSE_8NS << 3;
 607
 608         /* convert to 8ns units */
 609         pulse >>= 3;
 610
 611         if (!pulse || pulse > period || pulse > BCM_MAX_PULSE_8NS)
 612                 return -EINVAL;
 613
 614         bcm_phy_write_exp(phydev, SYNC_OUT_0, period);
 615
 616         val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
 617         bcm_phy_write_exp(phydev, SYNC_OUT_1, val);
 618
 619         val = ((pulse >> 2) & 0x7f) | (pulse << 7);
 620         bcm_phy_write_exp(phydev, SYNC_OUT_2, val);
 621
 622         if (priv->pin_active)
 623                 cancel_delayed_work_sync(&priv->pin_work);
 624
 625         priv->pin_active = true;
 626         INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_perout_work);
 627         schedule_delayed_work(&priv->pin_work, 0);
 628
 629         return 0;
 630 }
 631
 632 static void bcm_ptp_extts_work(struct work_struct *pin_work)
 633 {
 634         struct bcm_ptp_private *priv =
 635                 container_of(pin_work, struct bcm_ptp_private, pin_work.work);
 636         struct phy_device *phydev = priv->phydev;
 637         struct ptp_clock_event event;
 638         struct timespec64 ts;
 639         u16 reg;
 640
 641         mutex_lock(&priv->mutex);
 642
 643         /* no longer running */
 644         if (!priv->pin_active) {
 645                 mutex_unlock(&priv->mutex);
 646                 return;
 647         }
 648
 649         reg = bcm_phy_read_exp(phydev, INTR_STATUS);
 650         if ((reg & INTC_FSYNC) == 0)
 651                 goto out;
 652
 653         bcm_ptp_get_framesync_ts(phydev, &ts);
 654
 655         event.index = 0;
 656         event.type = PTP_CLOCK_EXTTS;
 657         event.timestamp = timespec64_to_ns(&ts);
 658         ptp_clock_event(priv->ptp_clock, &event);
 659
 660 out:
 661         mutex_unlock(&priv->mutex);
 662         schedule_delayed_work(&priv->pin_work, HZ / 4);
 663 }
 664
 665 static int bcm_ptp_extts_locked(struct bcm_ptp_private *priv, int on)
 666 {
 667         struct phy_device *phydev = priv->phydev;
 668
 669         if (!on)
 670                 return bcm_ptp_cancel_func(priv);
 671
 672         if (priv->pin_active)
 673                 cancel_delayed_work_sync(&priv->pin_work);
 674
 675         bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
 676
 677         priv->nse_ctrl |= NSE_SYNC1_FRAMESYNC | NSE_CAPTURE_EN;
 678
 679         bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
 680
 681         priv->pin_active = true;
 682         INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_extts_work);
 683         schedule_delayed_work(&priv->pin_work, 0);
 684
 685         return 0;
 686 }
 687
 688 static int bcm_ptp_enable(struct ptp_clock_info *info,
 689                           struct ptp_clock_request *rq, int on)
 690 {
 691         struct bcm_ptp_private *priv = ptp2priv(info);
 692         int err = -EBUSY;
 693
 694         mutex_lock(&priv->mutex);
 695
 696         switch (rq->type) {
 697         case PTP_CLK_REQ_PEROUT:
 698                 if (priv->pin.func == PTP_PF_PEROUT)
 699                         err = bcm_ptp_perout_locked(priv, &rq->perout, on);
 700                 break;
 701         case PTP_CLK_REQ_EXTTS:
 702                 if (priv->pin.func == PTP_PF_EXTTS)
 703                         err = bcm_ptp_extts_locked(priv, on);
 704                 break;
 705         default:
 706                 err = -EOPNOTSUPP;
 707                 break;
 708         }
 709
 710         mutex_unlock(&priv->mutex);
 711
 712         return err;
 713 }
 714
 715 static int bcm_ptp_verify(struct ptp_clock_info *info, unsigned int pin,
 716                           enum ptp_pin_function func, unsigned int chan)
 717 {
 718         switch (func) {
 719         case PTP_PF_NONE:
 720         case PTP_PF_EXTTS:
 721         case PTP_PF_PEROUT:
 722                 break;
 723         default:
 724                 return -EOPNOTSUPP;
 725         }
 726         return 0;
 727 }
 728
 729 static const struct ptp_clock_info bcm_ptp_clock_info = {
 730         .owner          = THIS_MODULE,
 731         .name           = KBUILD_MODNAME,
 732         .max_adj        = 100000000,
 733         .gettimex64     = bcm_ptp_gettimex,
 734         .settime64      = bcm_ptp_settime,
 735         .adjtime        = bcm_ptp_adjtime,
 736         .adjfine        = bcm_ptp_adjfine,
 737         .enable         = bcm_ptp_enable,
 738         .verify         = bcm_ptp_verify,
 739         .do_aux_work    = bcm_ptp_do_aux_work,
 740         .n_pins         = 1,
 741         .n_per_out      = 1,
 742         .n_ext_ts       = 1,
 743 };
 744
 745 static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts,
 746                              struct sk_buff *skb, int type)
 747 {
 748         struct bcm_ptp_private *priv = mii2priv(mii_ts);
 749         struct ptp_header *hdr;
 750         bool discard = false;
 751         int msgtype;
 752
 753         hdr = ptp_parse_header(skb, type);
 754         if (!hdr)
 755                 goto out;
 756         msgtype = ptp_get_msgtype(hdr, type);
 757
 758         switch (priv->tx_type) {
 759         case HWTSTAMP_TX_ONESTEP_P2P:
 760                 if (msgtype == PTP_MSGTYPE_PDELAY_RESP)
 761                         discard = true;
 762                 fallthrough;
 763         case HWTSTAMP_TX_ONESTEP_SYNC:
 764                 if (msgtype == PTP_MSGTYPE_SYNC)
 765                         discard = true;
 766                 fallthrough;
 767         case HWTSTAMP_TX_ON:
 768                 BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT;
 769                 BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id);
 770                 BCM_SKB_CB(skb)->msgtype = msgtype;
 771                 BCM_SKB_CB(skb)->discard = discard;
 772                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 773                 skb_queue_tail(&priv->tx_queue, skb);
 774                 ptp_schedule_worker(priv->ptp_clock, 0);
 775                 return;
 776         default:
 777                 break;
 778         }
 779
 780 out:
 781         kfree_skb(skb);
 782 }
 783
 784 static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts,
 785                             struct kernel_hwtstamp_config *cfg,
 786                             struct netlink_ext_ack *extack)
 787 {
 788         struct bcm_ptp_private *priv = mii2priv(mii_ts);
 789         u16 mode, ctrl;
 790
 791         switch (cfg->rx_filter) {
 792         case HWTSTAMP_FILTER_NONE:
 793                 priv->hwts_rx = false;
 794                 break;
 795         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 796         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 797         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 798         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 799         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 800         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 801         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 802         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 803         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 804                 cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 805                 priv->hwts_rx = true;
 806                 break;
 807         default:
 808                 return -ERANGE;
 809         }
 810
 811         priv->tx_type = cfg->tx_type;
 812
 813         ctrl  = priv->hwts_rx ? SLICE_RX_EN : 0;
 814         ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0;
 815
 816         mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) |
 817                TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) |
 818                TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) |
 819                TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS);
 820
 821         bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode);
 822
 823         mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) |
 824                RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) |
 825                RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) |
 826                RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64);
 827
 828         bcm_phy_write_exp(priv->phydev, RX_EVENT_MODE, mode);
 829
 830         bcm_phy_write_exp(priv->phydev, SLICE_CTRL, ctrl);
 831
 832         if (ctrl & SLICE_TX_EN)
 833                 bcm_phy_write_exp(priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN);
 834         else
 835                 ptp_cancel_worker_sync(priv->ptp_clock);
 836
 837         /* purge existing data */
 838         skb_queue_purge(&priv->tx_queue);
 839
 840         return 0;
 841 }
 842
 843 static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts,
 844                            struct ethtool_ts_info *ts_info)
 845 {
 846         struct bcm_ptp_private *priv = mii2priv(mii_ts);
 847
 848         ts_info->phc_index = ptp_clock_index(priv->ptp_clock);
 849         ts_info->so_timestamping =
 850                 SOF_TIMESTAMPING_TX_HARDWARE |
 851                 SOF_TIMESTAMPING_RX_HARDWARE |
 852                 SOF_TIMESTAMPING_RAW_HARDWARE;
 853         ts_info->tx_types =
 854                 BIT(HWTSTAMP_TX_ON) |
 855                 BIT(HWTSTAMP_TX_OFF) |
 856                 BIT(HWTSTAMP_TX_ONESTEP_SYNC) |
 857                 BIT(HWTSTAMP_TX_ONESTEP_P2P);
 858         ts_info->rx_filters =
 859                 BIT(HWTSTAMP_FILTER_NONE) |
 860                 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
 861
 862         return 0;
 863 }
 864
 865 void bcm_ptp_stop(struct bcm_ptp_private *priv)
 866 {
 867         ptp_cancel_worker_sync(priv->ptp_clock);
 868         bcm_ptp_cancel_func(priv);
 869 }
 870 EXPORT_SYMBOL_GPL(bcm_ptp_stop);
 871
 872 void bcm_ptp_config_init(struct phy_device *phydev)
 873 {
 874         /* init network sync engine */
 875         bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT);
 876
 877         /* enable time sync (TX/RX SOP capture) */
 878         bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN);
 879
 880         /* use sec.nsec heartbeat capture */
 881         bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2);
 882
 883         /* use 64 bit timecode for TX */
 884         bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL);
 885
 886         /* always allow FREQ_LOAD on framesync */
 887         bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD);
 888
 889         bcm_phy_write_exp(phydev, SYNC_IN_DIVIDER, 1);
 890 }
 891 EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
 892
 893 static void bcm_ptp_init(struct bcm_ptp_private *priv)
 894 {
 895         priv->nse_ctrl = NSE_GMODE_EN;
 896
 897         mutex_init(&priv->mutex);
 898         skb_queue_head_init(&priv->tx_queue);
 899
 900         priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp;
 901         priv->mii_ts.txtstamp = bcm_ptp_txtstamp;
 902         priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp;
 903         priv->mii_ts.ts_info = bcm_ptp_ts_info;
 904
 905         priv->phydev->mii_ts = &priv->mii_ts;
 906 }
 907
 908 struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
 909 {
 910         struct bcm_ptp_private *priv;
 911         struct ptp_clock *clock;
 912
 913         switch (BRCM_PHY_MODEL(phydev)) {
 914         case PHY_ID_BCM54210E:
 915                 break;
 916         default:
 917                 return NULL;
 918         }
 919
 920         priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
 921         if (!priv)
 922                 return ERR_PTR(-ENOMEM);
 923
 924         priv->ptp_info = bcm_ptp_clock_info;
 925
 926         snprintf(priv->pin.name, sizeof(priv->pin.name), "SYNC_OUT");
 927         priv->ptp_info.pin_config = &priv->pin;
 928
 929         clock = ptp_clock_register(&priv->ptp_info, &phydev->mdio.dev);
 930         if (IS_ERR(clock))
 931                 return ERR_CAST(clock);
 932         priv->ptp_clock = clock;
 933
 934         priv->phydev = phydev;
 935         bcm_ptp_init(priv);
 936
 937         return priv;
 938 }
 939 EXPORT_SYMBOL_GPL(bcm_ptp_probe);
 940
 941 MODULE_LICENSE("GPL");
 942 MODULE_DESCRIPTION("Broadcom PHY PTP driver");