945 lines
23 KiB
C
945 lines
23 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
|
/*
|
|
* Copyright (C) 2022 Meta Platforms Inc.
|
|
* Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com>
|
|
*/
|
|
|
|
#include <asm/unaligned.h>
|
|
#include <linux/mii.h>
|
|
#include <linux/phy.h>
|
|
#include <linux/ptp_classify.h>
|
|
#include <linux/ptp_clock_kernel.h>
|
|
#include <linux/net_tstamp.h>
|
|
#include <linux/netdevice.h>
|
|
#include <linux/workqueue.h>
|
|
|
|
#include "bcm-phy-lib.h"
|
|
|
|
/* IEEE 1588 Expansion registers */
|
|
#define SLICE_CTRL 0x0810
|
|
#define SLICE_TX_EN BIT(0)
|
|
#define SLICE_RX_EN BIT(8)
|
|
#define TX_EVENT_MODE 0x0811
|
|
#define MODE_TX_UPDATE_CF BIT(0)
|
|
#define MODE_TX_REPLACE_TS_CF BIT(1)
|
|
#define MODE_TX_REPLACE_TS GENMASK(1, 0)
|
|
#define RX_EVENT_MODE 0x0819
|
|
#define MODE_RX_UPDATE_CF BIT(0)
|
|
#define MODE_RX_INSERT_TS_48 BIT(1)
|
|
#define MODE_RX_INSERT_TS_64 GENMASK(1, 0)
|
|
|
|
#define MODE_EVT_SHIFT_SYNC 0
|
|
#define MODE_EVT_SHIFT_DELAY_REQ 2
|
|
#define MODE_EVT_SHIFT_PDELAY_REQ 4
|
|
#define MODE_EVT_SHIFT_PDELAY_RESP 6
|
|
|
|
#define MODE_SEL_SHIFT_PORT 0
|
|
#define MODE_SEL_SHIFT_CPU 8
|
|
|
|
#define RX_MODE_SEL(sel, evt, act) \
|
|
(((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
|
|
|
|
#define TX_MODE_SEL(sel, evt, act) \
|
|
(((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
|
|
|
|
/* needs global TS capture first */
|
|
#define TX_TS_CAPTURE 0x0821
|
|
#define TX_TS_CAP_EN BIT(0)
|
|
#define RX_TS_CAPTURE 0x0822
|
|
#define RX_TS_CAP_EN BIT(0)
|
|
|
|
#define TIME_CODE_0 0x0854
|
|
#define TIME_CODE_1 0x0855
|
|
#define TIME_CODE_2 0x0856
|
|
#define TIME_CODE_3 0x0857
|
|
#define TIME_CODE_4 0x0858
|
|
|
|
#define DPLL_SELECT 0x085b
|
|
#define DPLL_HB_MODE2 BIT(6)
|
|
|
|
#define SHADOW_CTRL 0x085c
|
|
#define SHADOW_LOAD 0x085d
|
|
#define TIME_CODE_LOAD BIT(10)
|
|
#define SYNC_OUT_LOAD BIT(9)
|
|
#define NCO_TIME_LOAD BIT(7)
|
|
#define FREQ_LOAD BIT(6)
|
|
#define INTR_MASK 0x085e
|
|
#define INTR_STATUS 0x085f
|
|
#define INTC_FSYNC BIT(0)
|
|
#define INTC_SOP BIT(1)
|
|
|
|
#define NCO_FREQ_LSB 0x0873
|
|
#define NCO_FREQ_MSB 0x0874
|
|
|
|
#define NCO_TIME_0 0x0875
|
|
#define NCO_TIME_1 0x0876
|
|
#define NCO_TIME_2_CTRL 0x0877
|
|
#define FREQ_MDIO_SEL BIT(14)
|
|
|
|
#define SYNC_OUT_0 0x0878
|
|
#define SYNC_OUT_1 0x0879
|
|
#define SYNC_OUT_2 0x087a
|
|
|
|
#define SYNC_IN_DIVIDER 0x087b
|
|
|
|
#define SYNOUT_TS_0 0x087c
|
|
#define SYNOUT_TS_1 0x087d
|
|
#define SYNOUT_TS_2 0x087e
|
|
|
|
#define NSE_CTRL 0x087f
|
|
#define NSE_GMODE_EN GENMASK(15, 14)
|
|
#define NSE_CAPTURE_EN BIT(13)
|
|
#define NSE_INIT BIT(12)
|
|
#define NSE_CPU_FRAMESYNC BIT(5)
|
|
#define NSE_SYNC1_FRAMESYNC BIT(3)
|
|
#define NSE_FRAMESYNC_MASK GENMASK(5, 2)
|
|
#define NSE_PEROUT_EN BIT(1)
|
|
#define NSE_ONESHOT_EN BIT(0)
|
|
#define NSE_SYNC_OUT_MASK GENMASK(1, 0)
|
|
|
|
#define TS_READ_CTRL 0x0885
|
|
#define TS_READ_START BIT(0)
|
|
#define TS_READ_END BIT(1)
|
|
|
|
#define HB_REG_0 0x0886
|
|
#define HB_REG_1 0x0887
|
|
#define HB_REG_2 0x0888
|
|
#define HB_REG_3 0x08ec
|
|
#define HB_REG_4 0x08ed
|
|
#define HB_STAT_CTRL 0x088e
|
|
#define HB_READ_START BIT(10)
|
|
#define HB_READ_END BIT(11)
|
|
#define HB_READ_MASK GENMASK(11, 10)
|
|
|
|
#define TS_REG_0 0x0889
|
|
#define TS_REG_1 0x088a
|
|
#define TS_REG_2 0x088b
|
|
#define TS_REG_3 0x08c4
|
|
|
|
#define TS_INFO_0 0x088c
|
|
#define TS_INFO_1 0x088d
|
|
|
|
#define TIMECODE_CTRL 0x08c3
|
|
#define TX_TIMECODE_SEL GENMASK(7, 0)
|
|
#define RX_TIMECODE_SEL GENMASK(15, 8)
|
|
|
|
#define TIME_SYNC 0x0ff5
|
|
#define TIME_SYNC_EN BIT(0)
|
|
|
|
struct bcm_ptp_private {
|
|
struct phy_device *phydev;
|
|
struct mii_timestamper mii_ts;
|
|
struct ptp_clock *ptp_clock;
|
|
struct ptp_clock_info ptp_info;
|
|
struct ptp_pin_desc pin;
|
|
struct mutex mutex;
|
|
struct sk_buff_head tx_queue;
|
|
int tx_type;
|
|
bool hwts_rx;
|
|
u16 nse_ctrl;
|
|
bool pin_active;
|
|
struct delayed_work pin_work;
|
|
};
|
|
|
|
struct bcm_ptp_skb_cb {
|
|
unsigned long timeout;
|
|
u16 seq_id;
|
|
u8 msgtype;
|
|
bool discard;
|
|
};
|
|
|
|
struct bcm_ptp_capture {
|
|
ktime_t hwtstamp;
|
|
u16 seq_id;
|
|
u8 msgtype;
|
|
bool tx_dir;
|
|
};
|
|
|
|
#define BCM_SKB_CB(skb) ((struct bcm_ptp_skb_cb *)(skb)->cb)
|
|
#define SKB_TS_TIMEOUT 10 /* jiffies */
|
|
|
|
#define BCM_MAX_PULSE_8NS ((1U << 9) - 1)
|
|
#define BCM_MAX_PERIOD_8NS ((1U << 30) - 1)
|
|
|
|
#define BRCM_PHY_MODEL(phydev) \
|
|
((phydev)->drv->phy_id & (phydev)->drv->phy_id_mask)
|
|
|
|
static struct bcm_ptp_private *mii2priv(struct mii_timestamper *mii_ts)
|
|
{
|
|
return container_of(mii_ts, struct bcm_ptp_private, mii_ts);
|
|
}
|
|
|
|
static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info)
|
|
{
|
|
return container_of(info, struct bcm_ptp_private, ptp_info);
|
|
}
|
|
|
|
static void bcm_ptp_get_framesync_ts(struct phy_device *phydev,
|
|
struct timespec64 *ts)
|
|
{
|
|
u16 hb[4];
|
|
|
|
bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_START);
|
|
|
|
hb[0] = bcm_phy_read_exp(phydev, HB_REG_0);
|
|
hb[1] = bcm_phy_read_exp(phydev, HB_REG_1);
|
|
hb[2] = bcm_phy_read_exp(phydev, HB_REG_2);
|
|
hb[3] = bcm_phy_read_exp(phydev, HB_REG_3);
|
|
|
|
bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_END);
|
|
bcm_phy_write_exp(phydev, HB_STAT_CTRL, 0);
|
|
|
|
ts->tv_sec = (hb[3] << 16) | hb[2];
|
|
ts->tv_nsec = (hb[1] << 16) | hb[0];
|
|
}
|
|
|
|
static u16 bcm_ptp_framesync_disable(struct phy_device *phydev, u16 orig_ctrl)
|
|
{
|
|
u16 ctrl = orig_ctrl & ~(NSE_FRAMESYNC_MASK | NSE_CAPTURE_EN);
|
|
|
|
bcm_phy_write_exp(phydev, NSE_CTRL, ctrl);
|
|
|
|
return ctrl;
|
|
}
|
|
|
|
static void bcm_ptp_framesync_restore(struct phy_device *phydev, u16 orig_ctrl)
|
|
{
|
|
if (orig_ctrl & NSE_FRAMESYNC_MASK)
|
|
bcm_phy_write_exp(phydev, NSE_CTRL, orig_ctrl);
|
|
}
|
|
|
|
static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl)
|
|
{
|
|
/* trigger framesync - must have 0->1 transition. */
|
|
bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC);
|
|
}
|
|
|
|
static int bcm_ptp_framesync_ts(struct phy_device *phydev,
|
|
struct ptp_system_timestamp *sts,
|
|
struct timespec64 *ts,
|
|
u16 orig_ctrl)
|
|
{
|
|
u16 ctrl, reg;
|
|
int i;
|
|
|
|
ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl);
|
|
|
|
ptp_read_system_prets(sts);
|
|
|
|
/* trigger framesync + capture */
|
|
bcm_ptp_framesync(phydev, ctrl | NSE_CAPTURE_EN);
|
|
|
|
ptp_read_system_postts(sts);
|
|
|
|
/* poll for FSYNC interrupt from TS capture */
|
|
for (i = 0; i < 10; i++) {
|
|
reg = bcm_phy_read_exp(phydev, INTR_STATUS);
|
|
if (reg & INTC_FSYNC) {
|
|
bcm_ptp_get_framesync_ts(phydev, ts);
|
|
break;
|
|
}
|
|
}
|
|
|
|
bcm_ptp_framesync_restore(phydev, orig_ctrl);
|
|
|
|
return reg & INTC_FSYNC ? 0 : -ETIMEDOUT;
|
|
}
|
|
|
|
static int bcm_ptp_gettimex(struct ptp_clock_info *info,
|
|
struct timespec64 *ts,
|
|
struct ptp_system_timestamp *sts)
|
|
{
|
|
struct bcm_ptp_private *priv = ptp2priv(info);
|
|
int err;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
err = bcm_ptp_framesync_ts(priv->phydev, sts, ts, priv->nse_ctrl);
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv,
|
|
const struct timespec64 *ts)
|
|
{
|
|
struct phy_device *phydev = priv->phydev;
|
|
u16 ctrl;
|
|
u64 ns;
|
|
|
|
ctrl = bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
|
|
|
|
/* set up time code */
|
|
bcm_phy_write_exp(phydev, TIME_CODE_0, ts->tv_nsec);
|
|
bcm_phy_write_exp(phydev, TIME_CODE_1, ts->tv_nsec >> 16);
|
|
bcm_phy_write_exp(phydev, TIME_CODE_2, ts->tv_sec);
|
|
bcm_phy_write_exp(phydev, TIME_CODE_3, ts->tv_sec >> 16);
|
|
bcm_phy_write_exp(phydev, TIME_CODE_4, ts->tv_sec >> 32);
|
|
|
|
/* set NCO counter to match */
|
|
ns = timespec64_to_ns(ts);
|
|
bcm_phy_write_exp(phydev, NCO_TIME_0, ns >> 4);
|
|
bcm_phy_write_exp(phydev, NCO_TIME_1, ns >> 20);
|
|
bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, (ns >> 36) & 0xfff);
|
|
|
|
/* set up load on next frame sync (auto-clears due to NSE_INIT) */
|
|
bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD);
|
|
|
|
/* must have NSE_INIT in order to write time code */
|
|
bcm_ptp_framesync(phydev, ctrl | NSE_INIT);
|
|
|
|
bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int bcm_ptp_settime(struct ptp_clock_info *info,
|
|
const struct timespec64 *ts)
|
|
{
|
|
struct bcm_ptp_private *priv = ptp2priv(info);
|
|
int err;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
err = bcm_ptp_settime_locked(priv, ts);
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv,
|
|
s64 delta_ns)
|
|
{
|
|
struct timespec64 ts;
|
|
int err;
|
|
s64 ns;
|
|
|
|
err = bcm_ptp_framesync_ts(priv->phydev, NULL, &ts, priv->nse_ctrl);
|
|
if (!err) {
|
|
ns = timespec64_to_ns(&ts) + delta_ns;
|
|
ts = ns_to_timespec64(ns);
|
|
err = bcm_ptp_settime_locked(priv, &ts);
|
|
}
|
|
return err;
|
|
}
|
|
|
|
static int bcm_ptp_adjtime(struct ptp_clock_info *info, s64 delta_ns)
|
|
{
|
|
struct bcm_ptp_private *priv = ptp2priv(info);
|
|
int err;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
err = bcm_ptp_adjtime_locked(priv, delta_ns);
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
/* A 125Mhz clock should adjust 8ns per pulse.
|
|
* The frequency adjustment base is 0x8000 0000, or 8*2^28.
|
|
*
|
|
* Frequency adjustment is
|
|
* adj = scaled_ppm * 8*2^28 / (10^6 * 2^16)
|
|
* which simplifies to:
|
|
* adj = scaled_ppm * 2^9 / 5^6
|
|
*/
|
|
static int bcm_ptp_adjfine(struct ptp_clock_info *info, long scaled_ppm)
|
|
{
|
|
struct bcm_ptp_private *priv = ptp2priv(info);
|
|
int neg_adj = 0;
|
|
u32 diff, freq;
|
|
u16 ctrl;
|
|
u64 adj;
|
|
|
|
if (scaled_ppm < 0) {
|
|
neg_adj = 1;
|
|
scaled_ppm = -scaled_ppm;
|
|
}
|
|
|
|
adj = scaled_ppm << 9;
|
|
diff = div_u64(adj, 15625);
|
|
freq = (8 << 28) + (neg_adj ? -diff : diff);
|
|
|
|
mutex_lock(&priv->mutex);
|
|
|
|
ctrl = bcm_ptp_framesync_disable(priv->phydev, priv->nse_ctrl);
|
|
|
|
bcm_phy_write_exp(priv->phydev, NCO_FREQ_LSB, freq);
|
|
bcm_phy_write_exp(priv->phydev, NCO_FREQ_MSB, freq >> 16);
|
|
|
|
bcm_phy_write_exp(priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL);
|
|
|
|
/* load on next framesync */
|
|
bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, FREQ_LOAD);
|
|
|
|
bcm_ptp_framesync(priv->phydev, ctrl);
|
|
|
|
/* clear load */
|
|
bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0);
|
|
|
|
bcm_ptp_framesync_restore(priv->phydev, priv->nse_ctrl);
|
|
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static bool bcm_ptp_rxtstamp(struct mii_timestamper *mii_ts,
|
|
struct sk_buff *skb, int type)
|
|
{
|
|
struct bcm_ptp_private *priv = mii2priv(mii_ts);
|
|
struct skb_shared_hwtstamps *hwts;
|
|
struct ptp_header *header;
|
|
u32 sec, nsec;
|
|
u8 *data;
|
|
int off;
|
|
|
|
if (!priv->hwts_rx)
|
|
return false;
|
|
|
|
header = ptp_parse_header(skb, type);
|
|
if (!header)
|
|
return false;
|
|
|
|
data = (u8 *)(header + 1);
|
|
sec = get_unaligned_be32(data);
|
|
nsec = get_unaligned_be32(data + 4);
|
|
|
|
hwts = skb_hwtstamps(skb);
|
|
hwts->hwtstamp = ktime_set(sec, nsec);
|
|
|
|
off = data - skb->data + 8;
|
|
if (off < skb->len) {
|
|
memmove(data, data + 8, skb->len - off);
|
|
__pskb_trim(skb, skb->len - 8);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static bool bcm_ptp_get_tstamp(struct bcm_ptp_private *priv,
|
|
struct bcm_ptp_capture *capts)
|
|
{
|
|
struct phy_device *phydev = priv->phydev;
|
|
u16 ts[4], reg;
|
|
u32 sec, nsec;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
|
|
reg = bcm_phy_read_exp(phydev, INTR_STATUS);
|
|
if ((reg & INTC_SOP) == 0) {
|
|
mutex_unlock(&priv->mutex);
|
|
return false;
|
|
}
|
|
|
|
bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_START);
|
|
|
|
ts[0] = bcm_phy_read_exp(phydev, TS_REG_0);
|
|
ts[1] = bcm_phy_read_exp(phydev, TS_REG_1);
|
|
ts[2] = bcm_phy_read_exp(phydev, TS_REG_2);
|
|
ts[3] = bcm_phy_read_exp(phydev, TS_REG_3);
|
|
|
|
/* not in be32 format for some reason */
|
|
capts->seq_id = bcm_phy_read_exp(priv->phydev, TS_INFO_0);
|
|
|
|
reg = bcm_phy_read_exp(phydev, TS_INFO_1);
|
|
capts->msgtype = reg >> 12;
|
|
capts->tx_dir = !!(reg & BIT(11));
|
|
|
|
bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_END);
|
|
bcm_phy_write_exp(phydev, TS_READ_CTRL, 0);
|
|
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
sec = (ts[3] << 16) | ts[2];
|
|
nsec = (ts[1] << 16) | ts[0];
|
|
capts->hwtstamp = ktime_set(sec, nsec);
|
|
|
|
return true;
|
|
}
|
|
|
|
static void bcm_ptp_match_tstamp(struct bcm_ptp_private *priv,
|
|
struct bcm_ptp_capture *capts)
|
|
{
|
|
struct skb_shared_hwtstamps hwts;
|
|
struct sk_buff *skb, *ts_skb;
|
|
unsigned long flags;
|
|
bool first = false;
|
|
|
|
ts_skb = NULL;
|
|
spin_lock_irqsave(&priv->tx_queue.lock, flags);
|
|
skb_queue_walk(&priv->tx_queue, skb) {
|
|
if (BCM_SKB_CB(skb)->seq_id == capts->seq_id &&
|
|
BCM_SKB_CB(skb)->msgtype == capts->msgtype) {
|
|
first = skb_queue_is_first(&priv->tx_queue, skb);
|
|
__skb_unlink(skb, &priv->tx_queue);
|
|
ts_skb = skb;
|
|
break;
|
|
}
|
|
}
|
|
spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
|
|
|
|
/* TX captures one-step packets, discard them if needed. */
|
|
if (ts_skb) {
|
|
if (BCM_SKB_CB(ts_skb)->discard) {
|
|
kfree_skb(ts_skb);
|
|
} else {
|
|
memset(&hwts, 0, sizeof(hwts));
|
|
hwts.hwtstamp = capts->hwtstamp;
|
|
skb_complete_tx_timestamp(ts_skb, &hwts);
|
|
}
|
|
}
|
|
|
|
/* not first match, try and expire entries */
|
|
if (!first) {
|
|
while ((skb = skb_dequeue(&priv->tx_queue))) {
|
|
if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) {
|
|
skb_queue_head(&priv->tx_queue, skb);
|
|
break;
|
|
}
|
|
kfree_skb(skb);
|
|
}
|
|
}
|
|
}
|
|
|
|
static long bcm_ptp_do_aux_work(struct ptp_clock_info *info)
|
|
{
|
|
struct bcm_ptp_private *priv = ptp2priv(info);
|
|
struct bcm_ptp_capture capts;
|
|
bool reschedule = false;
|
|
|
|
while (!skb_queue_empty_lockless(&priv->tx_queue)) {
|
|
if (!bcm_ptp_get_tstamp(priv, &capts)) {
|
|
reschedule = true;
|
|
break;
|
|
}
|
|
bcm_ptp_match_tstamp(priv, &capts);
|
|
}
|
|
|
|
return reschedule ? 1 : -1;
|
|
}
|
|
|
|
static int bcm_ptp_cancel_func(struct bcm_ptp_private *priv)
|
|
{
|
|
if (!priv->pin_active)
|
|
return 0;
|
|
|
|
priv->pin_active = false;
|
|
|
|
priv->nse_ctrl &= ~(NSE_SYNC_OUT_MASK | NSE_SYNC1_FRAMESYNC |
|
|
NSE_CAPTURE_EN);
|
|
bcm_phy_write_exp(priv->phydev, NSE_CTRL, priv->nse_ctrl);
|
|
|
|
cancel_delayed_work_sync(&priv->pin_work);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void bcm_ptp_perout_work(struct work_struct *pin_work)
|
|
{
|
|
struct bcm_ptp_private *priv =
|
|
container_of(pin_work, struct bcm_ptp_private, pin_work.work);
|
|
struct phy_device *phydev = priv->phydev;
|
|
struct timespec64 ts;
|
|
u64 ns, next;
|
|
u16 ctrl;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
|
|
/* no longer running */
|
|
if (!priv->pin_active) {
|
|
mutex_unlock(&priv->mutex);
|
|
return;
|
|
}
|
|
|
|
bcm_ptp_framesync_ts(phydev, NULL, &ts, priv->nse_ctrl);
|
|
|
|
/* this is 1PPS only */
|
|
next = NSEC_PER_SEC - ts.tv_nsec;
|
|
ts.tv_sec += next < NSEC_PER_MSEC ? 2 : 1;
|
|
ts.tv_nsec = 0;
|
|
|
|
ns = timespec64_to_ns(&ts);
|
|
|
|
/* force 0->1 transition for ONESHOT */
|
|
ctrl = bcm_ptp_framesync_disable(phydev,
|
|
priv->nse_ctrl & ~NSE_ONESHOT_EN);
|
|
|
|
bcm_phy_write_exp(phydev, SYNOUT_TS_0, ns & 0xfff0);
|
|
bcm_phy_write_exp(phydev, SYNOUT_TS_1, ns >> 16);
|
|
bcm_phy_write_exp(phydev, SYNOUT_TS_2, ns >> 32);
|
|
|
|
/* load values on next framesync */
|
|
bcm_phy_write_exp(phydev, SHADOW_LOAD, SYNC_OUT_LOAD);
|
|
|
|
bcm_ptp_framesync(phydev, ctrl | NSE_ONESHOT_EN | NSE_INIT);
|
|
|
|
priv->nse_ctrl |= NSE_ONESHOT_EN;
|
|
bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
|
|
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
next = next + NSEC_PER_MSEC;
|
|
schedule_delayed_work(&priv->pin_work, nsecs_to_jiffies(next));
|
|
}
|
|
|
|
static int bcm_ptp_perout_locked(struct bcm_ptp_private *priv,
|
|
struct ptp_perout_request *req, int on)
|
|
{
|
|
struct phy_device *phydev = priv->phydev;
|
|
u64 period, pulse;
|
|
u16 val;
|
|
|
|
if (!on)
|
|
return bcm_ptp_cancel_func(priv);
|
|
|
|
/* 1PPS */
|
|
if (req->period.sec != 1 || req->period.nsec != 0)
|
|
return -EINVAL;
|
|
|
|
period = BCM_MAX_PERIOD_8NS; /* write nonzero value */
|
|
|
|
if (req->flags & PTP_PEROUT_PHASE)
|
|
return -EOPNOTSUPP;
|
|
|
|
if (req->flags & PTP_PEROUT_DUTY_CYCLE)
|
|
pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
|
|
else
|
|
pulse = (u64)BCM_MAX_PULSE_8NS << 3;
|
|
|
|
/* convert to 8ns units */
|
|
pulse >>= 3;
|
|
|
|
if (!pulse || pulse > period || pulse > BCM_MAX_PULSE_8NS)
|
|
return -EINVAL;
|
|
|
|
bcm_phy_write_exp(phydev, SYNC_OUT_0, period);
|
|
|
|
val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
|
|
bcm_phy_write_exp(phydev, SYNC_OUT_1, val);
|
|
|
|
val = ((pulse >> 2) & 0x7f) | (pulse << 7);
|
|
bcm_phy_write_exp(phydev, SYNC_OUT_2, val);
|
|
|
|
if (priv->pin_active)
|
|
cancel_delayed_work_sync(&priv->pin_work);
|
|
|
|
priv->pin_active = true;
|
|
INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_perout_work);
|
|
schedule_delayed_work(&priv->pin_work, 0);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void bcm_ptp_extts_work(struct work_struct *pin_work)
|
|
{
|
|
struct bcm_ptp_private *priv =
|
|
container_of(pin_work, struct bcm_ptp_private, pin_work.work);
|
|
struct phy_device *phydev = priv->phydev;
|
|
struct ptp_clock_event event;
|
|
struct timespec64 ts;
|
|
u16 reg;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
|
|
/* no longer running */
|
|
if (!priv->pin_active) {
|
|
mutex_unlock(&priv->mutex);
|
|
return;
|
|
}
|
|
|
|
reg = bcm_phy_read_exp(phydev, INTR_STATUS);
|
|
if ((reg & INTC_FSYNC) == 0)
|
|
goto out;
|
|
|
|
bcm_ptp_get_framesync_ts(phydev, &ts);
|
|
|
|
event.index = 0;
|
|
event.type = PTP_CLOCK_EXTTS;
|
|
event.timestamp = timespec64_to_ns(&ts);
|
|
ptp_clock_event(priv->ptp_clock, &event);
|
|
|
|
out:
|
|
mutex_unlock(&priv->mutex);
|
|
schedule_delayed_work(&priv->pin_work, HZ / 4);
|
|
}
|
|
|
|
static int bcm_ptp_extts_locked(struct bcm_ptp_private *priv, int on)
|
|
{
|
|
struct phy_device *phydev = priv->phydev;
|
|
|
|
if (!on)
|
|
return bcm_ptp_cancel_func(priv);
|
|
|
|
if (priv->pin_active)
|
|
cancel_delayed_work_sync(&priv->pin_work);
|
|
|
|
bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
|
|
|
|
priv->nse_ctrl |= NSE_SYNC1_FRAMESYNC | NSE_CAPTURE_EN;
|
|
|
|
bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
|
|
|
|
priv->pin_active = true;
|
|
INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_extts_work);
|
|
schedule_delayed_work(&priv->pin_work, 0);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int bcm_ptp_enable(struct ptp_clock_info *info,
|
|
struct ptp_clock_request *rq, int on)
|
|
{
|
|
struct bcm_ptp_private *priv = ptp2priv(info);
|
|
int err = -EBUSY;
|
|
|
|
mutex_lock(&priv->mutex);
|
|
|
|
switch (rq->type) {
|
|
case PTP_CLK_REQ_PEROUT:
|
|
if (priv->pin.func == PTP_PF_PEROUT)
|
|
err = bcm_ptp_perout_locked(priv, &rq->perout, on);
|
|
break;
|
|
case PTP_CLK_REQ_EXTTS:
|
|
if (priv->pin.func == PTP_PF_EXTTS)
|
|
err = bcm_ptp_extts_locked(priv, on);
|
|
break;
|
|
default:
|
|
err = -EOPNOTSUPP;
|
|
break;
|
|
}
|
|
|
|
mutex_unlock(&priv->mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
static int bcm_ptp_verify(struct ptp_clock_info *info, unsigned int pin,
|
|
enum ptp_pin_function func, unsigned int chan)
|
|
{
|
|
switch (func) {
|
|
case PTP_PF_NONE:
|
|
case PTP_PF_EXTTS:
|
|
case PTP_PF_PEROUT:
|
|
break;
|
|
default:
|
|
return -EOPNOTSUPP;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static const struct ptp_clock_info bcm_ptp_clock_info = {
|
|
.owner = THIS_MODULE,
|
|
.name = KBUILD_MODNAME,
|
|
.max_adj = 100000000,
|
|
.gettimex64 = bcm_ptp_gettimex,
|
|
.settime64 = bcm_ptp_settime,
|
|
.adjtime = bcm_ptp_adjtime,
|
|
.adjfine = bcm_ptp_adjfine,
|
|
.enable = bcm_ptp_enable,
|
|
.verify = bcm_ptp_verify,
|
|
.do_aux_work = bcm_ptp_do_aux_work,
|
|
.n_pins = 1,
|
|
.n_per_out = 1,
|
|
.n_ext_ts = 1,
|
|
};
|
|
|
|
static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts,
|
|
struct sk_buff *skb, int type)
|
|
{
|
|
struct bcm_ptp_private *priv = mii2priv(mii_ts);
|
|
struct ptp_header *hdr;
|
|
bool discard = false;
|
|
int msgtype;
|
|
|
|
hdr = ptp_parse_header(skb, type);
|
|
if (!hdr)
|
|
goto out;
|
|
msgtype = ptp_get_msgtype(hdr, type);
|
|
|
|
switch (priv->tx_type) {
|
|
case HWTSTAMP_TX_ONESTEP_P2P:
|
|
if (msgtype == PTP_MSGTYPE_PDELAY_RESP)
|
|
discard = true;
|
|
fallthrough;
|
|
case HWTSTAMP_TX_ONESTEP_SYNC:
|
|
if (msgtype == PTP_MSGTYPE_SYNC)
|
|
discard = true;
|
|
fallthrough;
|
|
case HWTSTAMP_TX_ON:
|
|
BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT;
|
|
BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id);
|
|
BCM_SKB_CB(skb)->msgtype = msgtype;
|
|
BCM_SKB_CB(skb)->discard = discard;
|
|
skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
|
|
skb_queue_tail(&priv->tx_queue, skb);
|
|
ptp_schedule_worker(priv->ptp_clock, 0);
|
|
return;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
out:
|
|
kfree_skb(skb);
|
|
}
|
|
|
|
static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts,
|
|
struct ifreq *ifr)
|
|
{
|
|
struct bcm_ptp_private *priv = mii2priv(mii_ts);
|
|
struct hwtstamp_config cfg;
|
|
u16 mode, ctrl;
|
|
|
|
if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
|
|
return -EFAULT;
|
|
|
|
switch (cfg.rx_filter) {
|
|
case HWTSTAMP_FILTER_NONE:
|
|
priv->hwts_rx = false;
|
|
break;
|
|
case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
|
|
case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
|
|
case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
|
|
case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
|
|
case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
|
|
case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
|
|
case HWTSTAMP_FILTER_PTP_V2_EVENT:
|
|
case HWTSTAMP_FILTER_PTP_V2_SYNC:
|
|
case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
|
|
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
|
|
priv->hwts_rx = true;
|
|
break;
|
|
default:
|
|
return -ERANGE;
|
|
}
|
|
|
|
priv->tx_type = cfg.tx_type;
|
|
|
|
ctrl = priv->hwts_rx ? SLICE_RX_EN : 0;
|
|
ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0;
|
|
|
|
mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) |
|
|
TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) |
|
|
TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) |
|
|
TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS);
|
|
|
|
bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode);
|
|
|
|
mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) |
|
|
RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) |
|
|
RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) |
|
|
RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64);
|
|
|
|
bcm_phy_write_exp(priv->phydev, RX_EVENT_MODE, mode);
|
|
|
|
bcm_phy_write_exp(priv->phydev, SLICE_CTRL, ctrl);
|
|
|
|
if (ctrl & SLICE_TX_EN)
|
|
bcm_phy_write_exp(priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN);
|
|
else
|
|
ptp_cancel_worker_sync(priv->ptp_clock);
|
|
|
|
/* purge existing data */
|
|
skb_queue_purge(&priv->tx_queue);
|
|
|
|
return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
|
|
}
|
|
|
|
static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts,
|
|
struct ethtool_ts_info *ts_info)
|
|
{
|
|
struct bcm_ptp_private *priv = mii2priv(mii_ts);
|
|
|
|
ts_info->phc_index = ptp_clock_index(priv->ptp_clock);
|
|
ts_info->so_timestamping =
|
|
SOF_TIMESTAMPING_TX_HARDWARE |
|
|
SOF_TIMESTAMPING_RX_HARDWARE |
|
|
SOF_TIMESTAMPING_RAW_HARDWARE;
|
|
ts_info->tx_types =
|
|
BIT(HWTSTAMP_TX_ON) |
|
|
BIT(HWTSTAMP_TX_OFF) |
|
|
BIT(HWTSTAMP_TX_ONESTEP_SYNC) |
|
|
BIT(HWTSTAMP_TX_ONESTEP_P2P);
|
|
ts_info->rx_filters =
|
|
BIT(HWTSTAMP_FILTER_NONE) |
|
|
BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void bcm_ptp_stop(struct bcm_ptp_private *priv)
|
|
{
|
|
ptp_cancel_worker_sync(priv->ptp_clock);
|
|
bcm_ptp_cancel_func(priv);
|
|
}
|
|
EXPORT_SYMBOL_GPL(bcm_ptp_stop);
|
|
|
|
void bcm_ptp_config_init(struct phy_device *phydev)
|
|
{
|
|
/* init network sync engine */
|
|
bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT);
|
|
|
|
/* enable time sync (TX/RX SOP capture) */
|
|
bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN);
|
|
|
|
/* use sec.nsec heartbeat capture */
|
|
bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2);
|
|
|
|
/* use 64 bit timecode for TX */
|
|
bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL);
|
|
|
|
/* always allow FREQ_LOAD on framesync */
|
|
bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD);
|
|
|
|
bcm_phy_write_exp(phydev, SYNC_IN_DIVIDER, 1);
|
|
}
|
|
EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
|
|
|
|
static void bcm_ptp_init(struct bcm_ptp_private *priv)
|
|
{
|
|
priv->nse_ctrl = NSE_GMODE_EN;
|
|
|
|
mutex_init(&priv->mutex);
|
|
skb_queue_head_init(&priv->tx_queue);
|
|
|
|
priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp;
|
|
priv->mii_ts.txtstamp = bcm_ptp_txtstamp;
|
|
priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp;
|
|
priv->mii_ts.ts_info = bcm_ptp_ts_info;
|
|
|
|
priv->phydev->mii_ts = &priv->mii_ts;
|
|
}
|
|
|
|
struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
|
|
{
|
|
struct bcm_ptp_private *priv;
|
|
struct ptp_clock *clock;
|
|
|
|
switch (BRCM_PHY_MODEL(phydev)) {
|
|
case PHY_ID_BCM54210E:
|
|
break;
|
|
default:
|
|
return NULL;
|
|
}
|
|
|
|
priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
|
|
if (!priv)
|
|
return ERR_PTR(-ENOMEM);
|
|
|
|
priv->ptp_info = bcm_ptp_clock_info;
|
|
|
|
snprintf(priv->pin.name, sizeof(priv->pin.name), "SYNC_OUT");
|
|
priv->ptp_info.pin_config = &priv->pin;
|
|
|
|
clock = ptp_clock_register(&priv->ptp_info, &phydev->mdio.dev);
|
|
if (IS_ERR(clock))
|
|
return ERR_CAST(clock);
|
|
priv->ptp_clock = clock;
|
|
|
|
priv->phydev = phydev;
|
|
bcm_ptp_init(priv);
|
|
|
|
return priv;
|
|
}
|
|
EXPORT_SYMBOL_GPL(bcm_ptp_probe);
|
|
|
|
MODULE_LICENSE("GPL");
|