PROCESSOR-SDK-AM335X: ptp phy driver debug for motorcomm 1000M phy which supports 1588v2 & gptp(802.1as)

// SPDX-License-Identifier: GPL-2.0+
/*
 * drivers/net/phy/motorcomm.c
 *
 * Driver for Motorcomm PHYs
 *
 * Author: Jie Han<jie.han@motor-comm.com>
 *
 * Copyright (c) 2025 Motorcomm, Inc.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 *
 * Support Motorcomm Automotive Phys:
 * 1000M Phys: YT8011
 */
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/phy.h>
#ifndef LINUX_VERSION_CODE
#include <linux/version.h>
#else
#define KERNEL_VERSION(a, b, c) (((a) << 16) + ((b) << 8) + (c))
#endif

#include <linux/netdevice.h>
#include <linux/debugfs.h>
#include <linux/uaccess.h>
#include <linux/string.h>
#include <linux/ethtool.h>
#include <linux/list.h>
#include <linux/mii.h>
#include <linux/net_tstamp.h>
#include <linux/ptp_classify.h>
#include <linux/ptp_clock_kernel.h>
#include <linux/math64.h>
//#include <linux/skbuff.h>
//#include <linux/net.h>      // sock_put()
//#include <linux/socket.h>   // SCM_TSTAMP_SND
//#include <linux/skbuff.h>   // __skb_complete_tx_timestamp()
#include <net/sock.h>
//#include <linux/compiler-attributes.h>	//fallthrough
#include <linux/rtc.h>
#include <linux/time64.h>

//#include <linux/sock.h>
//debug (wrapper to unify log format)
#define DEBUG_PTP
#if defined DEBUG_PTP
#define dbg_ptp(fmt, ...) pr_info(fmt, ##__VA_ARGS__)
#else
#define dbg_ptp(fmt, ...)
#endif
	
#define YTPHY_LINUX_VERSION "2.2.45591"


#define MODULE_NAME "yt"
#define MOTORCOMM_PHY_ID_MASK		0xffffffff

#define PHY_ID_YT8011			0x4f51eb01

#define REG_PHY_SPEC_STATUS		0x11
#define REG_DEBUG_ADDR_OFFSET		0x1e
#define REG_DEBUG_DATA			0x1f
#define REG_MII_MMD_CTRL		0x0D
#define REG_MII_MMD_DATA		0x0E

#define YTXXXX_SPEED_MODE		0xc000
#define YTXXXX_SPEED_MODE_BIT		14
#define YTXXXX_DUPLEX_BIT		13
#define YTXXXX_LINK_STATUS_BIT		10

#define YTPHY_UTP_INTR_REG		0x12
#define YTPHY_UTP_INTR_STATUS_REG	0x13
#define YTPHY_INTR_LINK_STATUS		(BIT(11) | BIT(10))
#define YTPHY_REG_SPACE_UTP		0

#define YT801X_REG_SMI_MUX		0x9000
enum GPTP_ROLE {
	SLAVE = 0,
	MASTER
};

struct yt8xxx_priv {
	u8 chip_mode;
	u8 role;
	struct yt_ptp_private *ptp_priv;
};

enum yt8011_reg_space_type {
	YT8011_REG_SPACE_UTP,
	YT8011_REG_SPACE_SDS,
};

/* ext reg 0x8000 bit1:0 PTP_CLK_TYPE
 * 2'b00 Ordinary/boundary two-step clock;
 * 2'b01 Ordinary/boundary one-step clock;
 * 2'b10 Transparent two-step clock;
 * 2'b11 Transparent one-step clock;
 */
enum GPTP_CLK_MODE {
	OC_2_STEPS = 0,
	OC_1_STEPS,
	TC_2_STEPS,
	TC_1_STEPS
};

/* ext reg 0x8000 bit3:2 RTC_CLK_SEL
 * 2'b00: ADC clock, after rotator;
 * 2'b01: DAC clock;
 * 2'b10: ADC clock, before rotator;
 * 2'b11: SYNC_IN;
 */
enum CLK_SOURCE {
	ADC_CLK_AFTER_ROTATOR = 0,
	DAC_CLK,
	ADC_CLK_BEFORE_ROTATOR,
	SYNC_IN
};
#define VLAN_HLEN 4

#define SYNC                  0x0
#define DELAY_REQ             0x1
#define PDELAY_REQ            0x2
#define PDELAY_RESP           0x3
#define FOLLOW_UP             0x8
#define DELAY_RESP            0x9
#define PDELAY_RESP_FOLLOW_UP 0xA
#define ANNOUNCE              0xB
#define SIGNALING             0xC
#define MANAGEMENT            0xD

static int yt8011_select_page(struct phy_device *phydev, int page);
static int yt8011_restore_page(struct phy_device *phydev, int oldpage, int ret);
static long yt8011_ptp_do_aux_work(struct ptp_clock_info *clock_info);

#if (KERNEL_VERSION(5, 5, 0) > LINUX_VERSION_CODE)
static inline void phy_lock_mdio_bus(struct phy_device *phydev)
{
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	mutex_lock(&phydev->bus->mdio_lock);
#else
	mutex_lock(&phydev->mdio.bus->mdio_lock);
#endif
}

static inline void phy_unlock_mdio_bus(struct phy_device *phydev)
{
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	mutex_unlock(&phydev->bus->mdio_lock);
#else
	mutex_unlock(&phydev->mdio.bus->mdio_lock);
#endif
}
#endif

#if (KERNEL_VERSION(4, 16, 0) > LINUX_VERSION_CODE)
static inline int __phy_read(struct phy_device *phydev, u32 regnum)
{
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	struct mii_bus *bus = phydev->bus;
	int addr = phydev->addr;
	return bus->read(bus, phydev->addr, regnum);
#else
	struct mii_bus *bus = phydev->mdio.bus;
	int addr = phydev->mdio.addr;
#endif
	return bus->read(bus, addr, regnum);
}

static inline int __phy_write(struct phy_device *phydev, u32 regnum, u16 val)
{
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	struct mii_bus *bus = phydev->bus;
	int addr = phydev->addr;
#else
	struct mii_bus *bus = phydev->mdio.bus;
	int addr = phydev->mdio.addr;
#endif
	return bus->write(bus, addr, regnum, val);
}
#endif

static int __ytphy_read_ext(struct phy_device *phydev,
						    u32 regnum)
{
	int ret;

	ret = __phy_write(phydev, REG_DEBUG_ADDR_OFFSET, regnum);
	if (ret < 0)
		return ret;

	return __phy_read(phydev, REG_DEBUG_DATA);
}

static int ytphy_read_ext(struct phy_device *phydev, u32 regnum)
{
	int ret;

	phy_lock_mdio_bus(phydev);
	ret = __phy_write(phydev, REG_DEBUG_ADDR_OFFSET, regnum);
	if (ret < 0)
		goto err_handle;

	ret = __phy_read(phydev, REG_DEBUG_DATA);
	if (ret < 0)
		goto err_handle;

err_handle:
	phy_unlock_mdio_bus(phydev);

	return ret;
}

static int ytphy_write_ext(struct phy_device *phydev, u32 regnum, u16 val)
{
	int ret;

	phy_lock_mdio_bus(phydev);
	ret = __phy_write(phydev, REG_DEBUG_ADDR_OFFSET, regnum);
	if (ret < 0)
		goto err_handle;

	ret = __phy_write(phydev, REG_DEBUG_DATA, val);
	if (ret < 0)
		goto err_handle;

err_handle:
	phy_unlock_mdio_bus(phydev);

	return ret;
}

static int __ytphy_write_ext(struct phy_device *phydev,
						     u32 regnum, u16 val)
{
	int ret;

	ret = __phy_write(phydev, REG_DEBUG_ADDR_OFFSET, regnum);
	if (ret < 0)
		return ret;

	ret = __phy_write(phydev, REG_DEBUG_DATA, val);
	if (ret < 0)
		return ret;

	return 0;
}

static int ytphy_read_mmd(struct phy_device* phydev,
						  u16 device, u16 reg)
{
	int val;

	phy_lock_mdio_bus(phydev);

	__phy_write(phydev, REG_MII_MMD_CTRL, device);
	__phy_write(phydev, REG_MII_MMD_DATA, reg);
	__phy_write(phydev, REG_MII_MMD_CTRL, device | 0x4000);
	val = __phy_read(phydev, REG_MII_MMD_DATA);
	if (val < 0) {
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
		dev_err(&phydev->dev, "error read mmd device(%u) reg (%u)\n",
			device, reg);
#else
		dev_err(&phydev->mdio.dev,
			"error read mmd device(%u) reg (%u)\n", device, reg);
#endif

		goto err_handle;
	}

err_handle:
	phy_unlock_mdio_bus(phydev);

	return val;
}

__attribute__((unused)) static int ytphy_write_mmd(struct phy_device* phydev,
						   u16 device, u16 reg,
						   u16 value)
{
	int ret = 0;

	phy_lock_mdio_bus(phydev);

	__phy_write(phydev, REG_MII_MMD_CTRL, device);
	__phy_write(phydev, REG_MII_MMD_DATA, reg);
	__phy_write(phydev, REG_MII_MMD_CTRL, device | 0x4000);
	__phy_write(phydev, REG_MII_MMD_DATA, value);

	phy_unlock_mdio_bus(phydev);

	return ret;
}

__attribute__((unused)) static int __ytphy_read_mmd(struct phy_device* phydev,
						    u16 device, u16 reg)
{
	int val;

	__phy_write(phydev, REG_MII_MMD_CTRL, device);
	__phy_write(phydev, REG_MII_MMD_DATA, reg);
	__phy_write(phydev, REG_MII_MMD_CTRL, device | 0x4000);
	val = __phy_read(phydev, REG_MII_MMD_DATA);

	return val;
}

__attribute__((unused)) static int __ytphy_write_mmd(struct phy_device* phydev,
						     u16 device, u16 reg,
						     u16 value)
{
	__phy_write(phydev, REG_MII_MMD_CTRL, device);
	__phy_write(phydev, REG_MII_MMD_DATA, reg);
	__phy_write(phydev, REG_MII_MMD_CTRL, device | 0x4000);
	__phy_write(phydev, REG_MII_MMD_DATA, value);

	return 0;
}

static int __ytphy_soft_reset(struct phy_device *phydev)
{
	int ret = 0, val = 0;

	val = __phy_read(phydev, MII_BMCR);
	if (val < 0)
		return val;

	ret = __phy_write(phydev, MII_BMCR, val | BMCR_RESET);
	if (ret < 0)
		return ret;

	return ret;
}

struct yt_ptp_private {
	struct phy_device *phydev;
#if (KERNEL_VERSION(5, 5, 19) < LINUX_VERSION_CODE)
	struct mii_timestamper mii_ts;
#endif
	struct ptp_clock *clock;
	struct ptp_clock_info clock_info;
	struct mutex mutex;
	struct sk_buff_head tx_queue;
	int tx_type;	//ioctl()
	bool hwts_rx;	//ioctl()
	int layer;
	int version;
};

struct yt_ptp_skb_cb {
	unsigned long timeout;
	u16 seq_id;
	u8 msgtype;
};

struct yt_ptp_capture {
	ktime_t hwtstamp;
	u16 seq_id;
	u8 msgtype;
};

#if (KERNEL_VERSION(5, 5, 19) < LINUX_VERSION_CODE)
static struct yt_ptp_private *miits2ptppriv(struct mii_timestamper *mii_ts)
{
	return container_of(mii_ts, struct yt_ptp_private, mii_ts);
}
#endif

static struct yt_ptp_private *clkinfo2ptppriv(struct ptp_clock_info *clock_info)
{
	return container_of(clock_info, struct yt_ptp_private, clock_info);
}

static int yt8011_ptp_get_ts_paged(struct phy_device *phydev,
				   struct timespec64 *ts, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	/* ext reg 0x8049 rtc_real_time_i bit63:48(s_h) */
	ret = __ytphy_read_ext(phydev, 0x8049);
	if (ret < 0)
		goto err_restore_page;
	ts->tv_sec = (ret << 16);

	/* ext reg 0x804a rtc_real_time_i bit47:32(s_l) */
	ret = __ytphy_read_ext(phydev, 0x804a);
	if (ret < 0)
		goto err_restore_page;
	ts->tv_sec |= ret;

	/* ext reg 0x804b rtc_real_time_i bit31:16(ns_h) */
	ret = __ytphy_read_ext(phydev, 0x804b);
	if (ret < 0)
		goto err_restore_page;
	ts->tv_nsec = (ret << 16);

	/* ext reg 0x804c rtc_real_time_i bit15:0(ns_l) */
	ret = __ytphy_read_ext(phydev, 0x804c);
	if (ret < 0)
		goto err_restore_page;
	ts->tv_nsec |= ret;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

static int yt8011_ptp_set_ts_paged(struct phy_device *phydev,
				     const struct timespec64 *ts, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	/* ext reg 0x8062(sec) RTC_PRELOAD_VAL bit63:48 */
	ret = __ytphy_write_ext(phydev, 0x8062, (ts->tv_sec >> 16) & 0xffff);
	if (ret < 0)
		goto err_restore_page;
	/* ext reg 0x8063(sec) RTC_PRELOAD_VAL bit47:32 */
	ret = __ytphy_write_ext(phydev, 0x8063, ts->tv_sec & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	/* ext reg 0x8064(nsec) RTC_PRELOAD_VAL bit31:16 */
	ret = __ytphy_write_ext(phydev, 0x8064, (ts->tv_nsec >> 16) & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	/* ext reg 0x8065(nsec) RTC_PRELOAD_VAL bit15:0 */
	ret = __ytphy_write_ext(phydev, 0x8065, ts->tv_nsec & 0xffff);
	if (ret < 0)
		goto err_restore_page;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

static int yt8011_ptp_rtc_load_paged(struct phy_device *phydev, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	/* 
	 * YT8011 load RTC val, ext reg 0x8060 bit0 1'b1
	 * Setting it to 1 loads value 8061~8065 RTC_PRELOAD to RTC.
	 * This bit is self-clearing and always read back as 0.
	 */
	ret = __ytphy_read_ext(phydev, 0x8060);
	if (ret < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x8060, ret | BIT(0));
	if (ret < 0)
		goto err_restore_page;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

#if (KERNEL_VERSION(4, 20, 17) < LINUX_VERSION_CODE)
static int yt8011_ptp_gettimex(struct ptp_clock_info *clock_info,
			       struct timespec64 *ts,
			       struct ptp_system_timestamp *sts)
{
	struct yt_ptp_private *ptp_priv = clkinfo2ptppriv(clock_info);
	int err;

	mutex_lock(&ptp_priv->mutex);
	err = yt8011_ptp_get_ts_paged(ptp_priv->phydev, ts,
				      YT8011_REG_SPACE_UTP);
	mutex_unlock(&ptp_priv->mutex);

	return err < 0 ? err : 0;
}
#else
static int yt8011_ptp_gettime(struct ptp_clock_info *clock_info,
			      struct timespec64 *ts)
{
	struct yt_ptp_private *ptp_priv = clkinfo2ptppriv(clock_info);
	int err;

	mutex_lock(&ptp_priv->mutex);
	err = yt8011_ptp_get_ts_paged(ptp_priv->phydev, ts,
				      YT8011_REG_SPACE_UTP);
	mutex_unlock(&ptp_priv->mutex);

	return err < 0 ? err : 0;
}
#endif

static int yt8011_ptp_settime(struct ptp_clock_info *clock_info,
			      const struct timespec64 *ts)
{
	struct yt_ptp_private *ptp_priv = clkinfo2ptppriv(clock_info);
	int err;

	mutex_lock(&ptp_priv->mutex);
	err = yt8011_ptp_set_ts_paged(ptp_priv->phydev, ts,
				      YT8011_REG_SPACE_UTP);
	if (err < 0)
		goto out;

	err = yt8011_ptp_rtc_load_paged(ptp_priv->phydev,
					YT8011_REG_SPACE_UTP);
out:
	mutex_unlock(&ptp_priv->mutex);

	return err < 0 ? err : 0;
}

static int yt8011_ptp_adjtime_paged(struct phy_device *phydev,
				    struct timespec64* ts, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	/* ext reg 0x8045 OFFSET_SEC bit31:16 */
	ret = __ytphy_write_ext(phydev, 0x8045, (ts->tv_sec >> 16) & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	/* ext reg 0x8046 OFFSET_SEC bit15:0 */
	ret = __ytphy_write_ext(phydev, 0x8046, ts->tv_sec & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	/* ext reg 0x8042 OFFSET_NANO bit31:16 */
	ret = __ytphy_write_ext(phydev, 0x8042, (ts->tv_nsec >> 16) & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	/* ext reg 0x8043 OFFSET_NANO bit15:0 */
	ret = __ytphy_write_ext(phydev, 0x8043, ts->tv_nsec & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	/* ext reg 0x8047 EN_OFFSET bit0 
	 * 1'b1 adjust the RTC with the value OFFSET_SEC and OFFSET_NANO.
	 */
	ret = __ytphy_read_ext(phydev, 0x8047);
	if (ret < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x8047, ret | BIT(0));
	if (ret < 0)
		goto err_restore_page;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

static int yt8011_ptp_adjtime(struct ptp_clock_info *clock_info, s64 delta_ns)
{
	struct yt_ptp_private *ptp_priv = clkinfo2ptppriv(clock_info);
	struct phy_device *phydev = ptp_priv->phydev;
	struct timespec64 ts;
	int err;

	ts = ns_to_timespec64(delta_ns);
	mutex_lock(&ptp_priv->mutex);
	err = yt8011_ptp_adjtime_paged(phydev, &ts, YT8011_REG_SPACE_UTP);
	mutex_unlock(&ptp_priv->mutex);

	return err < 0 ? err : 0;
}

static int yt8011_ptp_adjfine_paged(struct phy_device *phydev,
				    unsigned long adj_step, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;
	/*
	 * ext reg 0x8040 RTC_STEP bit31:16
	 * ext reg 0x8041 RTC_STEP bit15:0
	 */
	ret = __ytphy_write_ext(phydev, 0x8040, (adj_step >> 16) & 0xffff);
	if (ret < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x8041, adj_step & 0xffff);
	if (ret < 0)
		goto err_restore_page;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

/* rtc clk 375MHz, rtc step = 1 000 000 000 / 375 000 000 = 2.66ns
 * fractional part(0.66 << 26 = 0x2AA AAAA), integer part(2 << 26 = 0x800 0000)
 * so, the frequency adjustment base is 0xAAA AAAA(0x2AA AAAA | 0x800 0000),
 * or 2.6666667 * (2^26) = (2.6666667 << 26)
 *
 * Frequency adjustment is
 * adj = {scaled_ppm * 0xAAAAAAA} / (10^6 * 2^16)
 * adj = {scaled_ppm * [2.6666667 * (2^26)]} / (10^6 * 2^16)
 * adj = {scaled_ppm * [2.6666667 * (2^26)]} / ((2 * 5)^6 * 2^16)
 * adj = {scaled_ppm * [2.6666667 * (2^26)]} / (5^6 * 2^22)
 * adj = {scaled_ppm * [2.6666667 * (2^4)]} / (5^6)
 * adj = {scaled_ppm * [2.6666667 * (2^4)]} / 15625
 * scaled_ppm: long(unit: ppm) and Q16.16 format
 * #define RTC_DEFAULT_CYCLE_RATE 0xAAAAAAA(2.6666667*(2^26))
 * double rate = (1000000 + scaled_ppm) / 10^6;
 * double rate = (1000000 + scaled_ppm) / 1000000;
 * double step = ((double)RTC_DEFAULT_CYCLE_RATE / 2^26) * rate;
 * double step = ((double)RTC_DEFAULT_CYCLE_RATE / 0x4000000) * rate;
 * unsigned long adj_step = (unsigned long)(step << 26 = step * 2^26);
 * unsigned long adj_step = (unsigned long)(step * 0x4000000);
 */
static int yt8011_ptp_adjfine(struct ptp_clock_info *clock_info,
			      long scaled_ppm)
{
#define RTC_DEFAULT_CYCLE_RATE 0xAAAAAAA
	struct yt_ptp_private *ptp_priv = clkinfo2ptppriv(clock_info);
	unsigned long adj_step;
	int neg_adj = 0;
	u32 diff;
	u64 adj;

	dbg_ptp("@jie.han %s, %s, %d scaled_ppm: %ld\n",
		__FILE__, __func__, __LINE__, scaled_ppm);

	if (scaled_ppm < 0) {
		neg_adj = 1;
		scaled_ppm = -scaled_ppm;
	}

	//adj = (scaled_ppm * 266667LL) / 100000LL;
	adj = (u64)scaled_ppm * 266667LL;
	adj = div64_u64(adj, 100000LL);
	adj <<= 4;
	diff = div_u64(adj, 15625);
	//diff = div64_u64(adj, 15625);
	adj_step = RTC_DEFAULT_CYCLE_RATE + (neg_adj ? -diff : diff);
	mutex_lock(&ptp_priv->mutex);
	yt8011_ptp_adjfine_paged(ptp_priv->phydev, adj_step,
				 YT8011_REG_SPACE_UTP);
	mutex_unlock(&ptp_priv->mutex);

	return 0;
}

#define YT_SKB_CB(skb)		((struct yt_ptp_skb_cb *)(skb)->cb)
static const struct ptp_clock_info yt8011_ptp_clock_info = {
	.owner		= THIS_MODULE,
	.name		= KBUILD_MODNAME,
	.max_adj	= 100000000,
#if (KERNEL_VERSION(4, 20, 17) < LINUX_VERSION_CODE)
	.gettimex64	= yt8011_ptp_gettimex,
#else
	.gettime64	= yt8011_ptp_gettime,
#endif
	.settime64	= yt8011_ptp_settime,
	.adjtime	= yt8011_ptp_adjtime,
	.adjfine	= yt8011_ptp_adjfine,
	.do_aux_work	= yt8011_ptp_do_aux_work,
};

static int yt_ptp_get_txtstamp_paged(struct phy_device *phydev,
				     struct yt_ptp_capture *capts, u8 page)
{
	int ret = 0, oldpage;
	uint32_t sec, nsec;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	ret = __ytphy_read_ext(phydev, 0x8020);
	if (ret < 0)
		goto err_restore_page;
	capts->seq_id = ret;

	ret = (__ytphy_read_ext(phydev, 0x802b) & 0xf000) >> 12;
	if (ret < 0)
		goto err_restore_page;
	capts->msgtype = ret;

	ret = __ytphy_read_ext(phydev, 0x8027);
	if (ret < 0)
		goto err_restore_page;
	sec = ret << 16;

	ret = __ytphy_read_ext(phydev, 0x8028);
	if (ret < 0)
		goto err_restore_page;
	sec |= ret;

	ret = __ytphy_read_ext(phydev, 0x8029);
	if (ret < 0)
		goto err_restore_page;
	nsec = ret << 16;

	ret = __ytphy_read_ext(phydev, 0x802a);
	if (ret < 0)
		goto err_restore_page;
	nsec |= ret;

	capts->hwtstamp = ktime_set(sec, nsec);
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret < 0 ? ret : 0);
}

static bool yt8011_ptp_get_txtstamp(struct yt_ptp_private *ptp_priv,
				    struct yt_ptp_capture *capts)
{
	struct phy_device *phydev = ptp_priv->phydev;
	int ret;

	//dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	mutex_lock(&ptp_priv->mutex);
	ret = yt_ptp_get_txtstamp_paged(phydev, capts, YT8011_REG_SPACE_UTP);
	mutex_unlock(&ptp_priv->mutex);
	dbg_ptp("@jie.han %s, %s, %d yt_ptp_get_txtstamp_paged() ret: %d, "
		"capts->seq_id: %d, capts->msgtype: %s\n",
		__FILE__, __func__, __LINE__, ret,
		capts->seq_id,
		capts->msgtype == SYNC ? "SYNC" :
		capts->msgtype == DELAY_REQ ? "DELAY_REQ" :
		capts->msgtype == PDELAY_REQ ? "PDELAY_REQ" :
		capts->msgtype == PDELAY_RESP ? "PDELAY_RESP" :
		capts->msgtype == FOLLOW_UP ? "FOLLOW_UP" :
		capts->msgtype == DELAY_RESP ? "DELAY_RESP" :
		capts->msgtype == PDELAY_RESP_FOLLOW_UP ? "PDELAY_RESP_FOLLOW_UP" :
		capts->msgtype == ANNOUNCE ? "ANNOUNCE" :
		capts->msgtype == SIGNALING ? "SIGNALING" :
		capts->msgtype == MANAGEMENT ? "MANAGEMENT" : "other");
	return (ret < 0 ? false : true);
}

static bool yt8011_ptp_match_txtstamp(struct yt_ptp_private *ptp_priv,
				      struct yt_ptp_capture *capts)
{
	//struct skb_shared_hwtstamps hwts;
	struct skb_shared_hwtstamps *shhwtstamps;
	//struct sk_buff *skb, *ts_skb;
	unsigned long now = jiffies;
	struct sk_buff *skb, *skb_tmp;
	bool matched = false;
	unsigned long flags;
	//bool first = false;
	u8 msgtype;
	u16 seqid;

	//dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	//ts_skb = NULL;
	spin_lock_irqsave(&ptp_priv->tx_queue.lock, flags);
	skb_queue_walk_safe(&ptp_priv->tx_queue, skb, skb_tmp) {
		dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_flags: %#lx\n",
			__FILE__, __func__, __LINE__, skb->sk->sk_flags);
		//dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_refcnt: %d\n",
			//__FILE__, __func__, __LINE__, atomic_read(&skb->sk->sk_refcnt));	//compile err
			//__FILE__, __func__, __LINE__, refcount_read(&skb->sk->sk_refcnt));//compile err
			//__FILE__, __func__, __LINE__, skb->sk->sk_refcnt.refs.counter);
		dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_refcnt: %d\n",
			__FILE__, __func__, __LINE__, skb->sk->sk_refcnt.refs.counter);
		if (time_after(now, YT_SKB_CB(skb)->timeout)) {
			dbg_ptp("@jie.han %s, %s, %d time_after(skb) timeout, then free it.\n",
				__FILE__, __func__, __LINE__);
			__skb_unlink(skb, &ptp_priv->tx_queue);
			if (skb->sk)
				sock_put(skb->sk);
			kfree_skb(skb); 		
			continue;
		}

		seqid = YT_SKB_CB(skb)->seq_id;
		msgtype = YT_SKB_CB(skb)->msgtype;
		if (seqid == capts->seq_id && msgtype == capts->msgtype) {
			__skb_unlink(skb, &ptp_priv->tx_queue);
			dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_flags: %#lx\n",
				__FILE__, __func__, __LINE__, skb->sk->sk_flags);

			shhwtstamps = skb_hwtstamps(skb);
			memset(shhwtstamps, 0, sizeof(*shhwtstamps));
			shhwtstamps->hwtstamp = capts->hwtstamp;

			//skb->tstamp = ktime_to_ns(capts->hwtstamp);
			skb->tstamp = ktime_get_real();
			skb->ip_summed = CHECKSUM_UNNECESSARY;
			
			dbg_ptp("@jie.han %s, %s, %d matched, seqid: %d, msgtype: %s\n",
				__FILE__, __func__, __LINE__,
				seqid,
				msgtype == SYNC ? "SYNC" :
				msgtype == DELAY_REQ ? "DELAY_REQ" :
				msgtype == PDELAY_REQ ? "PDELAY_REQ" :
				msgtype == PDELAY_RESP ? "PDELAY_RESP" :
				msgtype == FOLLOW_UP ? "FOLLOW_UP" :
				msgtype == DELAY_RESP ? "DELAY_RESP" :
				msgtype == PDELAY_RESP_FOLLOW_UP ? "PDELAY_RESP_FOLLOW_UP" :
				msgtype == ANNOUNCE ? "ANNOUNCE" :
				msgtype == SIGNALING ? "SIGNALING" :
				msgtype == MANAGEMENT ? "MANAGEMENT" : "other");

			if (skb->sk) {
				//__skb_complete_tx_timestamp(skb, skb->sk,
				//			SCM_TSTAMP_SND, false);
				//sock_put(skb->sk);
				dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_flags: %#lx\n",
					__FILE__, __func__, __LINE__, skb->sk->sk_flags);
				memset(YT_SKB_CB(skb), 0x0, sizeof(struct yt_ptp_skb_cb));
				skb_complete_tx_timestamp(skb, shhwtstamps);
			} else
				kfree_skb(skb);

			matched = true;
			break;
		}
	}

	spin_unlock_irqrestore(&ptp_priv->tx_queue.lock, flags);

	return matched;
}

void yt8011_ptp_purge_tx_queue(struct yt_ptp_private *ptp_priv)
{
	struct sk_buff *skb, *tmp;
	unsigned long flags;

	spin_lock_irqsave(&ptp_priv->tx_queue.lock, flags);

	skb_queue_walk_safe(&ptp_priv->tx_queue, skb, tmp) {
		__skb_unlink(skb, &ptp_priv->tx_queue);

		if (skb->sk)
			sock_put(skb->sk);

		kfree_skb(skb);
	}

	spin_unlock_irqrestore(&ptp_priv->tx_queue.lock, flags);
}

static long yt8011_ptp_do_aux_work(struct ptp_clock_info *clock_info)
{
	struct yt_ptp_private *ptp_priv = clkinfo2ptppriv(clock_info);
	struct yt_ptp_capture capts;
	bool reschedule = false;
	int limit = 8;
	
	memset(&capts, 0x0, sizeof(capts));
	//dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	/* ref to bcm and nxp in kernel v6.9 */
	while (!skb_queue_empty_lockless(&ptp_priv->tx_queue) && limit--) {
		if (!yt8011_ptp_get_txtstamp(ptp_priv, &capts)) {
			reschedule = true;
			break;
		}
		if (!yt8011_ptp_match_txtstamp(ptp_priv, &capts)) {		
			pr_warn("@jie.han %s, %s, %d "
				"failed to match TX timestamp, msgtype = 0x%x, seqid = %d\n",
				__FILE__, __func__, __LINE__, capts.msgtype, capts.seq_id);
			//skb_queue_purge(&ptp_priv->tx_queue);
			yt8011_ptp_purge_tx_queue(ptp_priv);
			reschedule = true;
			break;
		}
	}

	/* delay of the next auxiliary work scheduling time (>=0) or
	 * negative value in case further scheduling is not required.
	 */
	dbg_ptp("@jie.han %s, %s, %d reschedule: %s\n",
		__FILE__, __func__, __LINE__, reschedule == true ? "true" : "false");
	return reschedule ? 1 : -1;
}

static int yt8011_ptp_get_rxtstamp_paged(struct phy_device *phydev,
					 u8 eventtype, u16 seqid, 
					 u32 *sec, u32 *nsec,
					 u8 page)
{
	int ret = 0, oldpage;
	uint16_t id;
	int type;
	int idx;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	for (idx = 0; idx < 4; idx++) {
		if (idx == 0) {
			ret = __ytphy_read_ext(phydev, 0x8010);
			if (ret < 0)
				goto err_restore_page;
			id = ret;
			ret = __ytphy_read_ext(phydev, 0x801b);
			if (ret < 0)
				goto err_restore_page;
			type = (ret & 0xf000) >> 12;
			if (id == seqid && type == eventtype) {
				ret = __ytphy_read_ext(phydev, 0x8017);
				if (ret < 0)
					goto err_restore_page;
				*sec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x8018);
				if (ret < 0)
					goto err_restore_page;
				*sec |= ret;

				ret = __ytphy_read_ext(phydev, 0x8019);
				if (ret < 0)
					goto err_restore_page;
				*nsec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x801a);
				if (ret < 0)
					goto err_restore_page;
				*nsec |= ret;
				break;
			}
		} else if (idx == 1) {
			ret = __ytphy_read_ext(phydev, 0x8100);
			if (ret < 0)
				goto err_restore_page;
			id = ret;
			ret = __ytphy_read_ext(phydev, 0x810b);
			if (ret < 0)
				goto err_restore_page;
			type = (ret & 0xf000) >> 12;
			if (id == seqid && type == eventtype) {
				ret = __ytphy_read_ext(phydev, 0x8107);
				if (ret < 0)
					goto err_restore_page;
				*sec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x8108);
				if (ret < 0)
					goto err_restore_page;
				*sec |= ret;

				ret = __ytphy_read_ext(phydev, 0x8109);
				if (ret < 0)
					goto err_restore_page;
				*nsec = ret << 16;

				ret= __ytphy_read_ext(phydev, 0x810a);
				if (ret < 0)
					goto err_restore_page;
				*nsec |= ret;
				break;
			}
		} else if (idx == 2) {
			ret = __ytphy_read_ext(phydev, 0x8110);
			if (ret < 0)
				goto err_restore_page;
			ret = __ytphy_read_ext(phydev, 0x811b);
			if (ret < 0)
				goto err_restore_page;
			type = (ret & 0xf000) >> 12;
			if (id == seqid && type == eventtype) {
				ret = __ytphy_read_ext(phydev, 0x8117);
				if (ret < 0)
					goto err_restore_page;
				*sec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x8118);
				if (ret < 0)
					goto err_restore_page;
				*sec |= ret;

				ret = __ytphy_read_ext(phydev, 0x8119);
				if (ret < 0)
					goto err_restore_page;
				*nsec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x811a);
				if (ret < 0)
					goto err_restore_page;
				*nsec |= ret;
				break;
			}
		} else if (idx == 3) {
			ret = __ytphy_read_ext(phydev, 0x8120);
			if (ret < 0)
				goto err_restore_page;
			ret = __ytphy_read_ext(phydev, 0x812b);
			if (ret < 0)
				goto err_restore_page;
			type = (ret & 0xf000) >> 12;
			if (id == seqid && type == eventtype) {
				ret = __ytphy_read_ext(phydev, 0x8127);
				if (ret < 0)
					goto err_restore_page;
				*sec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x8128);
				if (ret < 0)
					goto err_restore_page;
				*sec |= ret;

				ret = __ytphy_read_ext(phydev, 0x8129);
				if (ret < 0)
					goto err_restore_page;
				*nsec = ret << 16;

				ret = __ytphy_read_ext(phydev, 0x812a);
				if (ret < 0)
					goto err_restore_page;
				*nsec |= ret;
				break;
			}
		}
	}
 
	dbg_ptp("@jie.han %s, %s, %d idx: %d\n",
		__FILE__, __func__, __LINE__, idx);
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

#if (KERNEL_VERSION(5, 5, 19) < LINUX_VERSION_CODE)
static bool yt8011_ptp_mii_rxtstamp(struct mii_timestamper *mii_ts,
				    struct sk_buff *skb, int type)
{
	struct yt_ptp_private *ptp_priv = miits2ptppriv(mii_ts);
	struct phy_device *phydev = ptp_priv->phydev;
	struct skb_shared_hwtstamps *hwts;
	struct ptp_header *header;
	u16 sequence_id;
	u32 sec, nsec;
	u8 msg_type;
	int ret;

	dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	if (!ptp_priv->hwts_rx)
		return false;

	header = ptp_parse_header(skb, type);
	if (!header)
		return false;
	msg_type = ptp_get_msgtype(header, type);
	sequence_id = header->sequence_id;

	ret = yt8011_ptp_get_rxtstamp_paged(phydev, msg_type,
					    sequence_id, &sec, &nsec,
					    YT8011_REG_SPACE_UTP);
	if (ret > 0) {
		//get the rx timestamp and assigned skb's hwts->hwtstamp
		hwts = skb_hwtstamps(skb);
		hwts->hwtstamp = ktime_set(sec, nsec);
		netif_rx(skb);
		return true;
	} else
		return false;
}

static void yt8011_ptp_mii_txtstamp(struct mii_timestamper *mii_ts,
				    struct sk_buff *skb, int type)
{
	struct yt_ptp_private *ptp_priv = miits2ptppriv(mii_ts);
	struct ptp_header *hdr;
	int msgtype;

	dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	hdr = ptp_parse_header(skb, type);
	if (!hdr)
		goto out;
	msgtype = ptp_get_msgtype(hdr, type);

	dbg_ptp("@jie.han %s, %s, %d ptp_priv->tx_type: %d\n",
		__FILE__, __func__, __LINE__, ptp_priv->tx_type);
	switch (ptp_priv->tx_type) {
	case HWTSTAMP_TX_ONESTEP_P2P:
	case HWTSTAMP_TX_ONESTEP_SYNC:
	case HWTSTAMP_TX_ON:
		YT_SKB_CB(skb)->timeout = jiffies + SKB_TIMESTAMP_TIMEOUT;
		YT_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id);
		YT_SKB_CB(skb)->msgtype = msgtype;
		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
		skb_queue_tail(&ptp_priv->tx_queue, skb);
		ptp_schedule_worker(ptp_priv->clock, 0);
		return;
	default:
		break;
	}

out:
	kfree_skb(skb);
}

static int yt8011_ptp_mii_hwtstamp(struct mii_timestamper *mii_ts,
				   struct kernel_hwtstamp_config *cfg,
				   struct netlink_ext_ack *extack)
{
	struct yt_ptp_private *ptp_priv = miits2ptppriv(mii_ts);
	struct yt8xxx_priv *priv;
	u16 cfg0 = 0, cfg1 = 0;

	dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	priv = container_of(ptp_priv, struct yt8xxx_priv, ptp_priv);
	switch (cfg.rx_filter) {
	case HWTSTAMP_FILTER_NONE:
	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
		ptp_priv->hwts_rx = 0;
		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->hwts_rx = 1;
		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->hwts_rx = 1;
		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->hwts_rx = 1;
		ptp_priv->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
		ptp_priv->version = PTP_CLASS_V2;
		break;
	default:
		return -ERANGE;
	}
	ptp_priv->tx_type = cfg->tx_type;

	cfg0 |= OC_2_STEPS;

	if (priv->role == MASTER)
		cfg0 |= (DAC_CLK << 2);
	else
		cfg0 |= (ADC_CLK_AFTER_ROTATOR << 2);

	/* ext reg 0x8000 bit4 BP_1588
	 * 1'b0 = PTP timestamp engine normal operation.
	 * 1'b1 = Bypass IEEE1588v2 related functions.	  
	 */
	cfg0 &= ~BIT(4);
	
	/* ext reg 0x8000 bit7 EN_GATE_1588
	 * 1'b0 = Disable gating 1588 clock domain.
	 * 1'b1 = Enable gating 1588 clock domain.
	 */
	cfg0 &= ~BIT(7);

	/* ext reg 0x8001 bit4
	 * 1'b1: PTP mode  1'b0:gPTP mode(802.1AS)
	 */
	if (ptp_priv->layer & PTP_CLASS_L4)
		cfg1 |= BIT(4);
	else
		cfg1 &= ~BIT(4);
	mutex_lock(&ptp_priv->mutex);
	yt8011_ptp_init_paged(ptp_priv->phydev, cfg0, cfg1,
			      YT8011_REG_SPACE_UTP);

	mutex_unlock(&ptp_priv->mutex);

	/* purge existing data */
	skb_queue_purge(&priv->tx_queue);

	return 0;
}

static int yt8011_ptp_mii_ts_info(struct mii_timestamper *mii_ts,
				  struct ethtool_ts_info *ts_info)
{
	struct yt_ptp_private *ptp_priv = miits2ptppriv(mii_ts);

	dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	ts_info->phc_index = ptp_clock_index(ptp_priv->clock);
	ts_info->so_timestamping =	//software and hardware ts support
		SOF_TIMESTAMPING_TX_SOFTWARE |
		SOF_TIMESTAMPING_RX_SOFTWARE |
		SOF_TIMESTAMPING_SOFTWARE |
		SOF_TIMESTAMPING_TX_HARDWARE |
		SOF_TIMESTAMPING_RX_HARDWARE |
		SOF_TIMESTAMPING_RAW_HARDWARE;

	//v5.15 support HWTSTAMP_TX_ONESTEP_P2P
	ts_info->tx_types =			//hardware ts support only
		BIT(HWTSTAMP_TX_ON) |
		BIT(HWTSTAMP_TX_OFF) |
		//BIT(HWTSTAMP_TX_ONESTEP_SYNC) |
		BIT(HWTSTAMP_TX_ONESTEP_P2P);
	ts_info->rx_filters =		//hardware ts support only
		BIT(HWTSTAMP_FILTER_NONE) |
		BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);

	return 0;
}
#endif

time64_t read_rtc_time_sec(void) {
	struct rtc_device *rtc;
	struct rtc_time tm;
	int ret;
	time64_t seconds = 0;
	//u64 nanoseconds;

	// 1. �� RTC �豸(�����豸��Ϊ "rtc0")
	//https://elixir.bootlin.com/linux/v3.8.13/A/ident/rtc_class_open
	//https://elixir.bootlin.com/linux/v6.15-rc5/A/ident/rtc_class_open
	rtc = rtc_class_open("rtc0");
	if (IS_ERR(rtc)) {
		pr_err("Failed to open RTC device");
		return seconds;
	}

	// 2. ��ȡʱ�䵽 tm �ṹ��
	//https://elixir.bootlin.com/linux/v3.8.13/A/ident/rtc_read_time
	//https://elixir.bootlin.com/linux/v6.15-rc5/A/ident/rtc_read_time
	ret = rtc_read_time(rtc, &tm);
	if (ret) {
		pr_err("Failed to read RTC time");
		goto out;
	}

	// 3. �� rtc_time ת��Ϊ Unix ʱ���(��)
	//https://elixir.bootlin.com/linux/v3.19/A/ident/rtc_tm_to_time64
	//https://elixir.bootlin.com/linux/v6.15-rc5/A/ident/rtc_tm_to_time64
	seconds = rtc_tm_to_time64(&tm);

	// 4. ת��Ϊ����
	//nanoseconds = seconds * NSEC_PER_SEC;

	//pr_info("RTC Time: %lld.%09d seconds since 1970-01-01 00:00:00 UTC",
	//        (long long)seconds, (int)(nanoseconds % NSEC_PER_SEC));

out:
	rtc_class_close(rtc); // �ر��豸
	return seconds;
}

static int yt8011_ptp_init_paged(struct phy_device *phydev,
				 u16 cfg0, u16 cfg1, u8 page)
{
	int ret = 0, oldpage;
	time64_t seconds;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x8000, cfg0);
	if (ret < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x8001, cfg1);
	if (ret < 0)
		goto err_restore_page;

	/* rtc step cfg
	 * rtc clk f = 375MHz, T = 2.6666667ns
	 * rtc step = T << 26 = 0xaaa aaaa
	 * ext reg 0x8040 RTC_STEP
	 * bit31:16 MSB 16bits of part of the RTC step.
	 * ext reg 0x8041 RTC_STEP
	 * bit15:0 LSB 16bits of part of the RTC step.
	 */
	ret = __ytphy_write_ext(phydev, 0x8040, 0x0aaa);
	if (ret < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x8041, 0xaaaa);
	if (ret < 0)
		goto err_restore_page;

	/* rtc cfg
	 * eg, 2023-01-01 00h00m00 s -> 1672531200 s -> 0x63b0 cd00
	 * write ext reg 0x8061(sec) RTC_PRELOAD_VAL bit79:64
	 * write ext reg 0x8062(sec) RTC_PRELOAD_VAL bit63:48
	 * write ext reg 0x8063(sec) RTC_PRELOAD_VAL bit47:32
	 * write ext reg 0x8064(nsec) RTC_PRELOAD_VAL bit31:16
	 * write ext reg 0x8065(nsec) RTC_PRELOAD_VAL bit15:0
	 */
	seconds = read_rtc_time_sec();
	/* s_h */
	ret = __ytphy_write_ext(phydev, 0x8061, 0x0);
	if (ret < 0)
		goto err_restore_page;
	/* s_m */
	//ret = __ytphy_write_ext(phydev, 0x8062, 0x63b0);
	ret = __ytphy_write_ext(phydev, 0x8062, (seconds >> 16) & 0xffff);
	if (ret < 0)
		goto err_restore_page;
	/* s_l */
	//ret = __ytphy_write_ext(phydev, 0x8063, 0xcd00);
	ret = __ytphy_write_ext(phydev, 0x8063, seconds & 0xffff);
	if (ret < 0)
		goto err_restore_page;
	/* ns_h */
	ret = __ytphy_write_ext(phydev, 0x8064, 0x0);
	if (ret < 0)
		goto err_restore_page;
	/* ns_l */
	ret = __ytphy_write_ext(phydev, 0x8065, 0x0);
	if (ret < 0)
		goto err_restore_page;

	/* load rtc
	 * ext reg 0x8060 bit0 LOAD_RTC
	 * 1'b1 loads value 0x8061~0x8065 RTC_PRELOAD to RTC.
	 * This bit is self-clearing and always read back as 0.
	 */
	ret = __ytphy_read_ext(phydev, 0x8060);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x8060, ret | BIT(0));
	if (ret < 0)
		goto err_restore_page;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

struct yt_ptp_private *yt8011_ptp_priv(struct phy_device *phydev)
{
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	struct device *dev = &phydev->dev;
#else
	struct device *dev = &phydev->mdio.dev;
#endif
	struct yt_ptp_private *ptp_priv;
	struct ptp_clock *clock;

	ptp_priv = devm_kzalloc(dev, sizeof(*ptp_priv), GFP_KERNEL);
	if (!ptp_priv)
		return ERR_PTR(-ENOMEM);
		//return -ENOMEM;

	ptp_priv->clock_info = yt8011_ptp_clock_info;

	clock = ptp_clock_register(&ptp_priv->clock_info, dev);
	if (IS_ERR(clock))
		return ERR_CAST(clock);
	ptp_priv->clock = clock;

	ptp_priv->phydev = phydev;
	mutex_init(&ptp_priv->mutex);
	skb_queue_head_init(&ptp_priv->tx_queue);
#if (KERNEL_VERSION(5, 6, 0) > LINUX_VERSION_CODE)
#else
	ptp_priv->mii_ts.rxtstamp = yt8011_ptp_mii_rxtstamp;
	ptp_priv->mii_ts.txtstamp = yt8011_ptp_mii_txtstamp;
	ptp_priv->mii_ts.hwtstamp = yt8011_ptp_mii_hwtstamp;
	ptp_priv->mii_ts.ts_info = yt8011_ptp_mii_ts_info;
	ptp_priv->phydev->mii_ts = &ptp_priv->mii_ts;
#endif	
	return ptp_priv;
}

static int yt8011_probe(struct phy_device *phydev)
{
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	struct device *dev = &phydev->dev;
#else
	struct device *dev = &phydev->mdio.dev;
#endif
	struct yt8xxx_priv *priv;
	int chip_config;
	u16 role;

	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
	if (!priv)
		//return ERR_PTR(-ENOMEM);
		return -ENOMEM;

	phydev->priv = priv;

	/* ext reg 0x9030 bit0 
	 * 0 = chip works in RGMII mode; 1 = chip works in SGMII mode 
	 */
	chip_config = ytphy_read_ext(phydev, 0x9030);
	priv->chip_mode = chip_config & 0x1;

	role = ytphy_read_mmd(phydev, 0x1, 0x834);
	
	priv->role = role & BIT(14) ? MASTER : SLAVE;
	priv->ptp_priv = yt8011_ptp_priv(phydev);
	if (IS_ERR(priv->ptp_priv))
		return PTR_ERR(priv->ptp_priv);
	return 0;
}

static int yt8011_soft_reset_paged(struct phy_device *phydev, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage >= 0)
		ret = __ytphy_soft_reset(phydev);

	return yt8011_restore_page(phydev, oldpage, ret);
}

static int yt8011_soft_reset(struct phy_device *phydev)
{
	struct yt8xxx_priv *priv = phydev->priv;

	/* utp */
	yt8011_soft_reset_paged(phydev, YT8011_REG_SPACE_UTP);

	if (priv->chip_mode) {	/* sgmii */
		yt8011_soft_reset_paged(phydev, YT8011_REG_SPACE_SDS);
	}

	return 0;
}

static int yt8011_config_aneg(struct phy_device *phydev)
{
	return 0;
}

#if (KERNEL_VERSION(3, 14, 79) < LINUX_VERSION_CODE)
static int yt8011_aneg_done(struct phy_device *phydev)
{
	int link_utp = 0;

	/* UTP */
	ytphy_write_ext(phydev, 0x9000, 0);
	link_utp = !!(phy_read(phydev, REG_PHY_SPEC_STATUS) &
			(BIT(YTXXXX_LINK_STATUS_BIT)));

#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	netdev_info(phydev->attached_dev, "%s, phy addr: %d, link_utp: %d\n",
		    __func__, phydev->addr, link_utp);
#else
	netdev_info(phydev->attached_dev, "%s, phy addr: %d, link_utp: %d\n",
		    __func__, phydev->mdio.addr, link_utp);
#endif

	return !!(link_utp);
}
#endif

static int yt8011_config_paged(struct phy_device *phydev, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	ret = __ytphy_write_ext(phydev, 0x1008, 0x2119);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1092, 0x712);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x90bc, 0x7676);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x90b9, 0x620b);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x2001, 0x6418);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1019, 0x3712);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x101a, 0x3713);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x2005, 0x810);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x2015, 0x1012);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x2013, 0xff06);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3017, 0x4);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3027, 0xffe8);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3026, 0x1b58);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x301e, 0xb40);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3019, 0xffd4);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3014, 0x1115);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x301a, 0x7800);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1000, 0x28);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1053, 0xf);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x105e, 0xa46c);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1088, 0x2b);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1088, 0x2b);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x1088, 0xb);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3008, 0x141);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x3009, 0x1918);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x9095, 0x1a1a);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x9096, 0x1a10);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x9097, 0x101a);
	if (ret < 0)
		goto err_restore_page;
	ret = __ytphy_write_ext(phydev, 0x9098, 0x01ff);
	if (ret < 0)
		goto err_restore_page;
err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

__attribute__((unused))
static int yt8011_config_rgmii_dvddio_3v3_paged(struct phy_device *phydev, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	if (page == YT8011_REG_SPACE_SDS) {
		ret = __ytphy_write_ext(phydev, 0x0062, 0x0000);
		if (ret < 0)
			goto err_restore_page;
	} else if (page == YT8011_REG_SPACE_UTP) {
		ret = __ytphy_write_ext(phydev, 0x9031, 0xb200);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x903b, 0x0040);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x903e, 0x3b3b);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x903c, 0xf);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x903d, 0x1000);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9038, 0x0000);
		if (ret < 0)
			goto err_restore_page;
	}

err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

__attribute__((unused))
static int yt8011_config_rgmii_dvddio_2v5_paged(struct phy_device *phydev, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	if (page == YT8011_REG_SPACE_SDS) {
		ret = __ytphy_write_ext(phydev, 0x0062, 0x0000);
		if (ret < 0)
			goto err_restore_page;
	} else if (page == YT8011_REG_SPACE_UTP) {
		ret = __ytphy_write_ext(phydev, 0x9031, 0xb200);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9111, 0x5);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9114, 0x3939);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9112, 0xf);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9110, 0x0);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9113, 0x10);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x903d, 0x2);
		if (ret < 0)
			goto err_restore_page;
	}

err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

__attribute__((unused))
static int yt8011_config_rgmii_dvddio_1v8_paged(struct phy_device *phydev, int page)
{
	int ret = 0, oldpage;

	oldpage = yt8011_select_page(phydev, page);
	if (oldpage < 0)
		goto err_restore_page;

	if (page == YT8011_REG_SPACE_SDS) {
		ret = __ytphy_write_ext(phydev, 0x0062, 0x0000);
		if (ret < 0)
			goto err_restore_page;
	} else if (page == YT8011_REG_SPACE_UTP) {
		ret = __ytphy_write_ext(phydev, 0x9031, 0xb200);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9116, 0x6);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9119, 0x3939);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9117, 0xf);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9115, 0x0);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x9118, 0x20);
		if (ret < 0)
			goto err_restore_page;
		ret = __ytphy_write_ext(phydev, 0x903d, 0x3);
		if (ret < 0)
			goto err_restore_page;
	}

err_restore_page:
	return yt8011_restore_page(phydev, oldpage, ret);
}

/* #define YT801X_RGMII_DVDDIO_3V3 */
/* #define YT801X_RGMII_DVDDIO_2V5 */
/* #define YT801X_RGMII_DVDDIO_1V8 */
static int yt8011_config_init(struct phy_device *phydev)
{
	struct yt8xxx_priv *priv = phydev->priv;
	int ret;

	phydev->autoneg = AUTONEG_DISABLE;

	ret = yt8011_config_paged(phydev, YT8011_REG_SPACE_UTP);
	if (ret < 0)
		return ret;
	if (!(priv->chip_mode)) {	/* rgmii config */
#if defined (YT801X_RGMII_DVDDIO_3V3)
		ret = yt8011_config_rgmii_dvddio_3v3_paged(phydev,
							   YT8011_REG_SPACE_SDS);
		if (ret < 0)
			return ret;
		ret = yt8011_config_rgmii_dvddio_3v3_paged(phydev,
							   YT8011_REG_SPACE_UTP);
		if (ret < 0)
			return ret;
#elif defined (YT801X_RGMII_DVDDIO_2V5)
		ret = yt8011_config_rgmii_dvddio_2v5_paged(phydev,
							   YT8011_REG_SPACE_SDS);
		if (ret < 0)
			return ret;
		ret = yt8011_config_rgmii_dvddio_2v5_paged(phydev,
							   YT8011_REG_SPACE_UTP);
		if (ret < 0)
			return ret;
#elif defined (YT801X_RGMII_DVDDIO_1V8)
		ret = yt8011_config_rgmii_dvddio_1v8_paged(phydev,
							   YT8011_REG_SPACE_SDS);
		if (ret < 0)
			return ret;
		ret = yt8011_config_rgmii_dvddio_1v8_paged(phydev,
							   YT8011_REG_SPACE_UTP);
		if (ret < 0)
			return ret;
#endif
	}

	if (phydev->drv->txtstamp)
		dbg_ptp("@jie.han %s, %s, %d phydev->drv->txtstamp registed.\n",
		__FILE__, __func__, __LINE__);
	else
		dbg_ptp("@jie.han %s, %s, %d phydev->drv->txtstamp not registed.\n",
		__FILE__, __func__, __LINE__);
	if (phydev->drv->rxtstamp)
		dbg_ptp("@jie.han %s, %s, %d phydev->drv->rxtstamp registed.\n",
		__FILE__, __func__, __LINE__);
	else
		dbg_ptp("@jie.han %s, %s, %d phydev->drv->rxtstamp not registed.\n",
		__FILE__, __func__, __LINE__);

	yt8011_soft_reset(phydev);

#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
	netdev_info(phydev->attached_dev, "%s done, phy addr: %d\n",
		    __func__, phydev->addr);
#else
	netdev_info(phydev->attached_dev, "%s done, phy addr: %d\n",
		    __func__, phydev->mdio.addr);
#endif

	return 0;
}

static int ytxxxx_automotive_adjust_status(struct phy_device *phydev, int val)
{
	int speed_mode;
#if (KERNEL_VERSION(4, 0, 0) > LINUX_VERSION_CODE)
	int speed = -1;
#else
	int speed = SPEED_UNKNOWN;
#endif

	speed_mode = (val & YTXXXX_SPEED_MODE) >> YTXXXX_SPEED_MODE_BIT;
	switch (speed_mode) {
	case 1:
		speed = SPEED_100;
		break;
	case 2:
		speed = SPEED_1000;
		break;
	default:
#if (KERNEL_VERSION(4, 0, 0) > LINUX_VERSION_CODE)
		speed = -1;
#else
		speed = SPEED_UNKNOWN;
#endif
		break;
	}

	phydev->speed = speed;
	phydev->duplex = DUPLEX_FULL;

	return 0;
}

static int yt8011_read_status(struct phy_device *phydev)
{
	int ret;
	int val;
	int link;
	int link_utp = 0;

	/* UTP */
	ret = ytphy_write_ext(phydev, 0x9000, 0x0);
	if (ret < 0)
		return ret;

	val = phy_read(phydev, REG_PHY_SPEC_STATUS);
	if (val < 0)
		return val;

	link = val & (BIT(YTXXXX_LINK_STATUS_BIT));
	if (link) {
		link_utp = 1;
		ytxxxx_automotive_adjust_status(phydev, val);
	} else {
		link_utp = 0;
	}

	if (link_utp) {
		if (phydev->link == 0)
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
			netdev_info(phydev->attached_dev,
				    "%s, phy addr: %d, link up, media: UTP, mii reg 0x11 = 0x%x\n",
				    __func__, phydev->addr, (unsigned int)val);
#else
			netdev_info(phydev->attached_dev,
				    "%s, phy addr: %d, link up, media: UTP, mii reg 0x11 = 0x%x\n",
				    __func__, phydev->mdio.addr,
				    (unsigned int)val);
#endif
		phydev->link = 1;
	} else {
		if (phydev->link == 1)
#if (KERNEL_VERSION(4, 5, 0) > LINUX_VERSION_CODE)
			netdev_info(phydev->attached_dev,
				    "%s, phy addr: %d, link down\n",
				    __func__, phydev->addr);
#else
			netdev_info(phydev->attached_dev,
				    "%s, phy addr: %d, link down\n",
				    __func__, phydev->mdio.addr);
#endif
		phydev->link = 0;
	}

	if (link_utp)
		ytphy_write_ext(phydev, 0x9000, 0x0);

	return 0;
}

#if (KERNEL_VERSION(5, 6, 0) > LINUX_VERSION_CODE)
static int yt8011_ptp_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
{
	struct yt8xxx_priv *priv = phydev->priv;
	struct yt_ptp_private *ptp_priv = priv->ptp_priv;
	struct hwtstamp_config cfg;
	u16 cfg0 = 0, cfg1 = 0;
	//u16 role;

	//dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
		return -EFAULT;

	if (cfg.flags) /* reserved for future extensions */
		return -EINVAL;

	if (cfg.tx_type < 0 || cfg.tx_type > HWTSTAMP_TX_ONESTEP_SYNC)
		return -ERANGE;

	ptp_priv->tx_type = cfg.tx_type;

	dbg_ptp("@jie.han %s, %s, %d cfg.rx_filter: %s\n",
		__FILE__, __func__, __LINE__,
		cfg.rx_filter == HWTSTAMP_FILTER_PTP_V2_EVENT ? "HWTSTAMP_FILTER_PTP_V2_EVENT" : "other");
	switch (cfg.rx_filter) {
	case HWTSTAMP_FILTER_NONE:
	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
		ptp_priv->hwts_rx = 0;
		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->hwts_rx = 1;
		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:
		//YT8011 default 802.1as enable
		ptp_priv->hwts_rx = 1;
		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->hwts_rx = 1;
		ptp_priv->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
		ptp_priv->version = PTP_CLASS_V2;
		break;
	default:
		return -ERANGE;
	}

	cfg0 |= OC_2_STEPS;
	dbg_ptp("@jie.han %s, %s, %d role: %s\n",
		__FILE__, __func__, __LINE__,
		priv->role == MASTER ? "MASTER" : "SLAVE");
	if (priv->role == MASTER)
		cfg0 |= (DAC_CLK << 2);
	else
		cfg0 |= (ADC_CLK_AFTER_ROTATOR << 2);

	/* ext reg 0x8000 bit4 BP_1588
	 * 1'b0 = PTP timestamp engine normal operation as default.
	 * 1'b1 = Bypass IEEE1588v2 related functions.
	 */
	cfg0 &= ~BIT(4);
	
	/* ext reg 0x8000 bit7 EN_GATE_1588
	 * 1'b0 = Disable gating 1588 clock domain.
	 * 1'b1 = Enable gating 1588 clock domain as default.
	 */
	cfg0 &= ~BIT(7);

	/* ext reg 0x8001 bit4 PTP_MODE_SEL
	 * 1'b1: PTP mode
	 * 1'b0: gPTP mode(802.1AS) as default
	 */
	if (ptp_priv->layer & PTP_CLASS_L2)
		cfg1 &= ~BIT(4);

	mutex_lock(&ptp_priv->mutex);
	yt8011_ptp_init_paged(phydev, cfg0, cfg1, YT8011_REG_SPACE_UTP);
	mutex_unlock(&ptp_priv->mutex);

	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
}

static int parsing_ptp_msgtype_seqid(struct sk_buff *skb,
				     u8 *msgtype, u16 *seqid, int type)
{
	u8 *data = skb_mac_header(skb);
	unsigned int offset = 0;
	u8 *msg_type;
	//u16 *seq_id;

	//dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	if (type & PTP_CLASS_VLAN)
		offset += VLAN_HLEN;

	switch (type & PTP_CLASS_PMASK) {
	case PTP_CLASS_IPV4:
		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
		break;
	case PTP_CLASS_IPV6:
		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
		break;
	case PTP_CLASS_L2:
		offset += ETH_HLEN;
		break;
	default:
		return 0;
	}

	if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
		return 0;

	if (unlikely(type & PTP_CLASS_V1)) {
		if (offset + OFF_PTP_CONTROL >= skb->len)
			return 0;
		msg_type = data + offset + OFF_PTP_CONTROL;
	} else {
		if (offset >= skb->len)
			return 0;
		msg_type = data + offset;
	}
	//memcpy(msgtype, msg_type, sizeof(*msgtype));
	*msgtype = (*msg_type) & 0xf;

	//seq_id = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
	//memcpy(seqid, seq_id, sizeof(*seqid));
	//*seq_id = ntohs(*(u16 *)(data + offset + OFF_PTP_SEQUENCE_ID));
	*seqid = ntohs(*(u16 *)(data + offset + OFF_PTP_SEQUENCE_ID));
	dbg_ptp("@jie.han %s, %s, %d msgtype: %s, seq_id: %d\n",
		__FILE__, __func__, __LINE__,
		*msgtype == SYNC ? "SYNC" :
		*msgtype == DELAY_REQ ? "DELAY_REQ" :
		*msgtype == PDELAY_REQ ? "PDELAY_REQ" :
		*msgtype == PDELAY_RESP ? "PDELAY_RESP" :
		*msgtype == FOLLOW_UP ? "FOLLOW_UP" :
		*msgtype == DELAY_RESP ? "DELAY_RESP" :
		*msgtype == PDELAY_RESP_FOLLOW_UP ? "PDELAY_RESP_FOLLOW_UP" :
		*msgtype == ANNOUNCE ? "ANNOUNCE" :
		*msgtype == SIGNALING ? "SIGNALING" :
		*msgtype == MANAGEMENT ? "MANAGEMENT" : "others", *seqid);
	return 1;
}

static bool yt8011_ptp_rxtstamp(struct phy_device *phydev,
				struct sk_buff *skb, int type)
{
	struct skb_shared_hwtstamps *shhwtstamps;
	struct yt8xxx_priv *priv = phydev->priv;
	u16 sequence_id;
	u32 sec = 0, nsec = 0;
	u8 msg_type;
	int ret;

	dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	if (!priv->ptp_priv->hwts_rx)
		return false;

	if ((type & priv->ptp_priv->version) == 0 ||
		(type & priv->ptp_priv->layer) == 0)
		return false;

	dbg_ptp("@jie.han %s, %s, %d call parsing_ptp_msgtype_seqid()\n",
		__FILE__, __func__, __LINE__);
	parsing_ptp_msgtype_seqid(skb, &msg_type, &sequence_id, type);
	dbg_ptp("@jie.han %s, %s, %d after parsing_ptp_msgtype_seqid() "
		"msg_type: %s, sequence_id: %d\n",
		__FILE__, __func__, __LINE__,
		msg_type == SYNC ? "SYNC" :
		msg_type == DELAY_REQ ? "DELAY_REQ" :
		msg_type == PDELAY_REQ ? "PDELAY_REQ" :
		msg_type == PDELAY_RESP ? "PDELAY_RESP" :
		msg_type == FOLLOW_UP ? "FOLLOW_UP" :
		msg_type == DELAY_RESP ? "DELAY_RESP" :
		msg_type == PDELAY_RESP_FOLLOW_UP ? "PDELAY_RESP_FOLLOW_UP" :
		msg_type == ANNOUNCE ? "ANNOUNCE" :
		msg_type == SIGNALING ? "SIGNALING" :
		msg_type == MANAGEMENT ? "MANAGEMENT" : "other", sequence_id);
	dbg_ptp("@jie.han %s, %s, %d call yt8011_ptp_get_rxtstamp_paged()\n",
		__FILE__, __func__, __LINE__);
	ret = yt8011_ptp_get_rxtstamp_paged(phydev, msg_type,
					    sequence_id, &sec, &nsec,
					    YT8011_REG_SPACE_UTP);
	if (ret > 0) {
		shhwtstamps = skb_hwtstamps(skb);
		shhwtstamps->hwtstamp = ktime_set(sec, nsec);
		dbg_ptp("@jie.han %s, %s, %d "
			"shhwtstamps->hwtstamp: %lld then call netif_rx()\n",
			__FILE__, __func__, __LINE__, shhwtstamps->hwtstamp);
		netif_rx(skb);
		return true;
	} else
		return false;
}

#define SKB_TIMESTAMP_TIMEOUT	10 /* jiffies */

static void yt8011_ptp_txtstamp(struct phy_device *phydev,
				struct sk_buff *skb, int type)
{
	struct yt8xxx_priv *priv = phydev->priv;
	struct yt_ptp_private *ptp_priv = priv->ptp_priv;
	u8 msgtype;
	u16 seqid;

#if 0
	dbg_ptp("@jie.han %s, %s, %d ptp_priv->tx_type: %s",
		__FILE__, __func__, __LINE__,
		ptp_priv->tx_type == HWTSTAMP_TX_ON ? "HWTSTAMP_TX_ON" :
		ptp_priv->tx_type == HWTSTAMP_TX_ONESTEP_SYNC ? "HWTSTAMP_TX_ONESTEP_SYNC" :
		ptp_priv->tx_type == HWTSTAMP_TX_OFF ? "HWTSTAMP_TX_OFF" : "others");
#endif
	//parsing_ptp_msgtype_seqid(skb, &msgtype, &seqid, type);
	switch (ptp_priv->tx_type) {
	case HWTSTAMP_TX_ON:
		dbg_ptp("@jie.han %s, %s, %d ptp_priv->tx_type: HWTSTAMP_TX_ON\n",
			__FILE__, __func__, __LINE__);
		dbg_ptp("@jie.han %s, %s, %d skb: %p, skb->sk: %p, "
			"skb->sk->sk_flags: %#lx, tx_flags: 0x%x\n",
			__FILE__, __func__, __LINE__,
			skb, skb->sk, skb->sk->sk_flags, skb_shinfo(skb)->tx_flags);
		if (!skb->sk) {
			kfree_skb(skb);
			return;
		}

		if (!parsing_ptp_msgtype_seqid(skb, &msgtype, &seqid, type)) {
			pr_err("@jie.han %s, %s, %d parsing skb failed, drop skb.\n",
				__FILE__, __func__, __LINE__);
			kfree_skb(skb);
			return;
		}
		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
		memset(YT_SKB_CB(skb), 0x0, sizeof(struct yt_ptp_skb_cb));
		YT_SKB_CB(skb)->timeout = jiffies + SKB_TIMESTAMP_TIMEOUT;
		YT_SKB_CB(skb)->seq_id = seqid;
		YT_SKB_CB(skb)->msgtype = msgtype;
		//if (skb->sk)
		/* increment the reference count to prevent premature release */
		sock_hold(skb->sk);
		dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_flags: %#lx\n",
			__FILE__, __func__, __LINE__, skb->sk->sk_flags);
		//dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_refcnt: %d\n",
		//	__FILE__, __func__, __LINE__, atomic_read(&skb->sk->sk_refcnt));
		dbg_ptp("@jie.han %s, %s, %d skb->sk->sk_refcnt: %d\n",
			__FILE__, __func__, __LINE__, skb->sk->sk_refcnt.refs.counter);
		skb_queue_tail(&ptp_priv->tx_queue, skb);
		ptp_schedule_worker(priv->ptp_priv->clock, 0);
		break;
	case HWTSTAMP_TX_ONESTEP_SYNC:
		dbg_ptp("@jie.han %s, %s, %d ptp_priv->tx_type: HWTSTAMP_TX_ONESTEP_SYNC\n",
			__FILE__, __func__, __LINE__);
		//__fallthrough;
	case HWTSTAMP_TX_OFF:
	default:
		kfree_skb(skb);
		break;
	}
}

static int yt8011_ptp_ts_info(struct phy_device *phydev,
			      struct ethtool_ts_info *info)
{
	struct yt8xxx_priv *priv = phydev->priv;

	info->so_timestamping =
		SOF_TIMESTAMPING_TX_SOFTWARE |
		SOF_TIMESTAMPING_RX_SOFTWARE |
		SOF_TIMESTAMPING_SOFTWARE |
		SOF_TIMESTAMPING_TX_HARDWARE |
		SOF_TIMESTAMPING_RX_HARDWARE |
		SOF_TIMESTAMPING_RAW_HARDWARE;
	info->phc_index = ptp_clock_index(priv->ptp_priv->clock);
	dbg_ptp("@jie.han %s, %s, %d info->phc_index: %d\n",
		__FILE__, __func__, __LINE__, info->phc_index);
	info->tx_types =
		(1 << HWTSTAMP_TX_OFF) |
		(1 << HWTSTAMP_TX_ON);
	info->rx_filters =
		(1 << HWTSTAMP_FILTER_NONE) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
		(1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
	return 0;
}
#endif

static void yt8011_remove(struct phy_device *phydev)
{
	struct yt8xxx_priv *priv = phydev->priv;
	struct yt_ptp_private *ptp_priv = priv->ptp_priv;

	dbg_ptp("@jie.han %s, %s, %d\n", __FILE__, __func__, __LINE__);
	if (ptp_priv->clock)
		ptp_clock_unregister(ptp_priv->clock);

	skb_queue_purge(&ptp_priv->tx_queue);
}

#if (KERNEL_VERSION(4, 4, 302) < LINUX_VERSION_CODE)
static int __yt8011_write_page(struct phy_device *phydev, int page)
{
	int ret;

	ret = __ytphy_read_ext(phydev, YT801X_REG_SMI_MUX);
	if (ret < 0)
		return ret;

	if (page == YT8011_REG_SPACE_UTP)
		return __ytphy_write_ext(phydev, YT801X_REG_SMI_MUX,
					 ret & (~BIT(15)));
	else if (page == YT8011_REG_SPACE_SDS)
		return __ytphy_write_ext(phydev, YT801X_REG_SMI_MUX,
					 ret | BIT(15));
	return 0;
}

static int __yt8011_read_page(struct phy_device *phydev)
{
	int ret;
	
	ret = __ytphy_read_ext(phydev, YT801X_REG_SMI_MUX);
	if (ret < 0)
		return ret;

	if (ret & BIT(15))
		return YT8011_REG_SPACE_SDS;
	else
		return YT8011_REG_SPACE_UTP;
}

static int yt8011_save_page(struct phy_device *phydev)
{
	/* mutex_lock(&phydev->mdio.bus->mdio_lock); */
	phy_lock_mdio_bus(phydev);
	
	return __yt8011_read_page(phydev);
}

static int yt8011_select_page(struct phy_device *phydev, int page)
{
	int ret, oldpage;

	oldpage = ret = yt8011_save_page(phydev);
	if (ret < 0)
		return ret;

	if (oldpage != page) {
		ret = __yt8011_write_page(phydev, page);
		if (ret < 0)
			return ret;
	}

	return oldpage;
}

static int yt8011_restore_page(struct phy_device *phydev, int oldpage, int ret)
{
	int r;

	if (oldpage >= 0) {
		r = __yt8011_write_page(phydev, oldpage);

		/* Propagate the operation return code if the page write
		 * was successful.
		 */
		if (ret >= 0 && r < 0)
			ret = r;
	} else {
		/* Propagate the phy page selection error code */
		ret = oldpage;
	}

	/* mutex_unlock(&phydev->mdio.bus->mdio_lock); */
	phy_unlock_mdio_bus(phydev);

	return ret;
}
#endif

static int yt801x_config_intr(struct phy_device *phydev)
{
	int ret;

	ret = ytphy_write_ext(phydev, YT801X_REG_SMI_MUX,
			      YTPHY_REG_SPACE_UTP);
	if (ret < 0)
		return ret;

	if (phydev->interrupts == PHY_INTERRUPT_ENABLED)
		ret = phy_write(phydev, YTPHY_UTP_INTR_REG,
				YTPHY_INTR_LINK_STATUS);

	return ret;
}

#if (KERNEL_VERSION(5, 10, 232) < LINUX_VERSION_CODE)
static irqreturn_t ytphy_handle_interrupt(struct phy_device *phydev)
{
	int ret;

	ret = phy_read(phydev, YTPHY_UTP_INTR_STATUS_REG);
	if (ret > 0) {
		phy_trigger_machine(phydev);
		return IRQ_HANDLED;
	}
	else
		return IRQ_NONE;
}

static irqreturn_t yt801x_handle_interrupt(struct phy_device *phydev)
{
	int ret;

	ret = ytphy_write_ext(phydev, YT801X_REG_SMI_MUX,
			      YTPHY_REG_SPACE_UTP);
	if (ret < 0)
		return IRQ_NONE;

	ret = phy_read(phydev, YTPHY_UTP_INTR_STATUS_REG);
	if (ret > 0) {
		phy_trigger_machine(phydev);
		return IRQ_HANDLED;
	}
	else
		return IRQ_NONE;
}
#else
static int yt801x_ack_interrupt(struct phy_device *phydev)
{
	int ret;

	ret = ytphy_write_ext(phydev, YT801X_REG_SMI_MUX,
			      YTPHY_REG_SPACE_UTP);
	if (ret < 0)
		return ret;

	ret = phy_read(phydev, YTPHY_UTP_INTR_STATUS_REG);
	return ret < 0 ? ret : 0;
}
#endif

static struct phy_driver ytphy_drvs[] = {
	{
		.phy_id		= PHY_ID_YT8011,
		.name		= "YT8011 Automotive Gigabit Ethernet",
		.phy_id_mask	= MOTORCOMM_PHY_ID_MASK,
		.features	= PHY_GBIT_FEATURES,
		.config_intr	= yt801x_config_intr,
#if (KERNEL_VERSION(5, 10, 232) < LINUX_VERSION_CODE)
		.handle_interrupt	= yt801x_handle_interrupt,
#else
		.ack_interrupt	= yt801x_ack_interrupt,
#endif
		.probe		= yt8011_probe,
#if (KERNEL_VERSION(4, 0, 0) > LINUX_VERSION_CODE)
#else
		.soft_reset	= yt8011_soft_reset,
#endif
		.config_aneg	= yt8011_config_aneg,
#if (KERNEL_VERSION(3, 14, 79) < LINUX_VERSION_CODE)
		.aneg_done	= yt8011_aneg_done,
#endif
		.config_init	= yt8011_config_init,
		.read_status	= yt8011_read_status,
#if (KERNEL_VERSION(5, 6, 0) > LINUX_VERSION_CODE)
		.ts_info	= yt8011_ptp_ts_info,
		.hwtstamp	= yt8011_ptp_hwtstamp,
		.rxtstamp	= yt8011_ptp_rxtstamp,
		.txtstamp	= yt8011_ptp_txtstamp,
#endif
		.remove		= yt8011_remove,
		//.suspend		= yt8011_suspend,
	},
};

#if (KERNEL_VERSION(4, 0, 0) > LINUX_VERSION_CODE)
static int ytphy_drivers_register(struct phy_driver *phy_drvs, int size)
{
	int i, j;
	int ret;

	for (i = 0; i < size; i++) {
		ret = phy_driver_register(&phy_drvs[i]);
		if (ret)
			goto err;
	}

	return 0;

err:
	for (j = 0; j < i; j++)
		phy_driver_unregister(&phy_drvs[j]);

	return ret;
}

static void ytphy_drivers_unregister(struct phy_driver *phy_drvs, int size)
{
	int i;

	for (i = 0; i < size; i++)
		phy_driver_unregister(&phy_drvs[i]);
}

static int __init ytphy_init(void)
{
	return ytphy_drivers_register(ytphy_drvs, ARRAY_SIZE(ytphy_drvs));
}

static void __exit ytphy_exit(void)
{
	pr_info(MODULE_NAME ": Module unloaded\n");
	ytphy_drivers_unregister(ytphy_drvs, ARRAY_SIZE(ytphy_drvs));
}

module_init(ytphy_init);
module_exit(ytphy_exit);
#else
/* for linux 4.x ~ */
/* module_phy_driver(ytphy_drvs);*/
static int __init phy_module_init(void)
{
	return phy_drivers_register(ytphy_drvs, ARRAY_SIZE(ytphy_drvs),
				    THIS_MODULE);
}

static void __exit phy_module_exit(void)
{
	pr_info(MODULE_NAME ": Module unloaded\n");

	phy_drivers_unregister(ytphy_drvs, ARRAY_SIZE(ytphy_drvs));
}

module_init(phy_module_init);
module_exit(phy_module_exit);
#endif

MODULE_DESCRIPTION("Motorcomm PHY driver");
MODULE_VERSION(YTPHY_LINUX_VERSION);	/* for modinfo xxxx.ko in userspace */
MODULE_AUTHOR("Jie Han");
MODULE_LICENSE("GPL");

static struct mdio_device_id __maybe_unused motorcomm_tbl[] = {
	{ PHY_ID_YT8011, MOTORCOMM_PHY_ID_MASK },
	{ }
};

MODULE_DEVICE_TABLE(mdio, motorcomm_tbl);