[PATCH net-next 3/3] net: phy: micrel: 1588 support for LAN8814 phy

From: Divya Koppera
Date: Fri Mar 04 2022 - 04:36:50 EST

Add support for 1588 in LAN8814 phy driver.
It supports 1-step and 2-step timestamping.

Co-developed-by: Horatiu Vultur <horatiu.vultur@xxxxxxxxxxxxx>
Signed-off-by: Divya Koppera <Divya.Koppera@xxxxxxxxxxxxx>
---
drivers/net/phy/micrel.c | 1088 +++++++++++++++++++++++++++++++++++++-
1 file changed, 1066 insertions(+), 22 deletions(-)

diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 281cebc3d00c..81a76322254c 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -28,6 +28,10 @@
#include <linux/of.h>
#include <linux/clk.h>
#include <linux/delay.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/ptp_clock.h>
+#include <linux/ptp_classify.h>
+#include <linux/net_tstamp.h>

/* Operation Mode Strap Override */
#define MII_KSZPHY_OMSO 0x16
@@ -79,6 +83,119 @@
#define LAN8814_INTR_CTRL_REG_POLARITY BIT(1)
#define LAN8814_INTR_CTRL_REG_INTR_ENABLE BIT(0)

+/* Represents 1ppm adjustment in 2^32 format with
+ * each nsec contains 4 clock cycles.
+ * The value is calculated as following: (1/1000000)/((2^-32)/4)
+ */
+#define LAN8814_1PPM_FORMAT 17179
+
+#define PTP_RX_MOD 0x024F
+#define PTP_RX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ BIT(3)
+#define PTP_RX_TIMESTAMP_EN 0x024D
+#define PTP_TX_TIMESTAMP_EN 0x028D
+
+#define PTP_TIMESTAMP_EN_SYNC_ BIT(0)
+#define PTP_TIMESTAMP_EN_DREQ_ BIT(1)
+#define PTP_TIMESTAMP_EN_PDREQ_ BIT(2)
+#define PTP_TIMESTAMP_EN_PDRES_ BIT(3)
+
+#define PTP_RX_LATENCY_1000 0x0224
+#define PTP_TX_LATENCY_1000 0x0225
+
+#define PTP_RX_LATENCY_100 0x0222
+#define PTP_TX_LATENCY_100 0x0223
+
+#define PTP_RX_LATENCY_10 0x0220
+#define PTP_TX_LATENCY_10 0x0221
+
+#define PTP_TX_PARSE_L2_ADDR_EN 0x0284
+#define PTP_RX_PARSE_L2_ADDR_EN 0x0244
+
+#define PTP_TX_PARSE_IP_ADDR_EN 0x0285
+#define PTP_RX_PARSE_IP_ADDR_EN 0x0245
+#define LTC_HARD_RESET 0x023F
+#define LTC_HARD_RESET_ BIT(0)
+
+#define TSU_HARD_RESET 0x02C1
+#define TSU_HARD_RESET_ BIT(0)
+
+#define PTP_CMD_CTL 0x0200
+#define PTP_CMD_CTL_PTP_DISABLE_ BIT(0)
+#define PTP_CMD_CTL_PTP_ENABLE_ BIT(1)
+#define PTP_CMD_CTL_PTP_CLOCK_READ_ BIT(3)
+#define PTP_CMD_CTL_PTP_CLOCK_LOAD_ BIT(4)
+#define PTP_CMD_CTL_PTP_LTC_STEP_SEC_ BIT(5)
+#define PTP_CMD_CTL_PTP_LTC_STEP_NSEC_ BIT(6)
+
+#define PTP_CLOCK_SET_SEC_MID 0x0206
+#define PTP_CLOCK_SET_SEC_LO 0x0207
+#define PTP_CLOCK_SET_NS_HI 0x0208
+#define PTP_CLOCK_SET_NS_LO 0x0209
+
+#define PTP_CLOCK_READ_SEC_MID 0x022A
+#define PTP_CLOCK_READ_SEC_LO 0x022B
+#define PTP_CLOCK_READ_NS_HI 0x022C
+#define PTP_CLOCK_READ_NS_LO 0x022D
+
+#define PTP_OPERATING_MODE 0x0241
+#define PTP_OPERATING_MODE_STANDALONE_ BIT(0)
+
+#define PTP_TX_MOD 0x028F
+#define PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_ BIT(12)
+#define PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_ BIT(3)
+
+#define PTP_RX_PARSE_CONFIG 0x0242
+#define PTP_RX_PARSE_CONFIG_LAYER2_EN_ BIT(0)
+#define PTP_RX_PARSE_CONFIG_IPV4_EN_ BIT(1)
+#define PTP_RX_PARSE_CONFIG_IPV6_EN_ BIT(2)
+
+#define PTP_TX_PARSE_CONFIG 0x0282
+#define PTP_TX_PARSE_CONFIG_LAYER2_EN_ BIT(0)
+#define PTP_TX_PARSE_CONFIG_IPV4_EN_ BIT(1)
+#define PTP_TX_PARSE_CONFIG_IPV6_EN_ BIT(2)
+
+#define PTP_CLOCK_RATE_ADJ_HI 0x020C
+#define PTP_CLOCK_RATE_ADJ_LO 0x020D
+#define PTP_CLOCK_RATE_ADJ_DIR_ BIT(15)
+
+#define PTP_LTC_STEP_ADJ_HI 0x0212
+#define PTP_LTC_STEP_ADJ_LO 0x0213
+#define PTP_LTC_STEP_ADJ_DIR_ BIT(15)
+
+#define LAN8814_INTR_STS_REG 0x0033
+#define LAN8814_INTR_STS_REG_1588_TSU0_ BIT(0)
+#define LAN8814_INTR_STS_REG_1588_TSU1_ BIT(1)
+#define LAN8814_INTR_STS_REG_1588_TSU2_ BIT(2)
+#define LAN8814_INTR_STS_REG_1588_TSU3_ BIT(3)
+
+#define PTP_CAP_INFO 0x022A
+#define PTP_CAP_INFO_TX_TS_CNT_GET_(reg_val) (((reg_val) & 0x0f00) >> 8)
+#define PTP_CAP_INFO_RX_TS_CNT_GET_(reg_val) ((reg_val) & 0x000f)
+
+#define PTP_TX_EGRESS_SEC_HI 0x0296
+#define PTP_TX_EGRESS_SEC_LO 0x0297
+#define PTP_TX_EGRESS_NS_HI 0x0294
+#define PTP_TX_EGRESS_NS_LO 0x0295
+#define PTP_TX_MSG_HEADER2 0x0299
+
+#define PTP_RX_INGRESS_SEC_HI 0x0256
+#define PTP_RX_INGRESS_SEC_LO 0x0257
+#define PTP_RX_INGRESS_NS_HI 0x0254
+#define PTP_RX_INGRESS_NS_LO 0x0255
+#define PTP_RX_MSG_HEADER2 0x0259
+
+#define PTP_TSU_INT_EN 0x0200
+#define PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN_ BIT(3)
+#define PTP_TSU_INT_EN_PTP_TX_TS_EN_ BIT(2)
+#define PTP_TSU_INT_EN_PTP_RX_TS_OVRFL_EN_ BIT(1)
+#define PTP_TSU_INT_EN_PTP_RX_TS_EN_ BIT(0)
+
+#define PTP_TSU_INT_STS 0x0201
+#define PTP_TSU_INT_STS_PTP_TX_TS_OVRFL_INT_ BIT(3)
+#define PTP_TSU_INT_STS_PTP_TX_TS_EN_ BIT(2)
+#define PTP_TSU_INT_STS_PTP_RX_TS_OVRFL_INT_ BIT(1)
+#define PTP_TSU_INT_STS_PTP_RX_TS_EN_ BIT(0)
+
/* PHY Control 1 */
#define MII_KSZPHY_CTRL_1 0x1e
#define KSZ8081_CTRL1_MDIX_STAT BIT(4)
@@ -108,6 +225,7 @@
#define MII_KSZPHY_TX_DATA_PAD_SKEW 0x106

#define PS_TO_REG 200
+#define FIFO_SIZE 8

struct kszphy_hw_stat {
const char *string;
@@ -128,7 +246,57 @@ struct kszphy_type {
bool has_rmii_ref_clk_sel;
};

+/* Shared structure between the PHYs of the same package. */
+struct lan8814_shared_priv {
+ struct phy_device *phydev;
+ struct ptp_clock *ptp_clock;
+ struct ptp_clock_info ptp_clock_info;
+
+ /* Reference counter to how many ports in the package are enabling the
+ * timestamping
+ */
+ u8 ref;
+
+ /* Lock for ptp_clock and ref */
+ struct mutex shared_lock;
+};
+
+struct lan8814_ptp_rx_ts {
+ struct list_head list;
+ u32 seconds;
+ u32 nsec;
+ u16 seq_id;
+};
+
+struct kszphy_latencies {
+ u16 rx_10;
+ u16 tx_10;
+ u16 rx_100;
+ u16 tx_100;
+ u16 rx_1000;
+ u16 tx_1000;
+};
+
+struct kszphy_ptp_priv {
+ struct mii_timestamper mii_ts;
+ struct phy_device *phydev;
+
+ struct sk_buff_head tx_queue;
+ struct sk_buff_head rx_queue;
+
+ struct list_head rx_ts_list;
+ /* Lock for Rx ts fifo */
+ spinlock_t rx_ts_lock;
+
+ int hwts_tx_type;
+ enum hwtstamp_rx_filters rx_filter;
+ int layer;
+ int version;
+};
+
struct kszphy_priv {
+ struct kszphy_ptp_priv ptp_priv;
+ struct kszphy_latencies latencies;
const struct kszphy_type *type;
int led_mode;
bool rmii_ref_clk_sel;
@@ -136,6 +304,14 @@ struct kszphy_priv {
u64 stats[ARRAY_SIZE(kszphy_hw_stats)];
};

+static struct kszphy_latencies lan8814_latencies = {
+ .rx_10 = 0x22AA,
+ .tx_10 = 0x2E4A,
+ .rx_100 = 0x092A,
+ .tx_100 = 0x02C1,
+ .rx_1000 = 0x01AD,
+ .tx_1000 = 0x00C9,
+};
static const struct kszphy_type ksz8021_type = {
.led_mode_reg = MII_KSZPHY_CTRL_2,
.has_broadcast_disable = true,
@@ -1624,29 +1800,667 @@ static int lanphy_write_page_reg(struct phy_device *phydev, int page, u16 addr,
return val;
}

-static int lan8814_config_init(struct phy_device *phydev)
+static int lan8814_config_ts_intr(struct phy_device *phydev, bool enable)
{
- int val;
+ u16 val = 0;

- /* Reset the PHY */
- val = lanphy_read_page_reg(phydev, 4, LAN8814_QSGMII_SOFT_RESET);
- val |= LAN8814_QSGMII_SOFT_RESET_BIT;
- lanphy_write_page_reg(phydev, 4, LAN8814_QSGMII_SOFT_RESET, val);
+ if (enable)
+ val = PTP_TSU_INT_EN_PTP_TX_TS_EN_ |
+ PTP_TSU_INT_EN_PTP_TX_TS_OVRFL_EN_ |
+ PTP_TSU_INT_EN_PTP_RX_TS_EN_ |
+ PTP_TSU_INT_EN_PTP_RX_TS_OVRFL_EN_;

- /* Disable ANEG with QSGMII PCS Host side */
- val = lanphy_read_page_reg(phydev, 5, LAN8814_QSGMII_PCS1G_ANEG_CONFIG);
- val &= ~LAN8814_QSGMII_PCS1G_ANEG_CONFIG_ANEG_ENA;
- lanphy_write_page_reg(phydev, 5, LAN8814_QSGMII_PCS1G_ANEG_CONFIG, val);
+ return lanphy_write_page_reg(phydev, 5, PTP_TSU_INT_EN, val);
+}

- /* MDI-X setting for swap A,B transmit */
- val = lanphy_read_page_reg(phydev, 2, LAN8814_ALIGN_SWAP);
- val &= ~LAN8814_ALIGN_TX_A_B_SWAP_MASK;
- val |= LAN8814_ALIGN_TX_A_B_SWAP;
- lanphy_write_page_reg(phydev, 2, LAN8814_ALIGN_SWAP, val);
+static void lan8814_ptp_rx_ts_get(struct phy_device *phydev,
+ u32 *seconds, u32 *nano_seconds, u16 *seq_id)
+{
+ *seconds = lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_SEC_HI);
+ *seconds = (*seconds << 16) |
+ lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_SEC_LO);
+
+ *nano_seconds = lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_NS_HI);
+ *nano_seconds = ((*nano_seconds & 0x3fff) << 16) |
+ lanphy_read_page_reg(phydev, 5, PTP_RX_INGRESS_NS_LO);
+
+ *seq_id = lanphy_read_page_reg(phydev, 5, PTP_RX_MSG_HEADER2);
+}
+
+static void lan8814_ptp_tx_ts_get(struct phy_device *phydev,
+ u32 *seconds, u32 *nano_seconds, u16 *seq_id)
+{
+ *seconds = lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_SEC_HI);
+ *seconds = *seconds << 16 |
+ lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_SEC_LO);
+
+ *nano_seconds = lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_NS_HI);
+ *nano_seconds = ((*nano_seconds & 0x3fff) << 16) |
+ lanphy_read_page_reg(phydev, 5, PTP_TX_EGRESS_NS_LO);
+
+ *seq_id = lanphy_read_page_reg(phydev, 5, PTP_TX_MSG_HEADER2);
+}
+
+static int lan8814_ts_info(struct mii_timestamper *mii_ts, struct ethtool_ts_info *info)
+{
+ 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_shared_priv *shared = phydev->shared->priv;
+
+ info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
+ SOF_TIMESTAMPING_RX_HARDWARE |
+ SOF_TIMESTAMPING_RAW_HARDWARE;
+
+ info->phc_index = ptp_clock_index(shared->ptp_clock);
+
+ info->tx_types =
+ (1 << HWTSTAMP_TX_OFF) |
+ (1 << HWTSTAMP_TX_ON) |
+ (1 << HWTSTAMP_TX_ONESTEP_SYNC);
+
+ info->rx_filters =
+ (1 << HWTSTAMP_FILTER_NONE) |
+ (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
+ (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
+ (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
+ (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
+
+ return 0;
+}
+
+static void lan8814_flush_fifo(struct phy_device *phydev, bool egress)
+{
+ int i;
+
+ for (i = 0; i < FIFO_SIZE; ++i)
+ lanphy_read_page_reg(phydev, 5,
+ egress ? PTP_TX_MSG_HEADER2 : PTP_RX_MSG_HEADER2);
+
+ /* Read to clear overflow status bit */
+ lanphy_read_page_reg(phydev, 5, PTP_TSU_INT_STS);
+}
+
+static int lan8814_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
+{
+ 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_shared_priv *shared = phydev->shared->priv;
+ struct lan8814_ptp_rx_ts *rx_ts, *tmp;
+ struct hwtstamp_config config;
+ int txcfg = 0, rxcfg = 0;
+ int pkt_ts_enable;
+
+ if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+ return -EFAULT;
+
+ ptp_priv->hwts_tx_type = config.tx_type;
+ ptp_priv->rx_filter = config.rx_filter;
+
+ switch (config.rx_filter) {
+ case HWTSTAMP_FILTER_NONE:
+ ptp_priv->layer = 0;
+ ptp_priv->version = 0;
+ break;
+ case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+ ptp_priv->layer = PTP_CLASS_L4;
+ ptp_priv->version = PTP_CLASS_V2;
+ break;
+ case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+ ptp_priv->layer = PTP_CLASS_L2;
+ ptp_priv->version = PTP_CLASS_V2;
+ break;
+ case HWTSTAMP_FILTER_PTP_V2_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+ ptp_priv->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
+ ptp_priv->version = PTP_CLASS_V2;
+ break;
+ default:
+ return -ERANGE;
+ }
+
+ if (ptp_priv->layer & PTP_CLASS_L2) {
+ rxcfg = PTP_RX_PARSE_CONFIG_LAYER2_EN_;
+ txcfg = PTP_TX_PARSE_CONFIG_LAYER2_EN_;
+ } else if (ptp_priv->layer & PTP_CLASS_L4) {
+ rxcfg |= PTP_RX_PARSE_CONFIG_IPV4_EN_ | PTP_RX_PARSE_CONFIG_IPV6_EN_;
+ txcfg |= PTP_TX_PARSE_CONFIG_IPV4_EN_ | PTP_TX_PARSE_CONFIG_IPV6_EN_;
+ }
+ lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_RX_PARSE_CONFIG, rxcfg);
+ lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_TX_PARSE_CONFIG, txcfg);
+
+ pkt_ts_enable = PTP_TIMESTAMP_EN_SYNC_ | PTP_TIMESTAMP_EN_DREQ_ |
+ PTP_TIMESTAMP_EN_PDREQ_ | PTP_TIMESTAMP_EN_PDRES_;
+ lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_RX_TIMESTAMP_EN, pkt_ts_enable);
+ lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_TX_TIMESTAMP_EN, pkt_ts_enable);
+
+ if (ptp_priv->hwts_tx_type == HWTSTAMP_TX_ONESTEP_SYNC)
+ lanphy_write_page_reg(ptp_priv->phydev, 5, PTP_TX_MOD,
+ PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_);
+
+ if (config.rx_filter != HWTSTAMP_FILTER_NONE)
+ lan8814_config_ts_intr(ptp_priv->phydev, true);
+ else
+ lan8814_config_ts_intr(ptp_priv->phydev, false);
+
+ mutex_lock(&shared->shared_lock);
+ if (config.rx_filter != HWTSTAMP_FILTER_NONE)
+ shared->ref++;
+ else
+ shared->ref--;
+
+ if (shared->ref)
+ lanphy_write_page_reg(ptp_priv->phydev, 4, PTP_CMD_CTL,
+ PTP_CMD_CTL_PTP_ENABLE_);
+ else
+ lanphy_write_page_reg(ptp_priv->phydev, 4, PTP_CMD_CTL,
+ PTP_CMD_CTL_PTP_DISABLE_);
+ mutex_unlock(&shared->shared_lock);
+
+ /* 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);
+ }
+ skb_queue_purge(&ptp_priv->rx_queue);
+ skb_queue_purge(&ptp_priv->tx_queue);
+
+ lan8814_flush_fifo(ptp_priv->phydev, false);
+ lan8814_flush_fifo(ptp_priv->phydev, true);
+
+ return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? -EFAULT : 0;
+}
+
+static bool is_sync(struct sk_buff *skb, int type)
+{
+ struct ptp_header *hdr;
+
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
+ return false;
+
+ return ((ptp_get_msgtype(hdr, type) & 0xf) == 0);
+}
+
+static void lan8814_txtstamp(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);
+
+ switch (ptp_priv->hwts_tx_type) {
+ case HWTSTAMP_TX_ONESTEP_SYNC:
+ if (is_sync(skb, type)) {
+ kfree_skb(skb);
+ return;
+ }
+ fallthrough;
+ case HWTSTAMP_TX_ON:
+ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+ skb_queue_tail(&ptp_priv->tx_queue, skb);
+ break;
+ case HWTSTAMP_TX_OFF:
+ default:
+ kfree_skb(skb);
+ break;
+ }
+}
+
+static void lan8814_get_sig_rx(struct sk_buff *skb, u16 *sig)
+{
+ struct ptp_header *ptp_header;
+ u32 type;
+
+ skb_push(skb, ETH_HLEN);
+ type = ptp_classify_raw(skb);
+ ptp_header = ptp_parse_header(skb, type);
+ skb_pull_inline(skb, ETH_HLEN);
+
+ *sig = (__force u16)(ntohs(ptp_header->sequence_id));
+}
+
+static bool lan8814_match_rx_ts(struct kszphy_ptp_priv *ptp_priv,
+ struct sk_buff *skb)
+{
+ struct skb_shared_hwtstamps *shhwtstamps;
+ struct lan8814_ptp_rx_ts *rx_ts, *tmp;
+ unsigned long flags;
+ bool ret = false;
+ u16 skb_sig;
+
+ lan8814_get_sig_rx(skb, &skb_sig);
+
+ /* Iterate over all RX timestamps and match it with the received skbs */
+ spin_lock_irqsave(&ptp_priv->rx_ts_lock, flags);
+ list_for_each_entry_safe(rx_ts, tmp, &ptp_priv->rx_ts_list, list) {
+ /* Check if we found the signature we were looking for. */
+ if (memcmp(&skb_sig, &rx_ts->seq_id, sizeof(rx_ts->seq_id)))
+ continue;
+
+ shhwtstamps = skb_hwtstamps(skb);
+ memset(shhwtstamps, 0, sizeof(*shhwtstamps));
+ shhwtstamps->hwtstamp = ktime_set(rx_ts->seconds,
+ rx_ts->nsec);
+ netif_rx_ni(skb);
+
+ list_del(&rx_ts->list);
+ kfree(rx_ts);
+
+ ret = true;
+ break;
+ }
+ spin_unlock_irqrestore(&ptp_priv->rx_ts_lock, flags);
+
+ return ret;
+}
+
+static bool lan8814_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);
+
+ 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;
+
+ /* If we failed to match then add it to the queue for when the timestamp
+ * will come
+ */
+ if (!lan8814_match_rx_ts(ptp_priv, skb))
+ skb_queue_tail(&ptp_priv->rx_queue, skb);
+
+ return true;
+}
+
+static void lan8814_ptp_clock_set(struct phy_device *phydev,
+ u32 seconds, u32 nano_seconds)
+{
+ u32 sec_low, sec_high, nsec_low, nsec_high;
+
+ sec_low = seconds & 0xffff;
+ sec_high = (seconds >> 16) & 0xffff;
+ nsec_low = nano_seconds & 0xffff;
+ nsec_high = (nano_seconds >> 16) & 0x3fff;
+
+ lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_SEC_LO, sec_low);
+ lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_SEC_MID, sec_high);
+ lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_NS_LO, nsec_low);
+ lanphy_write_page_reg(phydev, 4, PTP_CLOCK_SET_NS_HI, nsec_high);
+
+ lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_LOAD_);
+}
+
+static void lan8814_ptp_clock_get(struct phy_device *phydev,
+ u32 *seconds, u32 *nano_seconds)
+{
+ lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_READ_);
+
+ *seconds = lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_SEC_MID);
+ *seconds = (*seconds << 16) |
+ lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_SEC_LO);
+
+ *nano_seconds = lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_NS_HI);
+ *nano_seconds = ((*nano_seconds & 0x3fff) << 16) |
+ lanphy_read_page_reg(phydev, 4, PTP_CLOCK_READ_NS_LO);
+}
+
+static int lan8814_ptpci_gettime64(struct ptp_clock_info *ptpci,
+ struct timespec64 *ts)
+{
+ struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
+ ptp_clock_info);
+ struct phy_device *phydev = shared->phydev;
+ u32 nano_seconds;
+ u32 seconds;
+
+ mutex_lock(&shared->shared_lock);
+ lan8814_ptp_clock_get(phydev, &seconds, &nano_seconds);
+ mutex_unlock(&shared->shared_lock);
+ ts->tv_sec = seconds;
+ ts->tv_nsec = nano_seconds;

return 0;
}

+static int lan8814_ptpci_settime64(struct ptp_clock_info *ptpci,
+ const struct timespec64 *ts)
+{
+ struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
+ ptp_clock_info);
+ struct phy_device *phydev = shared->phydev;
+
+ mutex_lock(&shared->shared_lock);
+ lan8814_ptp_clock_set(phydev, ts->tv_sec, ts->tv_nsec);
+ mutex_unlock(&shared->shared_lock);
+
+ return 0;
+}
+
+static void lan8814_ptp_clock_step(struct phy_device *phydev,
+ s64 time_step_ns)
+{
+ u32 nano_seconds_step;
+ u64 abs_time_step_ns;
+ u32 unsigned_seconds;
+ u32 nano_seconds;
+ u32 remainder;
+ s32 seconds;
+
+ if (time_step_ns > 15000000000LL) {
+ /* convert to clock set */
+ lan8814_ptp_clock_get(phydev, &unsigned_seconds, &nano_seconds);
+ unsigned_seconds += div_u64_rem(time_step_ns, 1000000000LL,
+ &remainder);
+ nano_seconds += remainder;
+ if (nano_seconds >= 1000000000) {
+ unsigned_seconds++;
+ nano_seconds -= 1000000000;
+ }
+ lan8814_ptp_clock_set(phydev, unsigned_seconds, nano_seconds);
+ return;
+ } else if (time_step_ns < -15000000000LL) {
+ /* convert to clock set */
+ time_step_ns = -time_step_ns;
+
+ lan8814_ptp_clock_get(phydev, &unsigned_seconds, &nano_seconds);
+ unsigned_seconds -= div_u64_rem(time_step_ns, 1000000000LL,
+ &remainder);
+ nano_seconds_step = remainder;
+ if (nano_seconds < nano_seconds_step) {
+ unsigned_seconds--;
+ nano_seconds += 1000000000;
+ }
+ nano_seconds -= nano_seconds_step;
+ lan8814_ptp_clock_set(phydev, unsigned_seconds,
+ nano_seconds);
+ return;
+ }
+
+ /* do clock step */
+ if (time_step_ns >= 0) {
+ abs_time_step_ns = (u64)time_step_ns;
+ seconds = (s32)div_u64_rem(abs_time_step_ns, 1000000000,
+ &remainder);
+ nano_seconds = remainder;
+ } else {
+ abs_time_step_ns = (u64)(-time_step_ns);
+ seconds = -((s32)div_u64_rem(abs_time_step_ns, 1000000000,
+ &remainder));
+ nano_seconds = remainder;
+ if (nano_seconds > 0) {
+ /* subtracting nano seconds is not allowed
+ * convert to subtracting from seconds,
+ * and adding to nanoseconds
+ */
+ seconds--;
+ nano_seconds = (1000000000 - nano_seconds);
+ }
+ }
+
+ if (nano_seconds > 0) {
+ /* add 8 ns to cover the likely normal increment */
+ nano_seconds += 8;
+ }
+
+ if (nano_seconds >= 1000000000) {
+ /* carry into seconds */
+ seconds++;
+ nano_seconds -= 1000000000;
+ }
+
+ while (seconds) {
+ if (seconds > 0) {
+ u32 adjustment_value = (u32)seconds;
+ u16 adjustment_value_lo, adjustment_value_hi;
+
+ if (adjustment_value > 0xF)
+ adjustment_value = 0xF;
+
+ adjustment_value_lo = adjustment_value & 0xffff;
+ adjustment_value_hi = (adjustment_value >> 16) & 0x3fff;
+
+ lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_LO,
+ adjustment_value_lo);
+ lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_HI,
+ PTP_LTC_STEP_ADJ_DIR_ |
+ adjustment_value_hi);
+ seconds -= ((s32)adjustment_value);
+ } else {
+ u32 adjustment_value = (u32)(-seconds);
+ u16 adjustment_value_lo, adjustment_value_hi;
+
+ if (adjustment_value > 0xF)
+ adjustment_value = 0xF;
+
+ adjustment_value_lo = adjustment_value & 0xffff;
+ adjustment_value_hi = (adjustment_value >> 16) & 0x3fff;
+
+ lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_LO,
+ adjustment_value_lo);
+ lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_HI,
+ adjustment_value_hi);
+ seconds += ((s32)adjustment_value);
+ }
+ lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL,
+ PTP_CMD_CTL_PTP_LTC_STEP_SEC_);
+ }
+ if (nano_seconds) {
+ u16 nano_seconds_lo;
+ u16 nano_seconds_hi;
+
+ nano_seconds_lo = nano_seconds & 0xffff;
+ nano_seconds_hi = (nano_seconds >> 16) & 0x3fff;
+
+ lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_LO,
+ nano_seconds_lo);
+ lanphy_write_page_reg(phydev, 4, PTP_LTC_STEP_ADJ_HI,
+ PTP_LTC_STEP_ADJ_DIR_ |
+ nano_seconds_hi);
+ lanphy_write_page_reg(phydev, 4, PTP_CMD_CTL,
+ PTP_CMD_CTL_PTP_LTC_STEP_NSEC_);
+ }
+}
+
+static int lan8814_ptpci_adjtime(struct ptp_clock_info *ptpci, s64 delta)
+{
+ struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
+ ptp_clock_info);
+ struct phy_device *phydev = shared->phydev;
+
+ mutex_lock(&shared->shared_lock);
+ lan8814_ptp_clock_step(phydev, delta);
+ mutex_unlock(&shared->shared_lock);
+
+ return 0;
+}
+
+static int lan8814_ptpci_adjfine(struct ptp_clock_info *ptpci, long scaled_ppm)
+{
+ struct lan8814_shared_priv *shared = container_of(ptpci, struct lan8814_shared_priv,
+ ptp_clock_info);
+ struct phy_device *phydev = shared->phydev;
+ u16 kszphy_rate_adj_lo, kszphy_rate_adj_hi;
+ bool positive = true;
+ u32 kszphy_rate_adj;
+
+ if (scaled_ppm < 0) {
+ scaled_ppm = -scaled_ppm;
+ positive = false;
+ }
+
+ kszphy_rate_adj = LAN8814_1PPM_FORMAT * (scaled_ppm >> 16);
+ kszphy_rate_adj += (LAN8814_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
+
+ kszphy_rate_adj_lo = kszphy_rate_adj & 0xffff;
+ kszphy_rate_adj_hi = (kszphy_rate_adj >> 16) & 0x3fff;
+
+ if (positive)
+ kszphy_rate_adj_hi |= PTP_CLOCK_RATE_ADJ_DIR_;
+
+ mutex_lock(&shared->shared_lock);
+ lanphy_write_page_reg(phydev, 4, PTP_CLOCK_RATE_ADJ_HI, kszphy_rate_adj_hi);
+ lanphy_write_page_reg(phydev, 4, PTP_CLOCK_RATE_ADJ_LO, kszphy_rate_adj_lo);
+ mutex_unlock(&shared->shared_lock);
+
+ return 0;
+}
+
+static void lan8814_get_sig_tx(struct sk_buff *skb, u16 *sig)
+{
+ struct ptp_header *ptp_header;
+ u32 type;
+
+ type = ptp_classify_raw(skb);
+ ptp_header = ptp_parse_header(skb, type);
+
+ *sig = (__force u16)(ntohs(ptp_header->sequence_id));
+}
+
+static void lan8814_dequeue_tx_skb(struct kszphy_ptp_priv *ptp_priv)
+{
+ struct phy_device *phydev = ptp_priv->phydev;
+ struct skb_shared_hwtstamps shhwtstamps;
+ struct sk_buff *skb, *skb_tmp;
+ unsigned long flags;
+ u32 seconds, nsec;
+ bool ret = false;
+ u16 skb_sig;
+ u16 seq_id;
+
+ lan8814_ptp_tx_ts_get(phydev, &seconds, &nsec, &seq_id);
+
+ spin_lock_irqsave(&ptp_priv->tx_queue.lock, flags);
+ skb_queue_walk_safe(&ptp_priv->tx_queue, skb, skb_tmp) {
+ lan8814_get_sig_tx(skb, &skb_sig);
+
+ if (memcmp(&skb_sig, &seq_id, sizeof(seq_id)))
+ continue;
+
+ __skb_unlink(skb, &ptp_priv->tx_queue);
+ ret = true;
+ break;
+ }
+ spin_unlock_irqrestore(&ptp_priv->tx_queue.lock, flags);
+
+ if (ret) {
+ memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+ shhwtstamps.hwtstamp = ktime_set(seconds, nsec);
+ skb_complete_tx_timestamp(skb, &shhwtstamps);
+ }
+}
+
+static void lan8814_get_tx_ts(struct kszphy_ptp_priv *ptp_priv)
+{
+ struct phy_device *phydev = ptp_priv->phydev;
+ u32 reg;
+
+ do {
+ lan8814_dequeue_tx_skb(ptp_priv);
+
+ /* If other timestamps are available in the FIFO,
+ * process them.
+ */
+ reg = lanphy_read_page_reg(phydev, 5, PTP_CAP_INFO);
+ } while (PTP_CAP_INFO_TX_TS_CNT_GET_(reg) > 0);
+}
+
+static bool lan8814_match_skb(struct kszphy_ptp_priv *ptp_priv,
+ struct lan8814_ptp_rx_ts *rx_ts)
+{
+ struct skb_shared_hwtstamps *shhwtstamps;
+ struct sk_buff *skb, *skb_tmp;
+ unsigned long flags;
+ bool ret = false;
+ u16 skb_sig;
+
+ spin_lock_irqsave(&ptp_priv->rx_queue.lock, flags);
+ skb_queue_walk_safe(&ptp_priv->rx_queue, skb, skb_tmp) {
+ lan8814_get_sig_rx(skb, &skb_sig);
+
+ if (memcmp(&skb_sig, &rx_ts->seq_id, sizeof(rx_ts->seq_id)))
+ continue;
+
+ __skb_unlink(skb, &ptp_priv->rx_queue);
+
+ ret = true;
+ break;
+ }
+ spin_unlock_irqrestore(&ptp_priv->rx_queue.lock, flags);
+
+ if (ret) {
+ shhwtstamps = skb_hwtstamps(skb);
+ memset(shhwtstamps, 0, sizeof(*shhwtstamps));
+ shhwtstamps->hwtstamp = ktime_set(rx_ts->seconds, rx_ts->nsec);
+ netif_rx_ni(skb);
+ }
+
+ return ret;
+}
+
+static void lan8814_get_rx_ts(struct kszphy_ptp_priv *ptp_priv)
+{
+ struct phy_device *phydev = ptp_priv->phydev;
+ struct lan8814_ptp_rx_ts *rx_ts;
+ unsigned long flags;
+ u32 reg;
+
+ do {
+ rx_ts = kzalloc(sizeof(*rx_ts), GFP_KERNEL);
+ if (!rx_ts)
+ return;
+
+ lan8814_ptp_rx_ts_get(phydev, &rx_ts->seconds, &rx_ts->nsec,
+ &rx_ts->seq_id);
+
+ /* If we failed to match the skb add it to the queue for when
+ * the frame will come
+ */
+ if (!lan8814_match_skb(ptp_priv, rx_ts)) {
+ spin_lock_irqsave(&ptp_priv->rx_ts_lock, flags);
+ list_add(&rx_ts->list, &ptp_priv->rx_ts_list);
+ spin_unlock_irqrestore(&ptp_priv->rx_ts_lock, flags);
+ } else {
+ kfree(rx_ts);
+ }
+
+ /* If other timestamps are available in the FIFO,
+ * process them.
+ */
+ reg = lanphy_read_page_reg(phydev, 5, PTP_CAP_INFO);
+ } while (PTP_CAP_INFO_RX_TS_CNT_GET_(reg) > 0);
+}
+
+static void lan8814_handle_ptp_interrupt(struct phy_device *phydev)
+{
+ struct kszphy_priv *priv = phydev->priv;
+ struct kszphy_ptp_priv *ptp_priv = &priv->ptp_priv;
+ u16 status;
+
+ status = lanphy_read_page_reg(phydev, 5, PTP_TSU_INT_STS);
+ if (status & PTP_TSU_INT_STS_PTP_TX_TS_EN_)
+ lan8814_get_tx_ts(ptp_priv);
+
+ if (status & PTP_TSU_INT_STS_PTP_RX_TS_EN_)
+ lan8814_get_rx_ts(ptp_priv);
+
+ if (status & PTP_TSU_INT_STS_PTP_TX_TS_OVRFL_INT_) {
+ lan8814_flush_fifo(phydev, true);
+ skb_queue_purge(&ptp_priv->tx_queue);
+ }
+
+ if (status & PTP_TSU_INT_STS_PTP_RX_TS_OVRFL_INT_) {
+ lan8814_flush_fifo(phydev, false);
+ skb_queue_purge(&ptp_priv->rx_queue);
+ }
+}
+
static int lan8804_config_init(struct phy_device *phydev)
{
int val;
@@ -1668,17 +2482,31 @@ static int lan8804_config_init(struct phy_device *phydev)

static irqreturn_t lan8814_handle_interrupt(struct phy_device *phydev)
{
+ u16 tsu_irq_status;
int irq_status;

irq_status = phy_read(phydev, LAN8814_INTS);
- if (irq_status < 0)
- return IRQ_NONE;
+ if (irq_status > 0 && (irq_status & LAN8814_INT_LINK))
+ phy_trigger_machine(phydev);

- if (!(irq_status & LAN8814_INT_LINK))
+ if (irq_status < 0) {
+ phy_error(phydev);
return IRQ_NONE;
+ }

- phy_trigger_machine(phydev);
+ while (1) {
+ tsu_irq_status = lanphy_read_page_reg(phydev, 4,
+ LAN8814_INTR_STS_REG);

+ if (tsu_irq_status > 0 &&
+ (tsu_irq_status & (LAN8814_INTR_STS_REG_1588_TSU0_ |
+ LAN8814_INTR_STS_REG_1588_TSU1_ |
+ LAN8814_INTR_STS_REG_1588_TSU2_ |
+ LAN8814_INTR_STS_REG_1588_TSU3_)))
+ lan8814_handle_ptp_interrupt(phydev);
+ else
+ break;
+ }
return IRQ_HANDLED;
}

@@ -1718,6 +2546,223 @@ static int lan8814_config_intr(struct phy_device *phydev)
return err;
}

+static void lan8814_ptp_init(struct phy_device *phydev)
+{
+ struct kszphy_priv *priv = phydev->priv;
+ struct kszphy_ptp_priv *ptp_priv = &priv->ptp_priv;
+ u32 temp;
+
+ lanphy_write_page_reg(phydev, 5, TSU_HARD_RESET, TSU_HARD_RESET_);
+
+ temp = lanphy_read_page_reg(phydev, 5, PTP_TX_MOD);
+ temp |= PTP_TX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_;
+ lanphy_write_page_reg(phydev, 5, PTP_TX_MOD, temp);
+
+ temp = lanphy_read_page_reg(phydev, 5, PTP_RX_MOD);
+ temp |= PTP_RX_MOD_BAD_UDPV4_CHKSUM_FORCE_FCS_DIS_;
+ lanphy_write_page_reg(phydev, 5, PTP_RX_MOD, temp);
+
+ lanphy_write_page_reg(phydev, 5, PTP_RX_PARSE_CONFIG, 0);
+ lanphy_write_page_reg(phydev, 5, PTP_TX_PARSE_CONFIG, 0);
+
+ /* Removing default registers configs related to L2 and IP */
+ lanphy_write_page_reg(phydev, 5, PTP_TX_PARSE_L2_ADDR_EN, 0);
+ lanphy_write_page_reg(phydev, 5, PTP_RX_PARSE_L2_ADDR_EN, 0);
+ lanphy_write_page_reg(phydev, 5, PTP_TX_PARSE_IP_ADDR_EN, 0);
+ lanphy_write_page_reg(phydev, 5, PTP_RX_PARSE_IP_ADDR_EN, 0);
+
+ 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;
+
+ ptp_priv->mii_ts.rxtstamp = lan8814_rxtstamp;
+ ptp_priv->mii_ts.txtstamp = lan8814_txtstamp;
+ ptp_priv->mii_ts.hwtstamp = lan8814_hwtstamp;
+ ptp_priv->mii_ts.ts_info = lan8814_ts_info;
+
+ phydev->mii_ts = &ptp_priv->mii_ts;
+}
+
+static int lan8814_ptp_probe_once(struct phy_device *phydev)
+{
+ struct lan8814_shared_priv *shared = phydev->shared->priv;
+
+ /* Initialise shared lock for clock*/
+ mutex_init(&shared->shared_lock);
+
+ shared->ptp_clock_info.owner = THIS_MODULE;
+ snprintf(shared->ptp_clock_info.name, 30, "%s", phydev->drv->name);
+ shared->ptp_clock_info.max_adj = 31249999;
+ shared->ptp_clock_info.n_alarm = 0;
+ shared->ptp_clock_info.n_ext_ts = 0;
+ shared->ptp_clock_info.n_pins = 0;
+ shared->ptp_clock_info.pps = 0;
+ shared->ptp_clock_info.pin_config = NULL;
+ shared->ptp_clock_info.adjfine = lan8814_ptpci_adjfine;
+ shared->ptp_clock_info.adjtime = lan8814_ptpci_adjtime;
+ shared->ptp_clock_info.gettime64 = lan8814_ptpci_gettime64;
+ shared->ptp_clock_info.settime64 = lan8814_ptpci_settime64;
+ shared->ptp_clock_info.getcrosststamp = NULL;
+
+ shared->ptp_clock = ptp_clock_register(&shared->ptp_clock_info,
+ &phydev->mdio.dev);
+ if (IS_ERR_OR_NULL(shared->ptp_clock)) {
+ phydev_err(phydev, "ptp_clock_register failed %lu\n",
+ PTR_ERR(shared->ptp_clock));
+ return -EINVAL;
+ }
+
+ phydev_dbg(phydev, "successfully registered ptp clock\n");
+
+ shared->phydev = phydev;
+
+ /* The EP.4 is shared between all the PHYs in the package and also it
+ * can be accessed by any of the PHYs
+ */
+ lanphy_write_page_reg(phydev, 4, LTC_HARD_RESET, LTC_HARD_RESET_);
+ lanphy_write_page_reg(phydev, 4, PTP_OPERATING_MODE,
+ PTP_OPERATING_MODE_STANDALONE_);
+
+ return 0;
+}
+
+static int lan8814_read_status(struct phy_device *phydev)
+{
+ struct kszphy_priv *priv = phydev->priv;
+ struct kszphy_latencies *latencies = &priv->latencies;
+ int err;
+ int regval;
+
+ err = genphy_read_status(phydev);
+ if (err)
+ return err;
+
+ switch (phydev->speed) {
+ case SPEED_1000:
+ lanphy_write_page_reg(phydev, 5, PTP_RX_LATENCY_1000,
+ latencies->rx_1000);
+ lanphy_write_page_reg(phydev, 5, PTP_TX_LATENCY_1000,
+ latencies->tx_1000);
+ break;
+ case SPEED_100:
+ lanphy_write_page_reg(phydev, 5, PTP_RX_LATENCY_100,
+ latencies->rx_100);
+ lanphy_write_page_reg(phydev, 5, PTP_TX_LATENCY_100,
+ latencies->tx_100);
+ break;
+ case SPEED_10:
+ lanphy_write_page_reg(phydev, 5, PTP_RX_LATENCY_10,
+ latencies->rx_10);
+ lanphy_write_page_reg(phydev, 5, PTP_TX_LATENCY_10,
+ latencies->tx_10);
+ break;
+ default:
+ break;
+ }
+
+ /* Make sure the PHY is not broken. Read idle error count,
+ * and reset the PHY if it is maxed out.
+ */
+ regval = phy_read(phydev, MII_STAT1000);
+ if ((regval & 0xFF) == 0xFF) {
+ phy_init_hw(phydev);
+ phydev->link = 0;
+ if (phydev->drv->config_intr && phy_interrupt_is_valid(phydev))
+ phydev->drv->config_intr(phydev);
+ return genphy_config_aneg(phydev);
+ }
+
+ return 0;
+}
+
+static int lan8814_config_init(struct phy_device *phydev)
+{
+ int val;
+
+ /* Reset the PHY */
+ val = lanphy_read_page_reg(phydev, 4, LAN8814_QSGMII_SOFT_RESET);
+ val |= LAN8814_QSGMII_SOFT_RESET_BIT;
+ lanphy_write_page_reg(phydev, 4, LAN8814_QSGMII_SOFT_RESET, val);
+
+ /* Disable ANEG with QSGMII PCS Host side */
+ val = lanphy_read_page_reg(phydev, 5, LAN8814_QSGMII_PCS1G_ANEG_CONFIG);
+ val &= ~LAN8814_QSGMII_PCS1G_ANEG_CONFIG_ANEG_ENA;
+ lanphy_write_page_reg(phydev, 5, LAN8814_QSGMII_PCS1G_ANEG_CONFIG, val);
+
+ /* MDI-X setting for swap A,B transmit */
+ val = lanphy_read_page_reg(phydev, 2, LAN8814_ALIGN_SWAP);
+ val &= ~LAN8814_ALIGN_TX_A_B_SWAP_MASK;
+ val |= LAN8814_ALIGN_TX_A_B_SWAP;
+ lanphy_write_page_reg(phydev, 2, LAN8814_ALIGN_SWAP, val);
+
+ return 0;
+}
+
+static void lan8814_parse_latency(struct phy_device *phydev)
+{
+ const struct device_node *np = phydev->mdio.dev.of_node;
+ struct kszphy_priv *priv = phydev->priv;
+ struct kszphy_latencies *latency = &priv->latencies;
+ u32 val;
+
+ if (!of_property_read_u32(np, "lan8814,latency_rx_10", &val))
+ latency->rx_10 = val;
+ if (!of_property_read_u32(np, "lan8814,latency_tx_10", &val))
+ latency->tx_10 = val;
+ if (!of_property_read_u32(np, "lan8814,latency_rx_100", &val))
+ latency->rx_100 = val;
+ if (!of_property_read_u32(np, "lan8814,latency_tx_100", &val))
+ latency->tx_100 = val;
+ if (!of_property_read_u32(np, "lan8814,latency_rx_1000", &val))
+ latency->rx_1000 = val;
+ if (!of_property_read_u32(np, "lan8814,latency_tx_1000", &val))
+ latency->tx_1000 = val;
+}
+
+static int lan8814_probe(struct phy_device *phydev)
+{
+ const struct device_node *np = phydev->mdio.dev.of_node;
+ struct kszphy_priv *priv;
+ u16 addr;
+ int err;
+
+ priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
+ if (!priv)
+ return -ENOMEM;
+
+ priv->led_mode = -1;
+
+ priv->latencies = lan8814_latencies;
+
+ phydev->priv = priv;
+
+ if (!IS_ENABLED(CONFIG_PTP_1588_CLOCK) ||
+ !IS_ENABLED(CONFIG_NETWORK_PHY_TIMESTAMPING) ||
+ of_property_read_bool(np, "lan8814,ignore-ts"))
+ return 0;
+
+ /* Strap-in value for PHY address, below register read gives starting
+ * phy address value
+ */
+ addr = lanphy_read_page_reg(phydev, 4, 0) & 0x1F;
+ devm_phy_package_join(&phydev->mdio.dev, phydev,
+ addr, sizeof(struct lan8814_shared_priv));
+
+ if (phy_package_init_once(phydev)) {
+ err = lan8814_ptp_probe_once(phydev);
+ if (err)
+ return err;
+ }
+
+ lan8814_parse_latency(phydev);
+ lan8814_ptp_init(phydev);
+
+ return 0;
+}
+
static struct phy_driver ksphy_driver[] = {
{
.phy_id = PHY_ID_KS8737,
@@ -1892,10 +2937,9 @@ static struct phy_driver ksphy_driver[] = {
.phy_id_mask = MICREL_PHY_ID_MASK,
.name = "Microchip INDY Gigabit Quad PHY",
.config_init = lan8814_config_init,
- .driver_data = &ksz9021_type,
- .probe = kszphy_probe,
+ .probe = lan8814_probe,
.soft_reset = genphy_soft_reset,
- .read_status = ksz9031_read_status,
+ .read_status = lan8814_read_status,
.get_sset_count = kszphy_get_sset_count,
.get_strings = kszphy_get_strings,
.get_stats = kszphy_get_stats,
--
2.17.1