ENGR00275371 net: fec: PTP: Add ptp support for IXXAT stack

These patch  add ptp support for IXXAT stack.
Cherry picked from commit 1c8839574ac87826f53b96f987975a9bb1d72915.

Signed-off-by: Luwei Zhou  <B45643@freescale.com>

3 files changed, 753 insertions, 55 deletions

diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index ea5c60d84889..888ff237338d 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -16,6 +16,9 @@
 #include <linux/clocksource.h>
 #include <linux/net_tstamp.h>
 #include <linux/ptp_clock_kernel.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/circ_buf.h>
 
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
     defined(CONFIG_M520x) || defined(CONFIG_M532x) || \
@@ -256,6 +259,98 @@ struct bufdesc_ex {
 #define FLAG_RX_CSUM_ENABLED    (BD_ENET_RX_ICE | BD_ENET_RX_PCR)
 #define FLAG_RX_CSUM_ERROR      (BD_ENET_RX_ICE | BD_ENET_RX_PCR)
 
+#define FALSE                  0
+#define TRUE                   1
+
+/* IEEE 1588 definition */
+#define FEC_T_PERIOD_ONE_SEC           0x3B9ACA00
+
+#define DEFAULT_PTP_RX_BUF_SZ          64
+#define DEFAULT_PTP_TX_BUF_SZ          64
+
+/* 1588stack API defines */
+#define PTP_ENBL_TXTS_IOCTL    SIOCDEVPRIVATE
+#define PTP_DSBL_TXTS_IOCTL    (SIOCDEVPRIVATE + 1)
+#define PTP_ENBL_RXTS_IOCTL    (SIOCDEVPRIVATE + 2)
+#define PTP_DSBL_RXTS_IOCTL    (SIOCDEVPRIVATE + 3)
+#define PTP_GET_TX_TIMESTAMP   (SIOCDEVPRIVATE + 4)
+#define PTP_GET_RX_TIMESTAMP   (SIOCDEVPRIVATE + 5)
+#define PTP_SET_RTC_TIME       (SIOCDEVPRIVATE + 6)
+#define PTP_GET_CURRENT_TIME   (SIOCDEVPRIVATE + 7)
+#define PTP_SET_COMPENSATION   (SIOCDEVPRIVATE + 9)
+#define PTP_GET_ORIG_COMP      (SIOCDEVPRIVATE + 10)
+#define PTP_FLUSH_TIMESTAMP    (SIOCDEVPRIVATE + 11)
+
+/* IEEE1588 ptp head format */
+#define PTP_CTRL_OFFS          0x52
+#define PTP_SOURCE_PORT_LENGTH 10
+#define PTP_HEADER_SEQ_OFFS    30
+#define PTP_HEADER_CTL_OFFS    32
+#define PTP_SPID_OFFS          20
+#define PTP_HEADER_SZE         34
+#define PTP_EVENT_PORT         0x013F
+
+#define FEC_VLAN_TAG_LEN       0x04
+#define FEC_ETHTYPE_LEN                0x02
+
+/* 1588-2008 network protocol enumeration values */
+#define FEC_PTP_PROT_IPV4              1
+#define FEC_PTP_PROT_IPV6              2
+#define FEC_PTP_PROT_802_3             3
+#define FEC_PTP_PROT_DONTCARE          0xFFFF
+#define FEC_PACKET_TYPE_UDP            0x11
+
+#define FEC_PTP_ORIG_COMP              0x15555555
+#define FEC_PTP_SPINNER_2              2
+#define FEC_PTP_SPINNER_4              4
+
+/* PTP standard time representation structure */
+struct ptp_time{
+        u64 sec;        /* seconds */
+        u32 nsec;       /* nanoseconds */
+};
+
+/* struct needed to identify a timestamp */
+struct fec_ptp_ident {
+        u8      version;
+        u8      message_type;
+        u16     netw_prot;
+        u16     seq_id;
+        u8      spid[10];
+};
+
+/* interface for PTP driver command GET_TX_TIME */
+struct fec_ptp_ts_data {
+        struct fec_ptp_ident ident;
+        /* PTP timestamp */
+        struct ptp_time ts;
+};
+
+/* circular buffer for ptp timestamps over ioctl */
+struct fec_ptp_circular {
+        int     front;
+        int     end;
+        int     size;
+        struct  fec_ptp_ts_data *data_buf;
+};
+
+/* interface for PTP driver command SET_RTC_TIME/GET_CURRENT_TIME */
+struct ptp_rtc_time {
+        struct ptp_time rtc_time;
+};
+
+/* interface for PTP driver command SET_COMPENSATION */
+struct ptp_set_comp {
+        u32 drift;
+        bool o_ops;
+        u32 freq_compensation;
+};
+
+struct ptp_time_correct {
+        u32 corr_period;
+        u32 corr_inc;
+};
+
 struct fec_enet_delayed_work {
         struct delayed_work delay_work;
         bool timeout;
@@ -312,13 +407,19 @@ struct fec_enet_private {
         int     speed;
         struct  completion mdio_done;
         int     irq[FEC_IRQ_NUM];
+        /* HW timestamping over ioctl enabled flag */
+        int hwts_tx_en_ioctl;
+        int hwts_rx_en_ioctl;
+        struct fec_ptp_circular tx_timestamps;
+        struct fec_ptp_circular rx_timestamps;
+        u64 prtc;
         int     bufdesc_ex;
         int     pause_flag;
 
         struct  napi_struct napi;
         int     csum_flags;
 
-        int     phy_reset_gpio;
+        int     phy_reset_gpio;
 
         struct ptp_clock *ptp_clock;
         struct ptp_clock_info ptp_caps;
@@ -339,6 +440,15 @@ struct fec_enet_private {
 void fec_ptp_init(struct platform_device *pdev);
 void fec_ptp_start_cyclecounter(struct net_device *ndev);
 int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd);
+void fec_ptp_cleanup(struct fec_enet_private *priv);
+void fec_ptp_stop(struct net_device *ndev);
+int fec_ptp_do_txstamp(struct sk_buff *skb);
+void fec_ptp_store_txstamp(struct fec_enet_private *priv,
+                                struct sk_buff *skb,
+                                struct bufdesc *bdp);
+void fec_ptp_store_rxstamp(struct fec_enet_private *priv,
+                                struct sk_buff *skb,
+                                struct bufdesc *bdp);
 
 /****************************************************************************/
 #endif /* FEC_H */
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 86c35a8817cf..18a76fb8e595 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -197,6 +197,8 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define FEC_ENET_RXB    ((uint)0x01000000)      /* A buffer was received */
 #define FEC_ENET_MII    ((uint)0x00800000)      /* MII interrupt */
 #define FEC_ENET_EBERR  ((uint)0x00400000)      /* SDMA bus error */
+#define FEC_ENET_TS_AVAIL       ((uint)0x00010000)
+#define FEC_ENET_TS_TIMER       ((uint)0x00008000)
 
 #define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII)
 #define FEC_RX_DISABLED_IMASK (FEC_DEFAULT_IMASK & (~FEC_ENET_RXF))
@@ -369,7 +371,8 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
                 struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
                 ebdp->cbd_bdu = 0;
                 if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
-                        fep->hwts_tx_en)) {
+                        fep->hwts_tx_en) || unlikely(fep->hwts_tx_en_ioctl &&
+                                                fec_ptp_do_txstamp(skb))) {
                         ebdp->cbd_esc = (BD_ENET_TX_TS | BD_ENET_TX_INT);
                         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
                 } else {
@@ -645,8 +648,13 @@ fec_restart(struct net_device *ndev, int duplex)
         writel(ecntl, fep->hwp + FEC_ECNTRL);
         writel(0, fep->hwp + FEC_R_DES_ACTIVE);
 
-        if (fep->bufdesc_ex)
+        if (fep->bufdesc_ex) {
                 fec_ptp_start_cyclecounter(ndev);
+                /* Enable interrupts we wish to service */
+                writel(FEC_DEFAULT_IMASK | FEC_ENET_TS_AVAIL |
+                        FEC_ENET_TS_TIMER, fep->hwp + FEC_IMASK);
+        } else
+                writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 
         /* Enable interrupts we wish to service */
         writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
@@ -681,6 +689,11 @@ fec_stop(struct net_device *ndev)
         writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
         writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 
+        if (fep->bufdesc_ex && (fep->hwts_tx_en_ioctl ||
+                fep->hwts_rx_en_ioctl || fep->hwts_tx_en ||
+                fep->hwts_rx_en))
+                fec_ptp_stop(ndev);
+
         /* We have to keep ENET enabled to have MII interrupt stay working */
         if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) {
                 writel(2, fep->hwp + FEC_ECNTRL);
@@ -775,8 +788,8 @@ fec_enet_tx(struct net_device *ndev)
                         ndev->stats.tx_bytes += bdp->cbd_datlen;
                 }
 
-                if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS) &&
-                        fep->bufdesc_ex) {
+                if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS) &&
+                                fep->hwts_tx_en) && fep->bufdesc_ex) {
                         struct skb_shared_hwtstamps shhwtstamps;
                         unsigned long flags;
                         struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
@@ -787,7 +800,8 @@ fec_enet_tx(struct net_device *ndev)
                                 timecounter_cyc2time(&fep->tc, ebdp->ts));
                         spin_unlock_irqrestore(&fep->tmreg_lock, flags);
                         skb_tstamp_tx(skb, &shhwtstamps);
-                }
+                } else if (unlikely(fep->hwts_tx_en_ioctl) && fep->bufdesc_ex)
+                        fec_ptp_store_txstamp(fep, skb, bdp);
 
                 if (status & BD_ENET_TX_READY)
                         netdev_err(ndev, "HEY! Enet xmit interrupt and TX_READY\n");
@@ -944,8 +958,6 @@ fec_enet_rx(struct net_device *ndev, int budget)
                                                        data + payload_offset,
                                                        pkt_len - 4 - (2 * ETH_ALEN));
 
-                        skb->protocol = eth_type_trans(skb, ndev);
-
                         /* Get receive timestamp from the skb */
                         if (fep->hwts_rx_en && fep->bufdesc_ex) {
                                 struct skb_shared_hwtstamps *shhwtstamps =
@@ -958,8 +970,11 @@ fec_enet_rx(struct net_device *ndev, int budget)
                                 shhwtstamps->hwtstamp = ns_to_ktime(
                                     timecounter_cyc2time(&fep->tc, ebdp->ts));
                                 spin_unlock_irqrestore(&fep->tmreg_lock, flags);
-                        }
+                        } else if (unlikely(fep->hwts_rx_en_ioctl) &&
+                                        fep->bufdesc_ex)
+                                fec_ptp_store_rxstamp(fep, skb, bdp);
 
+                        skb->protocol = eth_type_trans(skb, ndev);
                         if (fep->bufdesc_ex &&
                             (fep->csum_flags & FLAG_RX_CSUM_ENABLED)) {
                                 if (!(ebdp->cbd_esc & FLAG_RX_CSUM_ERROR)) {
@@ -1037,6 +1052,12 @@ fec_enet_interrupt(int irq, void *dev_id)
                         }
                 }
 
+                if ((int_events & FEC_ENET_TS_TIMER) && fep->bufdesc_ex) {
+                        ret = IRQ_HANDLED;
+                        if (fep->hwts_tx_en_ioctl || fep->hwts_rx_en_ioctl)
+                                fep->prtc++;
+                }
+
                 if (int_events & FEC_ENET_MII) {
                         ret = IRQ_HANDLED;
                         complete(&fep->mdio_done);
@@ -1656,8 +1677,11 @@ static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
         if (!phydev)
                 return -ENODEV;
 
-        if (cmd == SIOCSHWTSTAMP && fep->bufdesc_ex)
+        if (((cmd == SIOCSHWTSTAMP) || ((cmd >= PTP_ENBL_TXTS_IOCTL) &&
+                (cmd <= PTP_FLUSH_TIMESTAMP))) && fep->bufdesc_ex)
                 return fec_ptp_ioctl(ndev, rq, cmd);
+        else if (fep->bufdesc_ex)
+                return -ENODEV;
 
         return phy_mii_ioctl(phydev, rq, cmd);
 }
@@ -2255,7 +2279,10 @@ fec_drv_remove(struct platform_device *pdev)
         cancel_delayed_work_sync(&(fep->delay_work.delay_work));
         unregister_netdev(ndev);
         fec_enet_mii_remove(fep);
-        del_timer_sync(&fep->time_keep);
+        if (fep->bufdesc_ex) {
+                fec_ptp_cleanup(fep);
+                del_timer_sync(&fep->time_keep);
+        }
         for (i = 0; i < FEC_IRQ_NUM; i++) {
                 int irq = platform_get_irq(pdev, i);
                 if (irq > 0)
@@ -2263,9 +2290,11 @@ fec_drv_remove(struct platform_device *pdev)
         }
         if (fep->reg_phy)
                 regulator_disable(fep->reg_phy);
-        clk_disable_unprepare(fep->clk_ptp);
-        if (fep->ptp_clock)
-                ptp_clock_unregister(fep->ptp_clock);
+        if (fep->bufdesc_ex) {
+                clk_disable_unprepare(fep->clk_ptp);
+                if (fep->ptp_clock)
+                        ptp_clock_unregister(fep->ptp_clock);
+        }
         clk_disable_unprepare(fep->clk_enet_out);
         clk_disable_unprepare(fep->clk_ahb);
         clk_disable_unprepare(fep->clk_ipg);
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 5007e4f9fff9..06c679ebcd9c 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -1,7 +1,7 @@
 /*
  * Fast Ethernet Controller (ENET) PTP driver for MX6x.
  *
- * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012-2013 Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -37,6 +37,9 @@
 #include <linux/workqueue.h>
 #include <linux/bitops.h>
 #include <linux/io.h>
+#include <linux/vmalloc.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
 #include <linux/irq.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
@@ -49,6 +52,7 @@
 
 #include "fec.h"
 
+
 /* FEC 1588 register bits */
 #define FEC_T_CTRL_SLAVE                0x00002000
 #define FEC_T_CTRL_CAPTURE              0x00000800
@@ -71,6 +75,446 @@
 #define FEC_TS_TIMESTAMP        0x418
 
 #define FEC_CC_MULT     (1 << 31)
+
+/* Alloc the ring resource */
+static int fec_ptp_init_circ(struct fec_ptp_circular *buf, int size)
+{
+        buf->data_buf = (struct fec_ptp_ts_data *)
+                vmalloc(size * sizeof(struct fec_ptp_ts_data));
+
+        if (!buf->data_buf)
+                return 1;
+        buf->front = 0;
+        buf->end = 0;
+        buf->size = size;
+        return 0;
+}
+
+static inline int fec_ptp_calc_index(int size, int curr_index, int offset)
+{
+        return (curr_index + offset) % size;
+}
+
+static int fec_ptp_is_empty(struct fec_ptp_circular *buf)
+{
+        return (buf->front == buf->end);
+}
+
+static int fec_ptp_nelems(struct fec_ptp_circular *buf)
+{
+        const int front = buf->front;
+        const int end = buf->end;
+        const int size = buf->size;
+        int n_items;
+
+        if (end > front)
+                n_items = end - front;
+        else if (end < front)
+                n_items = size - (front - end);
+        else
+                n_items = 0;
+
+        return n_items;
+}
+
+static int fec_ptp_is_full(struct fec_ptp_circular *buf)
+{
+        if (fec_ptp_nelems(buf) == (buf->size - 1))
+                return 1;
+        else
+                return 0;
+}
+
+static int fec_ptp_insert(struct fec_ptp_circular *ptp_buf,
+                          struct fec_ptp_ts_data *data)
+{
+        struct fec_ptp_ts_data *tmp;
+
+        if (fec_ptp_is_full(ptp_buf))
+                ptp_buf->end = fec_ptp_calc_index(ptp_buf->size,
+                                                ptp_buf->end, 1);
+
+        tmp = (ptp_buf->data_buf + ptp_buf->end);
+        memcpy(tmp, data, sizeof(struct fec_ptp_ts_data));
+        ptp_buf->end = fec_ptp_calc_index(ptp_buf->size, ptp_buf->end, 1);
+
+        return 0;
+}
+
+static int fec_ptp_find_and_remove(struct fec_ptp_circular *ptp_buf,
+                        struct fec_ptp_ident *ident, struct ptp_time *ts)
+{
+        int i;
+        int size = ptp_buf->size, end = ptp_buf->end;
+        struct fec_ptp_ident *tmp_ident;
+
+        if (fec_ptp_is_empty(ptp_buf))
+                return 1;
+
+        i = ptp_buf->front;
+        while (i != end) {
+                tmp_ident = &(ptp_buf->data_buf + i)->ident;
+                if (tmp_ident->version == ident->version) {
+                        if (tmp_ident->message_type == ident->message_type) {
+                                if ((tmp_ident->netw_prot == ident->netw_prot)
+                                || (ident->netw_prot ==
+                                        FEC_PTP_PROT_DONTCARE)) {
+                                        if (tmp_ident->seq_id ==
+                                                        ident->seq_id) {
+                                                int ret =
+                                                memcmp(tmp_ident->spid,
+                                                        ident->spid,
+                                                        PTP_SOURCE_PORT_LENGTH);
+                                                if (0 == ret)
+                                                        break;
+                                        }
+                                }
+                        }
+                }
+                /* get next */
+                i = fec_ptp_calc_index(size, i, 1);
+        }
+
+        /* not found ? */
+        if (i == end) {
+                /* buffer full ? */
+                if (fec_ptp_is_full(ptp_buf))
+                        /* drop one in front */
+                        ptp_buf->front =
+                        fec_ptp_calc_index(size, ptp_buf->front, 1);
+
+                return 1;
+        }
+        *ts = (ptp_buf->data_buf + i)->ts;
+        ptp_buf->front = fec_ptp_calc_index(size, ptp_buf->front, 1);
+
+        return 0;
+}
+
+/* 1588 Module intialization */
+void fec_ptp_start(struct net_device *ndev)
+{
+        struct fec_enet_private *fep = netdev_priv(ndev);
+        unsigned long flags;
+        int inc;
+
+        inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(fep->clk_ptp);
+
+        spin_lock_irqsave(&fep->tmreg_lock, flags);
+        /* Select 1588 Timer source and enable module for starting Tmr Clock */
+        writel(FEC_T_CTRL_RESTART, fep->hwp + FEC_ATIME_CTRL);
+        writel(inc << FEC_T_INC_OFFSET,
+                        fep->hwp + FEC_ATIME_INC);
+        writel(FEC_T_PERIOD_ONE_SEC, fep->hwp + FEC_ATIME_EVT_PERIOD);
+        /* start counter */
+        writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE,
+                        fep->hwp + FEC_ATIME_CTRL);
+        spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+}
+
+/* Cleanup routine for 1588 module.
+ * When PTP is disabled this routing is called */
+void fec_ptp_stop(struct net_device *ndev)
+{
+        struct fec_enet_private *priv = netdev_priv(ndev);
+
+        writel(0, priv->hwp + FEC_ATIME_CTRL);
+        writel(FEC_T_CTRL_RESTART, priv->hwp + FEC_ATIME_CTRL);
+}
+
+static void fec_get_curr_cnt(struct fec_enet_private *priv,
+                        struct ptp_rtc_time *curr_time)
+{
+        u32 tempval;
+
+        tempval = readl(priv->hwp + FEC_ATIME_CTRL);
+        tempval |= FEC_T_CTRL_CAPTURE;
+
+        writel(tempval, priv->hwp + FEC_ATIME_CTRL);
+        curr_time->rtc_time.nsec = readl(priv->hwp + FEC_ATIME);
+        curr_time->rtc_time.sec = priv->prtc;
+
+        writel(tempval, priv->hwp + FEC_ATIME_CTRL);
+        tempval = readl(priv->hwp + FEC_ATIME);
+
+        if (tempval < curr_time->rtc_time.nsec) {
+                curr_time->rtc_time.nsec = tempval;
+                curr_time->rtc_time.sec = priv->prtc;
+        }
+}
+
+/* Set the 1588 timer counter registers */
+static void fec_set_1588cnt(struct fec_enet_private *priv,
+                        struct ptp_rtc_time *fec_time)
+{
+        u32 tempval;
+        unsigned long flags;
+
+        spin_lock_irqsave(&priv->tmreg_lock, flags);
+        priv->prtc = fec_time->rtc_time.sec;
+
+        tempval = fec_time->rtc_time.nsec;
+        writel(tempval, priv->hwp + FEC_ATIME);
+        spin_unlock_irqrestore(&priv->tmreg_lock, flags);
+}
+
+/**
+ * Parse packets if they are PTP.
+ * The PTP header can be found in an IPv4, IPv6 or in an IEEE802.3
+ * ethernet frame. The function returns the position of the PTP packet
+ * or NULL, if no PTP found
+ */
+u8 *fec_ptp_parse_packet(struct sk_buff *skb, u16 *eth_type)
+{
+        u8 *position = skb->data + ETH_ALEN + ETH_ALEN;
+        u8 *ptp_loc = NULL;
+
+        *eth_type = *((u16 *)position);
+        /* Check if outer vlan tag is here */
+        if (ntohs(*eth_type) == ETH_P_8021Q) {
+                position += FEC_VLAN_TAG_LEN;
+                *eth_type = *((u16 *)position);
+        }
+
+        /* set position after ethertype */
+        position += FEC_ETHTYPE_LEN;
+        if (ETH_P_1588 == ntohs(*eth_type)) {
+                ptp_loc = position;
+                /* IEEE1588 event message which needs timestamping */
+                if ((ptp_loc[0] & 0xF) <= 3) {
+                        if (skb->len >=
+                        ((ptp_loc - skb->data) + PTP_HEADER_SZE))
+                                return ptp_loc;
+                }
+        } else if (ETH_P_IP == ntohs(*eth_type)) {
+                u8 *ip_header, *prot, *udp_header;
+                u8 ip_version, ip_hlen;
+                ip_header = position;
+                ip_version = ip_header[0] >> 4; /* correct IP version? */
+                if (0x04 == ip_version) { /* IPv4 */
+                        prot = ip_header + 9; /* protocol */
+                        if (FEC_PACKET_TYPE_UDP == *prot) {
+                                u16 udp_dstPort;
+                                /* retrieve the size of the ip-header
+                                 * with the first byte of the ip-header:
+                                 * version ( 4 bits) + Internet header
+                                 * length (4 bits)
+                                 */
+                                ip_hlen   = (*ip_header & 0xf) * 4;
+                                udp_header = ip_header + ip_hlen;
+                                udp_dstPort = *((u16 *)(udp_header + 2));
+                                /* check the destination port address
+                                 * ( 319 (0x013F) = PTP event port )
+                                 */
+                                if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
+                                        ptp_loc = udp_header + 8;
+                                        /* long enough ? */
+                                        if (skb->len >= ((ptp_loc - skb->data)
+                                                        + PTP_HEADER_SZE))
+                                                return ptp_loc;
+                                }
+                        }
+                }
+        } else if (ETH_P_IPV6 == ntohs(*eth_type)) {
+                u8 *ip_header, *udp_header, *prot;
+                u8 ip_version;
+                ip_header = position;
+                ip_version = ip_header[0] >> 4;
+                if (0x06 == ip_version) {
+                        prot = ip_header + 6;
+                        if (FEC_PACKET_TYPE_UDP == *prot) {
+                                u16 udp_dstPort;
+                                udp_header = ip_header + 40;
+                                udp_dstPort = *((u16 *)(udp_header + 2));
+                                /* check the destination port address
+                                 * ( 319 (0x013F) = PTP event port )
+                                 */
+                                if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
+                                        ptp_loc = udp_header + 8;
+                                        /* long enough ? */
+                                        if (skb->len >= ((ptp_loc - skb->data)
+                                                        + PTP_HEADER_SZE))
+                                                return ptp_loc;
+                                }
+                        }
+                }
+        }
+
+        return NULL; /* no PTP frame */
+}
+
+/* Set the BD to ptp */
+int fec_ptp_do_txstamp(struct sk_buff *skb)
+{
+        u8 *ptp_loc;
+        u16 eth_type;
+
+        ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
+        if (ptp_loc != NULL)
+                return 1;
+
+        return 0;
+}
+
+void fec_ptp_store_txstamp(struct fec_enet_private *priv,
+                           struct sk_buff *skb,
+                           struct bufdesc *bdp)
+{
+        struct fec_ptp_ts_data tmp_tx_time;
+        struct bufdesc_ex *bdp_ex = NULL;
+        u8 *ptp_loc;
+        u16 eth_type;
+
+        bdp_ex = container_of(bdp, struct bufdesc_ex, desc);
+        ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
+        if (ptp_loc != NULL) {
+                /* store identification data */
+                switch (ntohs(eth_type)) {
+                case ETH_P_IP:
+                        tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
+                        break;
+                case ETH_P_IPV6:
+                        tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
+                        break;
+                case ETH_P_1588:
+                        tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
+                        break;
+                default:
+                        return;
+                }
+                tmp_tx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
+                tmp_tx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
+                tmp_tx_time.ident.seq_id =
+                        ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
+                memcpy(tmp_tx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
+                                                PTP_SOURCE_PORT_LENGTH);
+                /* store tx timestamp */
+                tmp_tx_time.ts.sec = priv->prtc;
+                tmp_tx_time.ts.nsec = bdp_ex->ts;
+                /* insert timestamp in circular buffer */
+                fec_ptp_insert(&(priv->tx_timestamps), &tmp_tx_time);
+        }
+}
+
+void fec_ptp_store_rxstamp(struct fec_enet_private *priv,
+                           struct sk_buff *skb,
+                           struct bufdesc *bdp)
+{
+        struct fec_ptp_ts_data tmp_rx_time;
+        struct bufdesc_ex *bdp_ex = NULL;
+        u8 *ptp_loc;
+        u16 eth_type;
+
+        bdp_ex = container_of(bdp, struct bufdesc_ex, desc);
+        ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
+        if (ptp_loc != NULL) {
+                /* store identification data */
+                tmp_rx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
+                tmp_rx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
+                switch (ntohs(eth_type)) {
+                case ETH_P_IP:
+                        tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
+                        break;
+                case ETH_P_IPV6:
+                        tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
+                        break;
+                case ETH_P_1588:
+                        tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
+                        break;
+                default:
+                        return;
+                }
+                tmp_rx_time.ident.seq_id =
+                        ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
+                memcpy(tmp_rx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
+                                                PTP_SOURCE_PORT_LENGTH);
+                /* store rx timestamp */
+                tmp_rx_time.ts.sec = priv->prtc;
+                tmp_rx_time.ts.nsec = bdp_ex->ts;
+
+                /* insert timestamp in circular buffer */
+                fec_ptp_insert(&(priv->rx_timestamps), &tmp_rx_time);
+        }
+}
+
+
+static void fec_handle_ptpdrift(struct fec_enet_private *priv,
+        struct ptp_set_comp *comp, struct ptp_time_correct *ptc)
+{
+        u32 ndrift;
+        u32 i, adj_inc, adj_period;
+        u32 tmp_current, tmp_winner;
+        u32 ptp_ts_clk, ptp_inc;
+
+        ptp_ts_clk = clk_get_rate(priv->clk_ptp);
+        ptp_inc = FEC_T_PERIOD_ONE_SEC / ptp_ts_clk;
+
+        ndrift = comp->drift;
+
+        if (ndrift == 0) {
+                ptc->corr_inc = 0;
+                ptc->corr_period = 0;
+                return;
+        } else if (ndrift >= ptp_ts_clk) {
+                ptc->corr_inc = (u32)(ndrift / ptp_ts_clk);
+                ptc->corr_period = 1;
+                return;
+        } else {
+                tmp_winner = 0xFFFFFFFF;
+                adj_inc = 1;
+
+                if (ndrift > (ptp_ts_clk / ptp_inc)) {
+                        adj_inc = ptp_inc / FEC_PTP_SPINNER_2;
+                } else if (ndrift > (ptp_ts_clk /
+                        (ptp_inc * FEC_PTP_SPINNER_4))) {
+                        adj_inc = ptp_inc / FEC_PTP_SPINNER_4;
+                        adj_period = FEC_PTP_SPINNER_2;
+                } else {
+                        adj_inc = FEC_PTP_SPINNER_4;
+                        adj_period = FEC_PTP_SPINNER_4;
+                }
+
+                for (i = 1; i < adj_inc; i++) {
+                        tmp_current = (ptp_ts_clk * i) % ndrift;
+                        if (tmp_current == 0) {
+                                ptc->corr_inc = i;
+                                ptc->corr_period = (u32)((ptp_ts_clk *
+                                                adj_period * i) / ndrift);
+                                break;
+                        } else if (tmp_current < tmp_winner) {
+                                ptc->corr_inc = i;
+                                ptc->corr_period = (u32)((ptp_ts_clk *
+                                                adj_period * i) / ndrift);
+                                tmp_winner = tmp_current;
+                        }
+                }
+        }
+}
+
+static void fec_set_drift(struct fec_enet_private *priv,
+                          struct ptp_set_comp *comp)
+{
+        struct ptp_time_correct tc;
+        u32 tmp, corr_ns;
+        u32 ptp_inc;
+
+        memset(&tc, 0, sizeof(struct ptp_time_correct));
+        fec_handle_ptpdrift(priv, comp, &tc);
+        if (tc.corr_inc == 0)
+                return;
+
+        ptp_inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(priv->clk_ptp);
+        if (comp->o_ops == TRUE)
+                corr_ns = ptp_inc + tc.corr_inc;
+        else
+                corr_ns = ptp_inc - tc.corr_inc;
+
+        tmp = readl(priv->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK;
+        tmp |= corr_ns << FEC_T_INC_CORR_OFFSET;
+        writel(tmp, priv->hwp + FEC_ATIME_INC);
+        writel(tc.corr_period, priv->hwp + FEC_ATIME_CORR);
+}
+
 /**
  * fec_ptp_read - read raw cycle counter (to be used by time counter)
  * @cc: the cyclecounter structure
@@ -106,18 +550,25 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev)
         unsigned long flags;
         int inc;
 
-        inc = 1000000000 / fep->cycle_speed;
+        inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(fep->clk_ptp);
 
         /* grab the ptp lock */
         spin_lock_irqsave(&fep->tmreg_lock, flags);
+        writel(FEC_T_CTRL_RESTART, fep->hwp + FEC_ATIME_CTRL);
 
         /* 1ns counter */
         writel(inc << FEC_T_INC_OFFSET, fep->hwp + FEC_ATIME_INC);
 
-        /* use free running count */
-        writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD);
-
-        writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL);
+        if (fep->hwts_rx_en_ioctl || fep->hwts_tx_en_ioctl) {
+                writel(FEC_T_PERIOD_ONE_SEC, fep->hwp + FEC_ATIME_EVT_PERIOD);
+                /* start counter */
+                writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE,
+                                fep->hwp + FEC_ATIME_CTRL);
+        } else if (fep->hwts_tx_en || fep->hwts_tx_en) {
+                /* use free running count */
+                writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD);
+                writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL);
+        }
 
         memset(&fep->cc, 0, sizeof(fep->cc));
         fep->cc.read = fec_ptp_read;
@@ -277,50 +728,140 @@ static int fec_ptp_enable(struct ptp_clock_info *ptp,
 int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
 {
         struct fec_enet_private *fep = netdev_priv(ndev);
-
         struct hwtstamp_config config;
-
-        if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
-                return -EFAULT;
-
-        /* reserved for future extensions */
-        if (config.flags)
-                return -EINVAL;
-
-        switch (config.tx_type) {
-        case HWTSTAMP_TX_OFF:
+        struct ptp_rtc_time curr_time;
+        struct ptp_time rx_time, tx_time;
+        struct fec_ptp_ts_data p_ts;
+        struct fec_ptp_ts_data *p_ts_user;
+        struct ptp_set_comp p_comp;
+        u32 freq_compensation;
+        int retval = 0;
+
+        switch (cmd) {
+        case SIOCSHWTSTAMP:
+                if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+                        return -EFAULT;
+
+                /* reserved for future extensions */
+                if (config.flags)
+                        return -EINVAL;
+
+                switch (config.tx_type) {
+                case HWTSTAMP_TX_OFF:
+                        fep->hwts_tx_en = 0;
+                        break;
+                case HWTSTAMP_TX_ON:
+                        fep->hwts_tx_en = 1;
+                        fep->hwts_tx_en_ioctl = 0;
+                        break;
+                default:
+                        return -ERANGE;
+                }
+
+                switch (config.rx_filter) {
+                case HWTSTAMP_FILTER_NONE:
+                        if (fep->hwts_rx_en)
+                                fep->hwts_rx_en = 0;
+                        config.rx_filter = HWTSTAMP_FILTER_NONE;
+                        break;
+
+                default:
+                        /*
+                         * register RXMTRL must be set in order
+                         * to do V1 packets, therefore it is not
+                         * possible to time stamp both V1 Sync and
+                         * Delay_Req messages and hardware does not support
+                         * timestamping all packets => return error
+                         */
+                        fep->hwts_rx_en = 1;
+                        fep->hwts_rx_en_ioctl = 0;
+                        config.rx_filter = HWTSTAMP_FILTER_ALL;
+                        break;
+                }
+
+                fec_ptp_start_cyclecounter(ndev);
+                return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+                    -EFAULT : 0;
+                break;
+        case PTP_ENBL_TXTS_IOCTL:
+        case PTP_ENBL_RXTS_IOCTL:
+                fep->hwts_rx_en_ioctl = 1;
+                fep->hwts_tx_en_ioctl = 1;
+                fep->hwts_rx_en = 0;
                 fep->hwts_tx_en = 0;
+                fec_ptp_start_cyclecounter(ndev);
                 break;
-        case HWTSTAMP_TX_ON:
-                fep->hwts_tx_en = 1;
+        case PTP_DSBL_RXTS_IOCTL:
+        case PTP_DSBL_TXTS_IOCTL:
+                fep->hwts_rx_en_ioctl = 0;
+                fep->hwts_tx_en_ioctl = 0;
                 break;
-        default:
-                return -ERANGE;
-        }
-
-        switch (config.rx_filter) {
-        case HWTSTAMP_FILTER_NONE:
-                if (fep->hwts_rx_en)
-                        fep->hwts_rx_en = 0;
-                config.rx_filter = HWTSTAMP_FILTER_NONE;
+        case PTP_GET_RX_TIMESTAMP:
+                p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
+                if (0 != copy_from_user(&p_ts.ident,
+                        &p_ts_user->ident, sizeof(p_ts.ident)))
+                        return -EINVAL;
+                retval = fec_ptp_find_and_remove(&fep->rx_timestamps,
+                                &p_ts.ident, &rx_time);
+                if (retval == 0 &&
+                        copy_to_user((void __user *)(&p_ts_user->ts),
+                                &rx_time, sizeof(rx_time)))
+                        return -EFAULT;
                 break;
-
-        default:
-                /*
-                 * register RXMTRL must be set in order to do V1 packets,
-                 * therefore it is not possible to time stamp both V1 Sync and
-                 * Delay_Req messages and hardware does not support
-                 * timestamping all packets => return error
-                 */
-                fep->hwts_rx_en = 1;
-                config.rx_filter = HWTSTAMP_FILTER_ALL;
+        case PTP_GET_TX_TIMESTAMP:
+                p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
+                if (0 != copy_from_user(&p_ts.ident,
+                        &p_ts_user->ident, sizeof(p_ts.ident)))
+                        return -EINVAL;
+                retval = fec_ptp_find_and_remove(&fep->tx_timestamps,
+                                &p_ts.ident, &tx_time);
+                if (retval == 0 &&
+                        copy_to_user((void __user *)(&p_ts_user->ts),
+                                &tx_time, sizeof(tx_time)))
+                        retval = -EFAULT;
+                break;
+        case PTP_GET_CURRENT_TIME:
+                fec_get_curr_cnt(fep, &curr_time);
+                if (0 != copy_to_user(ifr->ifr_data,
+                                        &(curr_time.rtc_time),
+                                        sizeof(struct ptp_time)))
+                        return -EFAULT;
+                break;
+        case PTP_SET_RTC_TIME:
+                if (0 != copy_from_user(&(curr_time.rtc_time),
+                                        ifr->ifr_data,
+                                        sizeof(struct ptp_time)))
+                        return -EINVAL;
+                fec_set_1588cnt(fep, &curr_time);
                 break;
+        case PTP_FLUSH_TIMESTAMP:
+                /* reset tx-timestamping buffer */
+                fep->tx_timestamps.front = 0;
+                fep->tx_timestamps.end = 0;
+                fep->tx_timestamps.size = (DEFAULT_PTP_TX_BUF_SZ + 1);
+                /* reset rx-timestamping buffer */
+                fep->rx_timestamps.front = 0;
+                fep->rx_timestamps.end = 0;
+                fep->rx_timestamps.size = (DEFAULT_PTP_RX_BUF_SZ + 1);
+                break;
+        case PTP_SET_COMPENSATION:
+                if (0 != copy_from_user(&p_comp, ifr->ifr_data,
+                        sizeof(struct ptp_set_comp)))
+                        return -EINVAL;
+                fec_set_drift(fep, &p_comp);
+                break;
+        case PTP_GET_ORIG_COMP:
+                freq_compensation = FEC_PTP_ORIG_COMP;
+                if (copy_to_user(ifr->ifr_data, &freq_compensation,
+                                        sizeof(freq_compensation)) > 0)
+                        return -EFAULT;
+                break;
+        default:
+                return -EINVAL;
         }
 
-        return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
-            -EFAULT : 0;
+        return retval;
 }
-
 /**
  * fec_time_keep - call timecounter_read every second to avoid timer overrun
  *                 because ENET just support 32bit counter, will timeout in 4s
@@ -383,4 +924,22 @@ void fec_ptp_init(struct platform_device *pdev)
                 fep->ptp_clock = NULL;
                 pr_err("ptp_clock_register failed\n");
         }
+
+        /* initialize circular buffer for tx timestamps */
+        if (fec_ptp_init_circ(&(fep->tx_timestamps),
+                (DEFAULT_PTP_TX_BUF_SZ+1)))
+                pr_err("init tx circular buffer failed\n");
+        /* initialize circular buffer for rx timestamps */
+        if (fec_ptp_init_circ(&(fep->rx_timestamps),
+                        (DEFAULT_PTP_RX_BUF_SZ+1)))
+                pr_err("init rx curcular buffer failed\n");
 }
+
+void fec_ptp_cleanup(struct fec_enet_private *priv)
+{
+        if (priv->tx_timestamps.data_buf)
+                vfree(priv->tx_timestamps.data_buf);
+        if (priv->rx_timestamps.data_buf)
+                vfree(priv->rx_timestamps.data_buf);
+}
+