#define PS_TO_REG 200
#define FIFO_SIZE 8
+/* Delay used to get the second part from the LTC */
+#define LAN8841_GET_SEC_LTC_DELAY (500 * NSEC_PER_MSEC)
+
struct kszphy_hw_stat {
const char *string;
u8 reg;
/* Lock for ptp_clock */
struct mutex ptp_lock;
struct ptp_pin_desc *pin_config;
+
+ s64 seconds;
+ /* Lock for accessing seconds */
+ spinlock_t seconds_lock;
};
struct kszphy_priv {
#define LAN8841_PTP_CMD_CTL_PTP_RESET BIT(0)
#define LAN8841_PTP_RX_PARSE_CONFIG 368
#define LAN8841_PTP_TX_PARSE_CONFIG 432
+#define LAN8841_PTP_RX_MODE 381
+#define LAN8841_PTP_INSERT_TS_EN BIT(0)
+#define LAN8841_PTP_INSERT_TS_32BIT BIT(1)
static int lan8841_config_init(struct phy_device *phydev)
{
lan8814_match_tx_skb(ptp_priv, sec, nsec, seq);
}
-#define LAN8841_PTP_RX_INGRESS_SEC_LO 389
-#define LAN8841_PTP_RX_INGRESS_SEC_HI 388
-#define LAN8841_PTP_RX_INGRESS_NS_LO 387
-#define LAN8841_PTP_RX_INGRESS_NS_HI 386
-#define LAN8841_PTP_RX_INGRESS_NSEC_HI_VALID BIT(15)
-#define LAN8841_PTP_RX_MSG_HEADER2 391
-
-static struct lan8814_ptp_rx_ts *lan8841_ptp_get_rx_ts(struct kszphy_ptp_priv *ptp_priv)
-{
- struct phy_device *phydev = ptp_priv->phydev;
- struct lan8814_ptp_rx_ts *rx_ts;
- u32 sec, nsec;
- u16 seq;
-
- nsec = phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_NS_HI);
- if (!(nsec & LAN8841_PTP_RX_INGRESS_NSEC_HI_VALID))
- return NULL;
-
- nsec = ((nsec & 0x3fff) << 16);
- nsec = nsec | phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_NS_LO);
-
- sec = phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_SEC_HI);
- sec = sec << 16;
- sec = sec | phy_read_mmd(phydev, 2, LAN8841_PTP_RX_INGRESS_SEC_LO);
-
- seq = phy_read_mmd(phydev, 2, LAN8841_PTP_RX_MSG_HEADER2);
-
- rx_ts = kzalloc(sizeof(*rx_ts), GFP_KERNEL);
- if (!rx_ts)
- return NULL;
-
- rx_ts->seconds = sec;
- rx_ts->nsec = nsec;
- rx_ts->seq_id = seq;
-
- return rx_ts;
-}
-
-static void lan8841_ptp_process_rx_ts(struct kszphy_ptp_priv *ptp_priv)
-{
- struct lan8814_ptp_rx_ts *rx_ts;
-
- while ((rx_ts = lan8841_ptp_get_rx_ts(ptp_priv)) != NULL)
- lan8814_match_rx_ts(ptp_priv, rx_ts);
-}
-
#define LAN8841_PTP_INT_STS 259
#define LAN8841_PTP_INT_STS_PTP_TX_TS_OVRFL_INT BIT(13)
#define LAN8841_PTP_INT_STS_PTP_TX_TS_INT BIT(12)
-#define LAN8841_PTP_INT_STS_PTP_RX_TS_OVRFL_INT BIT(9)
-#define LAN8841_PTP_INT_STS_PTP_RX_TS_INT BIT(8)
#define LAN8841_PTP_INT_STS_PTP_GPIO_CAP_INT BIT(2)
-static void lan8841_ptp_flush_fifo(struct kszphy_ptp_priv *ptp_priv, bool egress)
+static void lan8841_ptp_flush_fifo(struct kszphy_ptp_priv *ptp_priv)
{
struct phy_device *phydev = ptp_priv->phydev;
int i;
for (i = 0; i < FIFO_SIZE; ++i)
- phy_read_mmd(phydev, 2,
- egress ? LAN8841_PTP_TX_MSG_HEADER2 :
- LAN8841_PTP_RX_MSG_HEADER2);
+ phy_read_mmd(phydev, 2, LAN8841_PTP_TX_MSG_HEADER2);
phy_read_mmd(phydev, 2, LAN8841_PTP_INT_STS);
}
if (status & LAN8841_PTP_INT_STS_PTP_TX_TS_INT)
lan8841_ptp_process_tx_ts(ptp_priv);
- if (status & LAN8841_PTP_INT_STS_PTP_RX_TS_INT)
- lan8841_ptp_process_rx_ts(ptp_priv);
-
if (status & LAN8841_PTP_INT_STS_PTP_GPIO_CAP_INT)
lan8841_gpio_process_cap(ptp_priv);
if (status & LAN8841_PTP_INT_STS_PTP_TX_TS_OVRFL_INT) {
- lan8841_ptp_flush_fifo(ptp_priv, true);
+ lan8841_ptp_flush_fifo(ptp_priv);
skb_queue_purge(&ptp_priv->tx_queue);
}
- if (status & LAN8841_PTP_INT_STS_PTP_RX_TS_OVRFL_INT) {
- lan8841_ptp_flush_fifo(ptp_priv, false);
- skb_queue_purge(&ptp_priv->rx_queue);
- }
-
- } while (status);
+ } while (status & (LAN8841_PTP_INT_STS_PTP_TX_TS_INT |
+ LAN8841_PTP_INT_STS_PTP_GPIO_CAP_INT |
+ LAN8841_PTP_INT_STS_PTP_TX_TS_OVRFL_INT));
}
#define LAN8841_INTS_PTP BIT(9)
#define LAN8841_PTP_INT_EN 260
#define LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN BIT(13)
#define LAN8841_PTP_INT_EN_PTP_TX_TS_EN BIT(12)
-#define LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN BIT(9)
-#define LAN8841_PTP_INT_EN_PTP_RX_TS_EN BIT(8)
-static void lan8841_ptp_enable_int(struct kszphy_ptp_priv *ptp_priv,
- bool enable)
+static void lan8841_ptp_enable_processing(struct kszphy_ptp_priv *ptp_priv,
+ bool enable)
{
struct phy_device *phydev = ptp_priv->phydev;
- if (enable)
- /* Enable interrupts */
+ if (enable) {
+ /* Enable interrupts on the TX side */
phy_modify_mmd(phydev, 2, LAN8841_PTP_INT_EN,
LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN |
- LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN |
- LAN8841_PTP_INT_EN_PTP_TX_TS_EN |
- LAN8841_PTP_INT_EN_PTP_RX_TS_EN,
+ LAN8841_PTP_INT_EN_PTP_TX_TS_EN,
LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN |
- LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN |
- LAN8841_PTP_INT_EN_PTP_TX_TS_EN |
- LAN8841_PTP_INT_EN_PTP_RX_TS_EN);
- else
- /* Disable interrupts */
+ LAN8841_PTP_INT_EN_PTP_TX_TS_EN);
+
+ /* Enable the modification of the frame on RX side,
+ * this will add the ns and 2 bits of sec in the reserved field
+ * of the PTP header
+ */
+ phy_modify_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
+ LAN8841_PTP_RX_MODE,
+ LAN8841_PTP_INSERT_TS_EN |
+ LAN8841_PTP_INSERT_TS_32BIT,
+ LAN8841_PTP_INSERT_TS_EN |
+ LAN8841_PTP_INSERT_TS_32BIT);
+
+ ptp_schedule_worker(ptp_priv->ptp_clock, 0);
+ } else {
+ /* Disable interrupts on the TX side */
phy_modify_mmd(phydev, 2, LAN8841_PTP_INT_EN,
LAN8841_PTP_INT_EN_PTP_TX_TS_OVRFL_EN |
- LAN8841_PTP_INT_EN_PTP_RX_TS_OVRFL_EN |
- LAN8841_PTP_INT_EN_PTP_TX_TS_EN |
- LAN8841_PTP_INT_EN_PTP_RX_TS_EN, 0);
+ LAN8841_PTP_INT_EN_PTP_TX_TS_EN, 0);
+
+ /* Disable modification of the RX frames */
+ phy_modify_mmd(phydev, KSZ9131RN_MMD_COMMON_CTRL_REG,
+ LAN8841_PTP_RX_MODE,
+ LAN8841_PTP_INSERT_TS_EN |
+ LAN8841_PTP_INSERT_TS_32BIT, 0);
+
+ ptp_cancel_worker_sync(ptp_priv->ptp_clock);
+ }
}
#define LAN8841_PTP_RX_TIMESTAMP_EN 379
{
struct kszphy_ptp_priv *ptp_priv = container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
struct phy_device *phydev = ptp_priv->phydev;
- struct lan8814_ptp_rx_ts *rx_ts, *tmp;
struct hwtstamp_config config;
int txcfg = 0, rxcfg = 0;
int pkt_ts_enable;
PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_ : 0);
/* Now enable/disable the timestamping */
- lan8841_ptp_enable_int(ptp_priv,
- config.rx_filter != HWTSTAMP_FILTER_NONE);
-
- /* In case of multiple starts and stops, these needs to be cleared */
- list_for_each_entry_safe(rx_ts, tmp, &ptp_priv->rx_ts_list, list) {
- list_del(&rx_ts->list);
- kfree(rx_ts);
- }
+ lan8841_ptp_enable_processing(ptp_priv,
+ config.rx_filter != HWTSTAMP_FILTER_NONE);
- skb_queue_purge(&ptp_priv->rx_queue);
skb_queue_purge(&ptp_priv->tx_queue);
- lan8841_ptp_flush_fifo(ptp_priv, false);
- lan8841_ptp_flush_fifo(ptp_priv, true);
+ lan8841_ptp_flush_fifo(ptp_priv);
return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? -EFAULT : 0;
}
+static bool lan8841_rxtstamp(struct mii_timestamper *mii_ts,
+ struct sk_buff *skb, int type)
+{
+ struct kszphy_ptp_priv *ptp_priv =
+ container_of(mii_ts, struct kszphy_ptp_priv, mii_ts);
+ struct ptp_header *header = ptp_parse_header(skb, type);
+ struct skb_shared_hwtstamps *shhwtstamps;
+ struct timespec64 ts;
+ unsigned long flags;
+ u32 ts_header;
+
+ if (!header)
+ return false;
+
+ if (ptp_priv->rx_filter == HWTSTAMP_FILTER_NONE ||
+ type == PTP_CLASS_NONE)
+ return false;
+
+ if ((type & ptp_priv->version) == 0 || (type & ptp_priv->layer) == 0)
+ return false;
+
+ spin_lock_irqsave(&ptp_priv->seconds_lock, flags);
+ ts.tv_sec = ptp_priv->seconds;
+ spin_unlock_irqrestore(&ptp_priv->seconds_lock, flags);
+ ts_header = __be32_to_cpu(header->reserved2);
+
+ shhwtstamps = skb_hwtstamps(skb);
+ memset(shhwtstamps, 0, sizeof(*shhwtstamps));
+
+ /* Check for any wrap arounds for the second part */
+ if ((ts.tv_sec & GENMASK(1, 0)) == 0 && (ts_header >> 30) == 3)
+ ts.tv_sec -= GENMASK(1, 0) + 1;
+ else if ((ts.tv_sec & GENMASK(1, 0)) == 3 && (ts_header >> 30) == 0)
+ ts.tv_sec += 1;
+
+ shhwtstamps->hwtstamp =
+ ktime_set((ts.tv_sec & ~(GENMASK(1, 0))) | ts_header >> 30,
+ ts_header & GENMASK(29, 0));
+ header->reserved2 = 0;
+
+ netif_rx(skb);
+
+ return true;
+}
+
#define LAN8841_EVENT_A 0
#define LAN8841_EVENT_B 1
#define LAN8841_PTP_LTC_TARGET_SEC_HI(event) ((event) == LAN8841_EVENT_A ? 278 : 288)
struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
ptp_clock_info);
struct phy_device *phydev = ptp_priv->phydev;
+ unsigned long flags;
int ret;
/* Set the value to be stored */
ret = lan8841_ptp_update_target(ptp_priv, ts);
mutex_unlock(&ptp_priv->ptp_lock);
+ spin_lock_irqsave(&ptp_priv->seconds_lock, flags);
+ ptp_priv->seconds = ts->tv_sec;
+ spin_unlock_irqrestore(&ptp_priv->seconds_lock, flags);
+
return ret;
}
return 0;
}
+static void lan8841_ptp_getseconds(struct ptp_clock_info *ptp,
+ struct timespec64 *ts)
+{
+ struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
+ ptp_clock_info);
+ struct phy_device *phydev = ptp_priv->phydev;
+ time64_t s;
+
+ mutex_lock(&ptp_priv->ptp_lock);
+ /* Issue the command to read the LTC */
+ phy_write_mmd(phydev, 2, LAN8841_PTP_CMD_CTL,
+ LAN8841_PTP_CMD_CTL_PTP_LTC_READ);
+
+ /* Read the LTC */
+ s = phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_SEC_HI);
+ s <<= 16;
+ s |= phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_SEC_MID);
+ s <<= 16;
+ s |= phy_read_mmd(phydev, 2, LAN8841_PTP_LTC_RD_SEC_LO);
+ mutex_unlock(&ptp_priv->ptp_lock);
+
+ set_normalized_timespec64(ts, s, 0);
+}
+
#define LAN8841_PTP_LTC_STEP_ADJ_LO 276
#define LAN8841_PTP_LTC_STEP_ADJ_HI 275
#define LAN8841_PTP_LTC_STEP_ADJ_DIR BIT(15)
return 0;
}
+static long lan8841_ptp_do_aux_work(struct ptp_clock_info *ptp)
+{
+ struct kszphy_ptp_priv *ptp_priv = container_of(ptp, struct kszphy_ptp_priv,
+ ptp_clock_info);
+ struct timespec64 ts;
+ unsigned long flags;
+
+ lan8841_ptp_getseconds(&ptp_priv->ptp_clock_info, &ts);
+
+ spin_lock_irqsave(&ptp_priv->seconds_lock, flags);
+ ptp_priv->seconds = ts.tv_sec;
+ spin_unlock_irqrestore(&ptp_priv->seconds_lock, flags);
+
+ return nsecs_to_jiffies(LAN8841_GET_SEC_LTC_DELAY);
+}
+
static struct ptp_clock_info lan8841_ptp_clock_info = {
.owner = THIS_MODULE,
.name = "lan8841 ptp",
.adjfine = lan8841_ptp_adjfine,
.verify = lan8841_ptp_verify,
.enable = lan8841_ptp_enable,
+ .do_aux_work = lan8841_ptp_do_aux_work,
.n_per_out = LAN8841_PTP_GPIO_NUM,
.n_ext_ts = LAN8841_PTP_GPIO_NUM,
.n_pins = LAN8841_PTP_GPIO_NUM,
/* Initialize the SW */
skb_queue_head_init(&ptp_priv->tx_queue);
- skb_queue_head_init(&ptp_priv->rx_queue);
- INIT_LIST_HEAD(&ptp_priv->rx_ts_list);
- spin_lock_init(&ptp_priv->rx_ts_lock);
ptp_priv->phydev = phydev;
mutex_init(&ptp_priv->ptp_lock);
+ spin_lock_init(&ptp_priv->seconds_lock);
- ptp_priv->mii_ts.rxtstamp = lan8814_rxtstamp;
+ ptp_priv->mii_ts.rxtstamp = lan8841_rxtstamp;
ptp_priv->mii_ts.txtstamp = lan8814_txtstamp;
ptp_priv->mii_ts.hwtstamp = lan8841_hwtstamp;
ptp_priv->mii_ts.ts_info = lan8841_ts_info;
return 0;
}
+static int lan8841_suspend(struct phy_device *phydev)
+{
+ struct kszphy_priv *priv = phydev->priv;
+ struct kszphy_ptp_priv *ptp_priv = &priv->ptp_priv;
+
+ ptp_cancel_worker_sync(ptp_priv->ptp_clock);
+
+ return genphy_suspend(phydev);
+}
+
static struct phy_driver ksphy_driver[] = {
{
.phy_id = PHY_ID_KS8737,
.get_sset_count = kszphy_get_sset_count,
.get_strings = kszphy_get_strings,
.get_stats = kszphy_get_stats,
- .suspend = genphy_suspend,
+ .suspend = lan8841_suspend,
.resume = genphy_resume,
.cable_test_start = lan8814_cable_test_start,
.cable_test_get_status = ksz886x_cable_test_get_status,
projects / platform / kernel / linux-rpi.git / commitdiff