#define MAC_TSSR                       0x0d20
 #define MAC_TXSNR                      0x0d30
 #define MAC_TXSSR                      0x0d34
+#define MAC_TICNR                       0x0d58
+#define MAC_TICSNR                      0x0d5C
+#define MAC_TECNR                       0x0d60
+#define MAC_TECSNR                      0x0d64
 
 #define MAC_QTFCR_INC                  4
 #define MAC_MACA_INC                   4
 #define MAC_TSCR_SNAPTYPSEL_WIDTH      2
 #define MAC_TSCR_TSADDREG_INDEX                5
 #define MAC_TSCR_TSADDREG_WIDTH                1
+#define MAC_TSCR_TSUPDT_INDEX          3
+#define MAC_TSCR_TSUPDT_WIDTH          1
 #define MAC_TSCR_TSCFUPDT_INDEX                1
 #define MAC_TSCR_TSCFUPDT_WIDTH                1
 #define MAC_TSCR_TSCTRLSSR_INDEX       9
 #define MAC_TSSR_TXTSC_WIDTH           1
 #define MAC_TXSNR_TXTSSTSMIS_INDEX     31
 #define MAC_TXSNR_TXTSSTSMIS_WIDTH     1
+#define MAC_TICSNR_TSICSNS_INDEX       8
+#define MAC_TICSNR_TSICSNS_WIDTH       8
+#define MAC_TECSNR_TSECSNS_INDEX       8
+#define MAC_TECSNR_TSECSNS_WIDTH       8
 #define MAC_VLANHTR_VLHT_INDEX         0
 #define MAC_VLANHTR_VLHT_WIDTH         16
 #define MAC_VLANIR_VLTI_INDEX          20
 
        INIT_WORK(&pdata->stopdev_work, xgbe_stopdev);
        INIT_WORK(&pdata->tx_tstamp_work, xgbe_tx_tstamp);
 
+       /* Initialize PTP timestamping and clock. */
+       xgbe_init_ptp(pdata);
+
        ret = xgbe_alloc_memory(pdata);
        if (ret)
                goto err_ptpclk;
 
                if (XGMAC_GET_BITS(packet->attributes,
                                   RX_PACKET_ATTRIBUTES, RX_TSTAMP)) {
-                       u64 nsec;
-
-                       nsec = timecounter_cyc2time(&pdata->tstamp_tc,
-                                                   packet->rx_tstamp);
                        hwtstamps = skb_hwtstamps(skb);
-                       hwtstamps->hwtstamp = ns_to_ktime(nsec);
+                       hwtstamps->hwtstamp = ns_to_ktime(packet->rx_tstamp);
                }
 
                if (XGMAC_GET_BITS(packet->attributes,
 
 #include "xgbe.h"
 #include "xgbe-common.h"
 
+void xgbe_update_tstamp_time(struct xgbe_prv_data *pdata,
+                            unsigned int sec, unsigned int nsec)
+{
+       int count;
+
+       /* Set the time values and tell the device */
+       XGMAC_IOWRITE(pdata, MAC_STSUR, sec);
+       XGMAC_IOWRITE(pdata, MAC_STNUR, nsec);
+
+       /* issue command to update the system time value */
+       XGMAC_IOWRITE(pdata, MAC_TSCR,
+                     XGMAC_IOREAD(pdata, MAC_TSCR) |
+                     (1 << MAC_TSCR_TSUPDT_INDEX));
+
+       /* Wait for the time adjust/update to complete */
+       count = 10000;
+       while (--count && XGMAC_IOREAD_BITS(pdata, MAC_TSCR, TSUPDT))
+               udelay(5);
+
+       if (count < 0)
+               netdev_err(pdata->netdev,
+                          "timed out updating system timestamp\n");
+}
+
 void xgbe_update_tstamp_addend(struct xgbe_prv_data *pdata,
                               unsigned int addend)
 {
        if (XGMAC_GET_BITS_LE(rdesc->desc3, RX_CONTEXT_DESC3, TSA) &&
            !XGMAC_GET_BITS_LE(rdesc->desc3, RX_CONTEXT_DESC3, TSD)) {
                nsec = le32_to_cpu(rdesc->desc1);
-               nsec <<= 32;
-               nsec |= le32_to_cpu(rdesc->desc0);
+               nsec *= NSEC_PER_SEC;
+               nsec += le32_to_cpu(rdesc->desc0);
                if (nsec != 0xffffffffffffffffULL) {
                        packet->rx_tstamp = nsec;
                        XGMAC_SET_BITS(packet->attributes, RX_PACKET_ATTRIBUTES,
        }
 }
 
-int xgbe_config_tstamp(struct xgbe_prv_data *pdata, unsigned int mac_tscr)
+void xgbe_config_tstamp(struct xgbe_prv_data *pdata, unsigned int mac_tscr)
 {
-       /* Set one nano-second accuracy */
-       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSCTRLSSR, 1);
-
-       /* Set fine timestamp update */
-       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSCFUPDT, 1);
-
-       /* Overwrite earlier timestamps */
-       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TXTSSTSM, 1);
-
-       XGMAC_IOWRITE(pdata, MAC_TSCR, mac_tscr);
-
-       /* Exit if timestamping is not enabled */
-       if (!XGMAC_GET_BITS(mac_tscr, MAC_TSCR, TSENA))
-               return 0;
-
-       /* Initialize time registers */
-       XGMAC_IOWRITE_BITS(pdata, MAC_SSIR, SSINC, XGBE_TSTAMP_SSINC);
-       XGMAC_IOWRITE_BITS(pdata, MAC_SSIR, SNSINC, XGBE_TSTAMP_SNSINC);
-       xgbe_update_tstamp_addend(pdata, pdata->tstamp_addend);
-       xgbe_set_tstamp_time(pdata, 0, 0);
-
-       /* Initialize the timecounter */
-       timecounter_init(&pdata->tstamp_tc, &pdata->tstamp_cc,
-                        ktime_to_ns(ktime_get_real()));
+       unsigned int value = 0;
 
-       return 0;
+       value = XGMAC_IOREAD(pdata, MAC_TSCR);
+       value |= mac_tscr;
+       XGMAC_IOWRITE(pdata, MAC_TSCR, value);
 }
 
 void xgbe_tx_tstamp(struct work_struct *work)
                                                   tx_tstamp_work);
        struct skb_shared_hwtstamps hwtstamps;
        unsigned long flags;
-       u64 nsec;
 
        spin_lock_irqsave(&pdata->tstamp_lock, flags);
        if (!pdata->tx_tstamp_skb)
                goto unlock;
 
        if (pdata->tx_tstamp) {
-               nsec = timecounter_cyc2time(&pdata->tstamp_tc,
-                                           pdata->tx_tstamp);
-
                memset(&hwtstamps, 0, sizeof(hwtstamps));
-               hwtstamps.hwtstamp = ns_to_ktime(nsec);
+               hwtstamps.hwtstamp = ns_to_ktime(pdata->tx_tstamp);
                skb_tstamp_tx(pdata->tx_tstamp_skb, &hwtstamps);
        }
 
 
        skb_tx_timestamp(skb);
 }
+
+int xgbe_init_ptp(struct xgbe_prv_data *pdata)
+{
+       unsigned int mac_tscr = 0;
+       struct timespec64 now;
+       u64 dividend;
+
+       /* Register Settings to be done based on the link speed. */
+       switch (pdata->phy.speed) {
+       case SPEED_1000:
+               XGMAC_IOWRITE(pdata, MAC_TICNR, MAC_TICNR_1G_INITVAL);
+               XGMAC_IOWRITE(pdata, MAC_TECNR, MAC_TECNR_1G_INITVAL);
+               break;
+       case SPEED_2500:
+       case SPEED_10000:
+               XGMAC_IOWRITE_BITS(pdata, MAC_TICSNR, TSICSNS,
+                                  MAC_TICSNR_10G_INITVAL);
+               XGMAC_IOWRITE(pdata, MAC_TECNR, MAC_TECNR_10G_INITVAL);
+               XGMAC_IOWRITE_BITS(pdata, MAC_TECSNR, TSECSNS,
+                                  MAC_TECSNR_10G_INITVAL);
+               break;
+       case SPEED_UNKNOWN:
+       default:
+               break;
+       }
+
+       /* Enable IEEE1588 PTP clock. */
+       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSENA, 1);
+
+       /* Overwrite earlier timestamps */
+       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TXTSSTSM, 1);
+
+       /* Set one nano-second accuracy */
+       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSCTRLSSR, 1);
+
+       /* Set fine timestamp update */
+       XGMAC_SET_BITS(mac_tscr, MAC_TSCR, TSCFUPDT, 1);
+
+       xgbe_config_tstamp(pdata, mac_tscr);
+
+       /* Exit if timestamping is not enabled */
+       if (!XGMAC_GET_BITS(mac_tscr, MAC_TSCR, TSENA))
+               return -EOPNOTSUPP;
+
+       if (pdata->vdata->tstamp_ptp_clock_freq) {
+               /* Initialize time registers based on
+                * 125MHz PTP Clock Frequency
+                */
+               XGMAC_IOWRITE_BITS(pdata, MAC_SSIR, SSINC,
+                                  XGBE_V2_TSTAMP_SSINC);
+               XGMAC_IOWRITE_BITS(pdata, MAC_SSIR, SNSINC,
+                                  XGBE_V2_TSTAMP_SNSINC);
+       } else {
+               /* Initialize time registers based on
+                * 50MHz PTP Clock Frequency
+                */
+               XGMAC_IOWRITE_BITS(pdata, MAC_SSIR, SSINC, XGBE_TSTAMP_SSINC);
+               XGMAC_IOWRITE_BITS(pdata, MAC_SSIR, SNSINC, XGBE_TSTAMP_SNSINC);
+       }
+
+       /* Calculate the addend:
+        *   addend = 2^32 / (PTP ref clock / (PTP clock based on SSINC))
+        *          = (2^32 * (PTP clock based on SSINC)) / PTP ref clock
+        */
+       if (pdata->vdata->tstamp_ptp_clock_freq)
+               dividend = XGBE_V2_PTP_ACT_CLK_FREQ;
+       else
+               dividend = XGBE_PTP_ACT_CLK_FREQ;
+
+       dividend = (u64)(dividend << 32);
+       pdata->tstamp_addend = div_u64(dividend, pdata->ptpclk_rate);
+
+       xgbe_update_tstamp_addend(pdata, pdata->tstamp_addend);
+
+       dma_wmb();
+       /* initialize system time */
+       ktime_get_real_ts64(&now);
+
+       /* lower 32 bits of tv_sec are safe until y2106 */
+       xgbe_set_tstamp_time(pdata, (u32)now.tv_sec, now.tv_nsec);
+
+       return 0;
+}
 
        .tx_max_fifo_size               = 229376,
        .rx_max_fifo_size               = 229376,
        .tx_tstamp_workaround           = 1,
+       .tstamp_ptp_clock_freq          = 1,
        .ecc_support                    = 1,
        .i2c_support                    = 1,
        .irq_reissue_support            = 1,
        .tx_max_fifo_size               = 65536,
        .rx_max_fifo_size               = 65536,
        .tx_tstamp_workaround           = 1,
+       .tstamp_ptp_clock_freq          = 1,
        .ecc_support                    = 1,
        .i2c_support                    = 1,
        .irq_reissue_support            = 1,
 
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-static u64 xgbe_cc_read(const struct cyclecounter *cc)
-{
-       struct xgbe_prv_data *pdata = container_of(cc,
-                                                  struct xgbe_prv_data,
-                                                  tstamp_cc);
-       u64 nsec;
-
-       nsec = xgbe_get_tstamp_time(pdata);
-
-       return nsec;
-}
-
 static int xgbe_adjfine(struct ptp_clock_info *info, long scaled_ppm)
 {
        struct xgbe_prv_data *pdata = container_of(info,
        struct xgbe_prv_data *pdata = container_of(info,
                                                   struct xgbe_prv_data,
                                                   ptp_clock_info);
+       unsigned int neg_adjust = 0;
+       unsigned int sec, nsec;
+       u32 quotient, reminder;
        unsigned long flags;
 
+       if (delta < 0) {
+               neg_adjust = 1;
+               delta = -delta;
+       }
+
+       quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
+       sec = quotient;
+       nsec = reminder;
+
+       /* Negative adjustment for Hw timer register. */
+       if (neg_adjust) {
+               sec = -sec;
+               if (XGMAC_IOREAD_BITS(pdata, MAC_TSCR, TSCTRLSSR))
+                       nsec = (1000000000UL - nsec);
+               else
+                       nsec = (0x80000000UL - nsec);
+       }
+       nsec = (neg_adjust << 31) | nsec;
+
        spin_lock_irqsave(&pdata->tstamp_lock, flags);
-       timecounter_adjtime(&pdata->tstamp_tc, delta);
+       xgbe_update_tstamp_time(pdata, sec, nsec);
        spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 
        return 0;
 }
 
-static int xgbe_gettime(struct ptp_clock_info *info, struct timespec64 *ts)
+static int xgbe_gettimex(struct ptp_clock_info *info, struct timespec64 *ts,
+                        struct ptp_system_timestamp *sts)
 {
        struct xgbe_prv_data *pdata = container_of(info,
                                                   struct xgbe_prv_data,
        u64 nsec;
 
        spin_lock_irqsave(&pdata->tstamp_lock, flags);
-
-       nsec = timecounter_read(&pdata->tstamp_tc);
-
+       ptp_read_system_prets(sts);
+       nsec = xgbe_get_tstamp_time(pdata);
+       ptp_read_system_postts(sts);
        spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 
        *ts = ns_to_timespec64(nsec);
                                                   struct xgbe_prv_data,
                                                   ptp_clock_info);
        unsigned long flags;
-       u64 nsec;
-
-       nsec = timespec64_to_ns(ts);
 
        spin_lock_irqsave(&pdata->tstamp_lock, flags);
-
-       timecounter_init(&pdata->tstamp_tc, &pdata->tstamp_cc, nsec);
-
+       xgbe_set_tstamp_time(pdata, ts->tv_sec, ts->tv_nsec);
        spin_unlock_irqrestore(&pdata->tstamp_lock, flags);
 
        return 0;
 {
        struct ptp_clock_info *info = &pdata->ptp_clock_info;
        struct ptp_clock *clock;
-       struct cyclecounter *cc = &pdata->tstamp_cc;
-       u64 dividend;
 
        snprintf(info->name, sizeof(info->name), "%s",
                 netdev_name(pdata->netdev));
        info->max_adj = pdata->ptpclk_rate;
        info->adjfine = xgbe_adjfine;
        info->adjtime = xgbe_adjtime;
-       info->gettime64 = xgbe_gettime;
+       info->gettimex64 = xgbe_gettimex;
        info->settime64 = xgbe_settime;
        info->enable = xgbe_enable;
 
 
        pdata->ptp_clock = clock;
 
-       /* Calculate the addend:
-        *   addend = 2^32 / (PTP ref clock / 50Mhz)
-        *          = (2^32 * 50Mhz) / PTP ref clock
-        */
-       dividend = 50000000;
-       dividend <<= 32;
-       pdata->tstamp_addend = div_u64(dividend, pdata->ptpclk_rate);
-
-       /* Setup the timecounter */
-       cc->read = xgbe_cc_read;
-       cc->mask = CLOCKSOURCE_MASK(64);
-       cc->mult = 1;
-       cc->shift = 0;
-
-       timecounter_init(&pdata->tstamp_tc, &pdata->tstamp_cc,
-                        ktime_to_ns(ktime_get_real()));
-
        /* Disable all timestamping to start */
        XGMAC_IOWRITE(pdata, MAC_TSCR, 0);
        pdata->tstamp_config.tx_type = HWTSTAMP_TX_OFF;
 
 #define XGBE_MSI_BASE_COUNT    4
 #define XGBE_MSI_MIN_COUNT     (XGBE_MSI_BASE_COUNT + 1)
 
+/* Initial PTP register values based on Link Speed. */
+#define MAC_TICNR_1G_INITVAL   0x10
+#define MAC_TECNR_1G_INITVAL   0x28
+
+#define MAC_TICSNR_10G_INITVAL 0x33
+#define MAC_TECNR_10G_INITVAL  0x14
+#define MAC_TECSNR_10G_INITVAL 0xCC
+
 /* PCI clock frequencies */
 #define XGBE_V2_DMA_CLOCK_FREQ 500000000       /* 500 MHz */
 #define XGBE_V2_PTP_CLOCK_FREQ 125000000       /* 125 MHz */
  */
 #define XGBE_TSTAMP_SSINC      20
 #define XGBE_TSTAMP_SNSINC     0
+#define XGBE_PTP_ACT_CLK_FREQ  500000000
+
+#define XGBE_V2_TSTAMP_SSINC   0xA
+#define XGBE_V2_TSTAMP_SNSINC  0
+#define XGBE_V2_PTP_ACT_CLK_FREQ       1000000000
 
 /* Driver PMT macros */
 #define XGMAC_DRIVER_CONTEXT   1
        unsigned int tx_max_fifo_size;
        unsigned int rx_max_fifo_size;
        unsigned int tx_tstamp_workaround;
+       unsigned int tstamp_ptp_clock_freq;
        unsigned int ecc_support;
        unsigned int i2c_support;
        unsigned int irq_reissue_support;
        struct ptp_clock_info ptp_clock_info;
        struct ptp_clock *ptp_clock;
        struct hwtstamp_config tstamp_config;
-       struct cyclecounter tstamp_cc;
-       struct timecounter tstamp_tc;
        unsigned int tstamp_addend;
        struct work_struct tx_tstamp_work;
        struct sk_buff *tx_tstamp_skb;
 void xgbe_full_restart_dev(struct xgbe_prv_data *pdata);
 
 /* For Timestamp config */
-int xgbe_config_tstamp(struct xgbe_prv_data *pdata, unsigned int mac_tscr);
+void xgbe_config_tstamp(struct xgbe_prv_data *pdata, unsigned int mac_tscr);
 u64 xgbe_get_tstamp_time(struct xgbe_prv_data *pdata);
 u64 xgbe_get_tx_tstamp(struct xgbe_prv_data *pdata);
 void xgbe_get_rx_tstamp(struct xgbe_packet_data *packet,
                               unsigned int addend);
 void xgbe_set_tstamp_time(struct xgbe_prv_data *pdata, unsigned int sec,
                          unsigned int nsec);
-int xgbe_config_tstamp(struct xgbe_prv_data *pdata, unsigned int mac_tscr);
 void xgbe_tx_tstamp(struct work_struct *work);
 int xgbe_get_hwtstamp_settings(struct xgbe_prv_data *pdata,
                               struct ifreq *ifreq);
 void xgbe_prep_tx_tstamp(struct xgbe_prv_data *pdata,
                         struct sk_buff *skb,
                         struct xgbe_packet_data *packet);
-
+int xgbe_init_ptp(struct xgbe_prv_data *pdata);
+void xgbe_update_tstamp_time(struct xgbe_prv_data *pdata, unsigned int sec,
+                            unsigned int nsec);
 #ifdef CONFIG_DEBUG_FS
 void xgbe_debugfs_init(struct xgbe_prv_data *);
 void xgbe_debugfs_exit(struct xgbe_prv_data *);