946 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (C) 2022 Meta Platforms Inc.4 * Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com>5 */6 7#include <linux/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 0x081020#define SLICE_TX_EN BIT(0)21#define SLICE_RX_EN BIT(8)22#define TX_EVENT_MODE 0x081123#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 0x081927#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 032#define MODE_EVT_SHIFT_DELAY_REQ 233#define MODE_EVT_SHIFT_PDELAY_REQ 434#define MODE_EVT_SHIFT_PDELAY_RESP 635 36#define MODE_SEL_SHIFT_PORT 037#define MODE_SEL_SHIFT_CPU 838 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 0x082147#define TX_TS_CAP_EN BIT(0)48#define RX_TS_CAPTURE 0x082249#define RX_TS_CAP_EN BIT(0)50 51#define TIME_CODE_0 0x085452#define TIME_CODE_1 0x085553#define TIME_CODE_2 0x085654#define TIME_CODE_3 0x085755#define TIME_CODE_4 0x085856 57#define DPLL_SELECT 0x085b58#define DPLL_HB_MODE2 BIT(6)59 60#define SHADOW_CTRL 0x085c61#define SHADOW_LOAD 0x085d62#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 0x085e67#define INTR_STATUS 0x085f68#define INTC_FSYNC BIT(0)69#define INTC_SOP BIT(1)70 71#define NCO_FREQ_LSB 0x087372#define NCO_FREQ_MSB 0x087473 74#define NCO_TIME_0 0x087575#define NCO_TIME_1 0x087676#define NCO_TIME_2_CTRL 0x087777#define FREQ_MDIO_SEL BIT(14)78 79#define SYNC_OUT_0 0x087880#define SYNC_OUT_1 0x087981#define SYNC_OUT_2 0x087a82 83#define SYNC_IN_DIVIDER 0x087b84 85#define SYNOUT_TS_0 0x087c86#define SYNOUT_TS_1 0x087d87#define SYNOUT_TS_2 0x087e88 89#define NSE_CTRL 0x087f90#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 0x0885101#define TS_READ_START BIT(0)102#define TS_READ_END BIT(1)103 104#define HB_REG_0 0x0886105#define HB_REG_1 0x0887106#define HB_REG_2 0x0888107#define HB_REG_3 0x08ec108#define HB_REG_4 0x08ed109#define HB_STAT_CTRL 0x088e110#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 0x0889115#define TS_REG_1 0x088a116#define TS_REG_2 0x088b117#define TS_REG_3 0x08c4118 119#define TS_INFO_0 0x088c120#define TS_INFO_1 0x088d121 122#define TIMECODE_CTRL 0x08c3123#define TX_TIMECODE_SEL GENMASK(7, 0)124#define RX_TIMECODE_SEL GENMASK(15, 8)125 126#define TIME_SYNC 0x0ff5127#define TIME_SYNC_EN BIT(0)128 129struct 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 144struct bcm_ptp_skb_cb {145 unsigned long timeout;146 u16 seq_id;147 u8 msgtype;148 bool discard;149};150 151struct 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 167static 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 172static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info)173{174 return container_of(info, struct bcm_ptp_private, ptp_info);175}176 177static 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 196static 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 205static 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 211static 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 217static 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 248static 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 262static 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 295static 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 308static 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 324static 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 is340 * adj = scaled_ppm * 8*2^28 / (10^6 * 2^16)341 * which simplifies to:342 * adj = scaled_ppm * 2^9 / 5^6343 */344static 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 385static 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 418static 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 459static 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 503static 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 520static 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 536static 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 584static 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 else606 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 632static 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 660out:661 mutex_unlock(&priv->mutex);662 schedule_delayed_work(&priv->pin_work, HZ / 4);663}664 665static 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 688static 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 715static 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 729static 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 745static 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 780out:781 kfree_skb(skb);782}783 784static 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 else835 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 843static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts,844 struct kernel_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 865void bcm_ptp_stop(struct bcm_ptp_private *priv)866{867 ptp_cancel_worker_sync(priv->ptp_clock);868 bcm_ptp_cancel_func(priv);869}870EXPORT_SYMBOL_GPL(bcm_ptp_stop);871 872void 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}891EXPORT_SYMBOL_GPL(bcm_ptp_config_init);892 893static 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 908struct 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 /* Timestamp selected by default to keep legacy API */935 phydev->default_timestamp = true;936 937 priv->phydev = phydev;938 bcm_ptp_init(priv);939 940 return priv;941}942EXPORT_SYMBOL_GPL(bcm_ptp_probe);943 944MODULE_LICENSE("GPL");945MODULE_DESCRIPTION("Broadcom PHY PTP driver");946