gianfar: Add IEEE 1588 V2 support

This patch integrates the IEEE 1588 specification v2 ioctls with
the gianfar driver. The user space application should be IXXAT
IEEE1588 stack v1.04 or above.

Signed-off-by: Sandeep Gopalpet <sandeep.kumar@freescale.com>
Signed-off-by: Bhaskar Upadhaya <Bhaskar.Upadhaya@freescale.com>
Signed-off-by: Yangbo Lu <yangbo.lu@freescale.com>
Signed-off-by: Xie Xiaobo <X.Xie@freescale.com>
Change-Id: Ic7c3b4f2d5dd92d4e8e82dea3d41df8e75cf3559
Reviewed-on: http://git.am.freescale.net:8181/10331
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Claudiu Manoil <claudiu.manoil@freescale.com>
Reviewed-by: Jose Rivera <German.Rivera@freescale.com>

5 files changed, 1035 insertions, 5 deletions

diff --git a/drivers/net/ethernet/freescale/Kconfig b/drivers/net/ethernet/freescale/Kconfig
index ae84383..82fa7ae 100644
--- a/drivers/net/ethernet/freescale/Kconfig
+++ b/drivers/net/ethernet/freescale/Kconfig
@@ -95,6 +95,16 @@ config GIANFAR
 	---help---
 	  This driver supports the Gigabit TSEC on the MPC83xx, MPC85xx,
 	  and MPC86xx family of chips, and the FEC on the 8540.
+
+config FSL_GIANFAR_1588
+	bool "Gianfar 1588"
+	depends on GIANFAR
+	---help---
+	  This driver supports Freescale eTSEC 1588 function.
+	  Integrate the IEEE 1588 specification v2 ioctls with the gianfar driver.
+	  The user space application should be IXXAT IEEE 1588 stack v1.04 or above.
+	  Say Y here if you want to use IXXAT 1588 stack.
+
 config RX_TX_BUFF_XCHG
         default y
 	 bool "RX and TX ring buffer exchange for Routed packets"
diff --git a/drivers/net/ethernet/freescale/Makefile b/drivers/net/ethernet/freescale/Makefile
index 3092ee1..ec83354 100644
--- a/drivers/net/ethernet/freescale/Makefile
+++ b/drivers/net/ethernet/freescale/Makefile
@@ -17,5 +17,6 @@ obj-$(if $(CONFIG_FSL_DPAA_ETH),y) += dpa/
 obj-$(CONFIG_PTP_1588_CLOCK_GIANFAR) += gianfar_ptp.o
 gianfar_driver-objs := gianfar.o \
 		gianfar_ethtool.o
+gianfar_driver-$(CONFIG_FSL_GIANFAR_1588) += gianfar_1588.o
 obj-$(CONFIG_UCC_GETH) += ucc_geth_driver.o
 ucc_geth_driver-objs := ucc_geth.o ucc_geth_ethtool.o
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index cc9f6b0..e03954f 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -908,7 +908,11 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 
 	/* Find the TBI PHY.  If it's not there, we don't support SGMII */
 	priv->tbi_node = of_parse_phandle(np, "tbi-handle", 0);
-
+#if defined CONFIG_FSL_GIANFAR_1588
+	/* Handle IEEE1588 node */
+	if (!gfar_ptp_init(np, priv))
+		dev_info(&ofdev->dev, "ptp 1588 is initialized.\n");
+#endif
 	return 0;
 
 err_grp_init:
@@ -982,8 +986,15 @@ static int gfar_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 	if (!priv->phydev)
 		return -ENODEV;
-
+#if defined CONFIG_FSL_GIANFAR_1588
+	if ((cmd >= PTP_ENBL_TXTS_IOCTL) &&
+			(cmd <= PTP_CLEANUP_TS))
+		return gfar_ioctl_1588(dev, rq, cmd);
+	else
+		return phy_mii_ioctl(priv->phydev, rq, cmd);
+#else
 	return phy_mii_ioctl(priv->phydev, rq, cmd);
+#endif
 }
 
 static u32 cluster_entry_per_class(struct gfar_private *priv, u32 rqfar,
@@ -1424,6 +1435,9 @@ static int gfar_probe(struct platform_device *ofdev)
 	return 0;
 
 register_fail:
+#if defined CONFIG_FSL_GIANFAR_1588
+	gfar_ptp_cleanup(priv);
+#endif
 	unmap_group_regs(priv);
 	gfar_free_rx_queues(priv);
 	gfar_free_tx_queues(priv);
@@ -2193,9 +2207,13 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	do_csum = (CHECKSUM_PARTIAL == skb->ip_summed);
 	do_vlan = vlan_tx_tag_present(skb);
+#if defined CONFIG_FSL_GIANFAR_1588
+	do_tstamp = unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
+		    priv->hwts_tx_en) || unlikely(priv->hwts_tx_en_ioctl);
+#else
 	do_tstamp = (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
 		    priv->hwts_tx_en;
-
+#endif
 	if (do_csum || do_vlan)
 		fcb_len = GMAC_FCB_LEN;
 
@@ -2324,8 +2342,21 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Setup tx hardware time stamping if requested */
 	if (unlikely(do_tstamp)) {
+#if defined CONFIG_FSL_GIANFAR_1588
+		u32 vlan_ctrl;
+#endif
 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 		fcb->ptp = 1;
+#if defined CONFIG_FSL_GIANFAR_1588
+		/* When PTP in FCB is enabled, VLN in FCB is ignored.
+		 * Instead VLAN tag is read from DFVLAN register. Thus need
+		 * to copy VLCTL to DFVLAN register.
+		 */
+		vlan_ctrl = gfar_read(&regs->dfvlan);
+		vlan_ctrl &= ~0xFFFF;
+		vlan_ctrl |= (fcb->vlctl & 0xFFFF);
+		gfar_write(&regs->dfvlan, vlan_ctrl);
+#endif
 	}
 
 	txbdp_start->bufPtr = dma_map_single(priv->dev, skb->data,
@@ -2566,13 +2597,48 @@ static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 				 buflen, DMA_TO_DEVICE);
 
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) {
+#if defined CONFIG_FSL_GIANFAR_1588
+			struct gfar __iomem *regs = priv->gfargrp[0].regs;
+#endif
 			struct skb_shared_hwtstamps shhwtstamps;
+#if defined CONFIG_FSL_GIANFAR_1588
+			u32 high, low;
+			struct gfar_ptp_time tx_ts;
+			u64 ns;
+
+			if (priv->device_flags &
+					FSL_GIANFAR_DEV_HAS_TS_TO_BUFFER) {
+				/* get tx timestamp out of frame */
+				void *ts;
+				ts = (void *)(((uintptr_t)skb->data + 0x10)
+						& ~0x7);
+				ns = be64_to_cpup(ts);
+			} else
+				/* get tx timestamp from register */
+				ns = gfar_get_tx_timestamp(regs);
+
+			if (unlikely(priv->hwts_tx_en))
+				shhwtstamps.hwtstamp = ns_to_ktime(ns);
+			if (likely(priv->hwts_tx_en_ioctl)) {
+				high = upper_32_bits(ns);
+				low = lower_32_bits(ns);
+				gfar_cnt_to_ptp_time(high, low, &tx_ts);
+			}
+			/* remove tx fcb */
+			skb_pull(skb, GMAC_FCB_LEN + GMAC_TXPAL_LEN);
+			/* pass timestamp back */
+			if (unlikely(priv->hwts_tx_en))
+				skb_tstamp_tx(skb, &shhwtstamps);
+			if (likely(priv->hwts_tx_en_ioctl))
+				gfar_ptp_store_txstamp(dev, skb, &tx_ts);
+#else
 			u64 *ns = (u64*) (((u32)skb->data + 0x10) & ~0x7);
 
 			memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 			shhwtstamps.hwtstamp = ns_to_ktime(*ns);
 			skb_pull(skb, GMAC_FCB_LEN + GMAC_TXPAL_LEN);
 			skb_tstamp_tx(skb, &shhwtstamps);
+#endif
 			bdp->lstatus &= BD_LFLAG(TXBD_WRAP);
 			bdp = next;
 		}
@@ -2774,6 +2840,32 @@ static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	}
 
 	/* Get receive timestamp from the skb */
+#if defined CONFIG_FSL_GIANFAR_1588
+	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER) {
+		u32 high, low;
+
+		/* get timestamp */
+		high = *((u32 *)skb->data);
+		low = *(((u32 *)skb->data) + 1);
+		skb_pull(skb, 8);
+		/* proprietary PTP timestamping over ioctl */
+		if (unlikely(priv->hwts_rx_en_ioctl)) {
+			struct gfar_ptp_time rx_ts;
+			/* get rx timestamp */
+			gfar_cnt_to_ptp_time(high, low, &rx_ts);
+			/* parse and store rx timestamp */
+			gfar_ptp_store_rxstamp(dev, skb, &rx_ts);
+		} else if (unlikely(priv->hwts_rx_en)) {
+			/* kernel-API timestamping ? */
+			u64 nsec;
+			struct skb_shared_hwtstamps *hws;
+			hws = skb_hwtstamps(skb);
+			nsec = make64(high, low);
+			hws->hwtstamp = ns_to_ktime(nsec);
+		}
+	} else if (priv->padding)
+			skb_pull(skb, priv->padding);
+#else
 	if (priv->hwts_rx_en) {
 		struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
 		u64 *ns = (u64 *) skb->data;
@@ -2784,7 +2876,7 @@ static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 
 	if (priv->padding)
 		skb_pull(skb, priv->padding);
-
+#endif
 	if (dev->features & NETIF_F_RXCSUM)
 		gfar_rx_checksum(skb, fcb);
 
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 34b8e38..e3e3c68 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -47,6 +47,10 @@
 #include <linux/workqueue.h>
 #include <linux/ethtool.h>
 
+#if defined CONFIG_FSL_GIANFAR_1588
+#include <linux/circ_buf.h>
+#endif
+
 struct ethtool_flow_spec_container {
 	struct ethtool_rx_flow_spec fs;
 	struct list_head list;
@@ -172,6 +176,62 @@ static inline int devfp_register_tx_hook(devfp_hook_t hook)
 		| SUPPORTED_Pause \
 		| SUPPORTED_Asym_Pause)
 
+#if defined CONFIG_FSL_GIANFAR_1588
+/* 1588 defines */
+#define make64(high, low) (((u64)(high) << 32) | (low))
+
+#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_TIME		(SIOCDEVPRIVATE + 6)
+#define PTP_GET_TIME		(SIOCDEVPRIVATE + 7)
+#define PTP_SET_FIPER_ALARM	(SIOCDEVPRIVATE + 8)
+#define PTP_SET_ADJ		(SIOCDEVPRIVATE + 9)
+#define PTP_GET_ADJ		(SIOCDEVPRIVATE + 10)
+#define PTP_CLEANUP_TS		(SIOCDEVPRIVATE + 11)
+
+#define DEFAULT_PTP_TX_BUF_SZ		1024
+#define DEFAULT_PTP_RX_BUF_SZ		2048
+
+/* The threshold between the current found one and the oldest one */
+#define TS_ACCUMULATION_THRESHOLD	50
+
+#define GFAR_PTP_SOURCE_PORT_LENGTH	10
+#define	GFAR_PTP_HEADER_SEQ_OFFS	30
+#define GFAR_PTP_SPID_OFFS		20
+#define GFAR_PTP_HEADER_SZE		34
+#define GFAR_PTP_EVENT_PORT		0x013F
+
+#define GFAR_VLAN_QINQ_1		0x9100
+#define GFAR_VLAN_QINQ_2		0x9200
+#define GFAR_VLAN_QINQ_3		0x9300
+#define GFAR_VLAN_QINQ_4		0x88A8
+#define GFAR_VLAN_TAG_LEN		0x04
+#define GFAR_ETHTYPE_LEN		0x02
+#define GFAR_PACKET_TYPE_UDP		0x11
+/* 1588-2008 network protocol enumeration values */
+#define GFAR_PTP_PROT_IPV4		1
+#define GFAR_PTP_PROT_IPV6		2
+#define GFAR_PTP_PROT_802_3		3
+#define GFAR_PTP_PROT_DONTCARE		0xFFFF
+
+/* 1588 Module Registers bits */
+#define TMR_CTRL_CKSEL_MASK	0x00000003
+#define TMR_CTRL_ENABLE		0x00000004
+#define TMR_RTPE		0x00008000
+#define TMR_CTRL_TCLK_MASK	0x03ff0000
+#define TMR_CTRL_FIPER_START	0x10000000
+#define ONE_GIGA	1000000000
+
+/*Alarm to traigger at 15sec boundary */
+#define TMR_ALARM1_L	0xD964B800
+#define TMR_ALARM1_H	0x00000045
+#define NANOSEC_PER_SEC	1000000000
+#endif
+
 /* TBI register addresses */
 #define MII_TBICON		0x11
 
@@ -682,6 +742,80 @@ struct gfar_extra_stats {
 /* Number of stats exported via ethtool */
 #define GFAR_STATS_LEN (GFAR_RMON_LEN + GFAR_EXTRA_STATS_LEN)
 
+#if defined CONFIG_FSL_GIANFAR_1588
+/* IEEE-1588 Timer Controller Registers */
+struct gfar_regs_1588 {
+	u32	tmr_ctrl;	/* 0x.e00 - Timer Control Register */
+	u32	tmr_tevent;	/* 0x.e04 - Timer stamp event register */
+	u32	tmr_temask;	/* 0x.e08 - Timer event mask register */
+	u32	tmr_pevent;	/* 0x.e0c - Timer stamp event register */
+	u32	tmr_pemask;	/* 0x.e10 - Timer event mask register */
+	u32	tmr_stat;	/* 0x.e14 - Timer stamp status register */
+	u32	tmr_cnt_h;	/* 0x.e18 - Timer counter high register */
+	u32	tmr_cnt_l;	/* 0x.e1c - Timer counter low register */
+	u32	tmr_add;	/* 0x.e20 - Timer dirft compensation */
+					/* addend register */
+	u32	tmr_acc;	/* 0x.e24 - Timer accumulator register */
+	u32	tmr_prsc;	/* 0x.e28 - Timer prescale register */
+	u8	res24a[4];	/* 0x.e2c - 0x.e2f reserved */
+	u32	tmr_off_h;	/* 0x.e30 - Timer offset high register */
+	u32	tmr_off_l;	/* 0x.e34 - Timer offset low register */
+	u8	res24b[8];	/* 0x.e38 - 0x.e3f reserved */
+	u32	tmr_alarm1_h;	/* 0x.e40 - Timer alarm 1 high register */
+	u32	tmr_alarm1_l;	/* 0x.e44 - Timer alarm 1 low register */
+	u32	tmr_alarm2_h;	/* 0x.e48 - Timer alarm 2 high register */
+	u32	tmr_alarm2_l;	/* 0x.e4c - Timer alarm 2 low register */
+	u8	res24c[48];	/* 0x.e50 - 0x.e7f reserved */
+	u32	tmr_fiper1;	/* 0x.e80 - Timer fixed period register 1 */
+	u32	tmr_fiper2;	/* 0x.e84 - Timer fixed period register 2 */
+	u32	tmr_fiper3;	/* 0x.e88 - Timer fixed period register 3 */
+	u8	res24d[20];	/* 0x.e8c - 0x.ebf reserved */
+	u32	tmr_etts1_h;	/* 0x.ea0 - Timer stamp high of */
+					/* general purpose external trigger 1 */
+	u32	tmr_etts1_l;	/* 0x.ea4 - Timer stamp low of */
+					/* general purpose external trigger 1 */
+	u32	tmr_etts2_h;	/* 0x.ea8 - Timer stamp high of */
+					/* general purpose external trigger 2 */
+	u32	tmr_etts2_l;	/* 0x.eac - Timer stamp low of */
+};
+
+/* struct needed to identify a timestamp */
+struct gfar_ptp_ident {
+	u8  version;
+	u8  msg_type;
+	u16 netw_prot;
+	u16 seq_id;
+	u8  snd_port_id[GFAR_PTP_SOURCE_PORT_LENGTH];
+};
+
+/* timestamp format in 1588-2008 */
+struct gfar_ptp_time {
+	u64 sec; /* just 48 bit used */
+	u32 nsec;
+};
+
+/* needed for timestamp data over ioctl */
+struct gfar_ptp_data {
+	struct  gfar_ptp_ident  ident;
+	struct  gfar_ptp_time   ts;
+};
+
+/* circular buffer for ptp timestamps over ioctl */
+struct gfar_ptp_circular {
+	struct circ_buf circ_buf;
+	u32 size;
+	spinlock_t ptp_lock;
+};
+
+struct gfar_ptp_attr_t {
+	u32 tclk_period;
+	u32 nominal_freq;
+	u32 sysclock_freq;
+	u32 tmr_fiper1;
+	u32 freq_comp;
+};
+#endif
+
 struct gfar {
 	u32	tsec_id;	/* 0x.000 - Controller ID register */
 	u32	tsec_id2;	/* 0x.004 - Controller ID2 register */
@@ -754,7 +888,19 @@ struct gfar {
 	u32	tbase6;		/* 0x.234 - TxBD Base Address of ring 6 */
 	u8	res10g[4];
 	u32	tbase7;		/* 0x.23c - TxBD Base Address of ring 7 */
+#if defined CONFIG_FSL_GIANFAR_1588
+	u8	res10h[64];
+	u32	tmr_txts1_id;	/* 0x.280 Tx time stamp identification */
+	u32	tmr_txts2_id;	/* 0x.284 Tx time stamp Identification */
+	u8	res10i[56];
+	u32	tmr_txts1_h;	/* 0x.2c0 Tx time stamp high */
+	u32	tmr_txts1_l;	/* 0x.2c4 Tx Time Stamp low */
+	u32	tmr_txts2_h;	/* 0x.2c8 Tx time stamp high */
+	u32	tmr_txts2_l;	/* 0x.2cc  Tx Time Stamp low */
+	u8	res10j[48];
+#else
 	u8	res10[192];
+#endif
 	u32	rctrl;		/* 0x.300 - Receive Control Register */
 	u32	rstat;		/* 0x.304 - Receive Status Register */
 	u8	res12[8];
@@ -805,7 +951,14 @@ struct gfar {
 	u32	rbase6;		/* 0x.434 - RxBD base address of ring 6 */
 	u8	res17g[4];
 	u32	rbase7;		/* 0x.43c - RxBD base address of ring 7 */
+#if defined CONFIG_FSL_GIANFAR_1588
+	u8	res17h[128];
+	u32	tmr_rxts_h;	/* 0x.4c0 Rx Time Stamp high */
+	u32	tmr_rxts_l;	/* 0x.4c4 Rx Time Stamp low */
+	u8	res17i[56];
+#else
 	u8	res17[192];
+#endif
 	u32	maccfg1;	/* 0x.500 - MAC Configuration 1 Register */
 	u32	maccfg2;	/* 0x.504 - MAC Configuration 2 Register */
 	u32	ipgifg;		/* 0x.508 - Inter Packet Gap/Inter Frame Gap Register */
@@ -874,7 +1027,12 @@ struct gfar {
 	u8	res23c[248];
 	u32	attr;		/* 0x.bf8 - Attributes Register */
 	u32	attreli;	/* 0x.bfc - Attributes Extract Length and Extract Index Register */
+#if defined CONFIG_FSL_GIANFAR_1588
+	u8	res24[512];
+	struct gfar_regs_1588 regs_1588;
+#else
 	u8	res24[688];
+#endif
 	u32	isrg0;		/* 0x.eb0 - Interrupt steering group 0 register */
 	u32	isrg1;		/* 0x.eb4 - Interrupt steering group 1 register */
 	u32	isrg2;		/* 0x.eb8 - Interrupt steering group 2 register */
@@ -913,6 +1071,10 @@ struct gfar {
 #define FSL_GIANFAR_DEV_HAS_BUF_STASHING	0x00000400
 #define FSL_GIANFAR_DEV_HAS_TIMER		0x00000800
 
+#if defined CONFIG_FSL_GIANFAR_1588
+#define FSL_GIANFAR_DEV_HAS_TS_TO_BUFFER	0x00001000
+#endif
+
 #if (MAXGROUPS == 2)
 #define DEFAULT_MAPPING 	0xAA
 #else
@@ -1109,10 +1271,15 @@ struct gfar_private {
 	u16 padding;
 	u32 device_flags;
 
+#if defined CONFIG_FSL_GIANFAR_1588
+	/* HW TX timestamping enabled flag */
+	u16 hwts_tx_en;
+	u16 hwts_tx_en_ioctl;
+#else
 	/* HW time stamping enabled flag */
 	int hwts_rx_en;
 	int hwts_tx_en;
-
+#endif
 	struct gfar_priv_tx_q *tx_queue[MAX_TX_QS];
 	struct gfar_priv_rx_q *rx_queue[MAX_RX_QS];
 	struct gfar_priv_grp gfargrp[MAXGROUPS];
@@ -1128,6 +1295,17 @@ struct gfar_private {
 	/* Network Statistics */
 	struct gfar_extra_stats extra_stats;
 
+#if defined CONFIG_FSL_GIANFAR_1588
+	/* HW RX timestamping enabled flag */
+	u16 hwts_rx_en;
+	u16 hwts_rx_en_ioctl;
+
+	/* 1588 stuff */
+	struct gfar_regs_1588 __iomem *ptimer;
+	struct gfar_ptp_circular tx_timestamps;
+	struct gfar_ptp_circular rx_timestamps;
+#endif
+
 	/* PHY stuff */
 	phy_interface_t interface;
 	struct device_node *phy_node;
@@ -1256,6 +1434,19 @@ void reset_gfar(struct net_device *dev);
 void gfar_mac_reset(struct gfar_private *priv);
 void gfar_halt(struct gfar_private *priv);
 void gfar_start(struct gfar_private *priv);
+#if defined CONFIG_FSL_GIANFAR_1588
+void gfar_1588_start(struct gfar_private *priv);
+void gfar_1588_stop(struct gfar_private *priv);
+int gfar_ptp_init(struct device_node *np, struct gfar_private *priv);
+void gfar_ptp_cleanup(struct gfar_private *priv);
+int gfar_ioctl_1588(struct net_device *dev, struct ifreq *ifr, int cmd);
+void gfar_ptp_store_txstamp(struct net_device *dev,
+			struct sk_buff *skb, struct gfar_ptp_time *tx_ts);
+void gfar_ptp_store_rxstamp(struct net_device *dev,
+			struct sk_buff *skb, struct gfar_ptp_time *rx_ts);
+void gfar_cnt_to_ptp_time(u32 high, u32 low, struct gfar_ptp_time *time);
+u64 gfar_get_tx_timestamp(struct gfar __iomem *regs);
+#endif
 void gfar_phy_test(struct mii_bus *bus, struct phy_device *phydev, int enable,
 		   u32 regnum, u32 read);
 void gfar_configure_coalescing_all(struct gfar_private *priv);
diff --git a/drivers/net/ethernet/freescale/gianfar_1588.c b/drivers/net/ethernet/freescale/gianfar_1588.c
new file mode 100644
index 0000000..0a9e70a
--- /dev/null
+++ b/drivers/net/ethernet/freescale/gianfar_1588.c
@@ -0,0 +1,736 @@
+/* drivers/net/gianfar_1588.c
+ *
+ * Copyright 2008-2012 Freescale Semiconductor, Inc.
+ * Copyright 2009-2011 IXXAT Automation GmbH
+ *
+ * Author: Anup Gangwar <anup.gangwar@freescale.com>
+ *	   Yashpal Dutta <yashpal.dutta@freescale.com>
+ *
+ * Gianfar Ethernet Driver -- IEEE 1588 interface functionality
+ *
+ * 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.
+*/
+
+#include <linux/vmalloc.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/if_vlan.h>
+#include <linux/net_tstamp.h>
+#include <linux/of_platform.h>
+#include <linux/proc_fs.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/udp.h>
+#include "gianfar.h"
+
+static struct gfar_ptp_attr_t ptp_attr;
+
+static bool gfar_ptp_init_circ(struct gfar_ptp_circular *buf, int size)
+{
+	struct circ_buf *circ_buf = &buf->circ_buf;
+	circ_buf->buf = vmalloc(size * sizeof(struct gfar_ptp_data));
+
+	if (!circ_buf->buf)
+		return 1;
+
+	circ_buf->head = 0;
+	circ_buf->tail = 0;
+	buf->size = size;
+	spin_lock_init(&buf->ptp_lock);
+
+	return 0;
+}
+
+static bool gfar_ptp_is_full(struct gfar_ptp_circular *buf)
+{
+	struct circ_buf *circ_buf = &buf->circ_buf;
+	bool ret;
+
+	ret = CIRC_SPACE(circ_buf->head, circ_buf->tail,
+					buf->size) == 0;
+	return ret;
+}
+
+static bool gfar_ptp_is_empty(struct gfar_ptp_circular *buf)
+{
+	struct circ_buf *circ_buf = &buf->circ_buf;
+	bool ret;
+
+	ret = CIRC_CNT(circ_buf->head, circ_buf->tail,
+					buf->size) == 0;
+	return ret;
+}
+
+static bool gfar_ptp_insert(struct gfar_ptp_circular *buf,
+			struct gfar_ptp_data *data)
+{
+	struct gfar_ptp_data *tmp;
+	struct circ_buf *circ_buf = &buf->circ_buf;
+	unsigned int head;
+	unsigned long flags;
+
+	spin_lock_irqsave(&buf->ptp_lock, flags);
+	if (gfar_ptp_is_full(buf)) {
+		spin_unlock_irqrestore(&buf->ptp_lock, flags);
+		return 1;
+	}
+
+	head = circ_buf->head;
+	tmp = (struct gfar_ptp_data *)circ_buf->buf + head;
+	memcpy(tmp, data, sizeof(struct gfar_ptp_data));
+	circ_buf->head = (head + 1) & (buf->size - 1);
+	spin_unlock_irqrestore(&buf->ptp_lock, flags);
+
+	return 0;
+}
+
+static int gfar_ptp_is_ident_match(struct gfar_ptp_ident *dst,
+				  struct gfar_ptp_ident *src)
+{
+	int ret;
+
+	if ((dst->version != src->version) ||
+			(dst->msg_type != src->msg_type))
+		return 0;
+
+	if ((dst->netw_prot == src->netw_prot)
+			|| src->netw_prot == GFAR_PTP_PROT_DONTCARE) {
+		if (dst->seq_id != src->seq_id)
+			return 0;
+
+		ret = memcmp(dst->snd_port_id, src->snd_port_id,
+				GFAR_PTP_SOURCE_PORT_LENGTH);
+		if (ret)
+			return 0;
+		else
+			return 1;
+	}
+
+	return 0;
+}
+
+static bool gfar_ptp_find_and_remove(struct gfar_ptp_circular *buf,
+				struct gfar_ptp_ident *ident,
+				struct gfar_ptp_time *ts)
+{
+	struct circ_buf *circ_buf = &buf->circ_buf;
+	unsigned int size = buf->size;
+	unsigned int head, tail, idx;
+	struct gfar_ptp_data *tmp, *tmp2;
+	struct gfar_ptp_ident *tmp_ident;
+	unsigned long flags;
+
+	spin_lock_irqsave(&buf->ptp_lock, flags);
+	if (gfar_ptp_is_empty(buf)) {
+		spin_unlock_irqrestore(&buf->ptp_lock, flags);
+		return 1;
+	}
+
+	head = circ_buf->head;
+	tail = idx = circ_buf->tail;
+
+	while (idx != head) {
+		tmp = (struct gfar_ptp_data *)(circ_buf->buf) + idx;
+		tmp_ident = &tmp->ident;
+		if (gfar_ptp_is_ident_match(tmp_ident, ident))
+				break;
+
+		/* get next */
+		idx = (idx + 1) & (size - 1);
+	}
+
+	/* not found ? */
+	if (idx == head) {
+		spin_unlock_irqrestore(&buf->ptp_lock, flags);
+		return 1;
+	}
+
+	*ts = tmp->ts;
+
+	if (idx != tail) {
+		if (CIRC_CNT(idx, tail, size) > TS_ACCUMULATION_THRESHOLD) {
+			tail = circ_buf->tail =
+				(idx - TS_ACCUMULATION_THRESHOLD) & (size - 1);
+		}
+
+		while (CIRC_CNT(idx, tail, size) > 0) {
+			tmp = (struct gfar_ptp_data *)(circ_buf->buf) + idx;
+			idx = (idx - 1) & (size - 1);
+			tmp2 = (struct gfar_ptp_data *)(circ_buf->buf) + idx;
+			*tmp = *tmp2;
+		}
+	}
+
+	/* set tail pointer to postion after found */
+	circ_buf->tail = (tail + 1) & (size - 1);
+
+	spin_unlock_irqrestore(&buf->ptp_lock, flags);
+
+	return 0;
+}
+
+
+/* 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.
+*/
+static void *gfar_ptp_parse_packet(struct sk_buff *skb, u16 *eth_type)
+{
+	struct iphdr *iph;
+	struct udphdr *udph;
+	struct ipv6hdr *ipv6h;
+	void *pos = skb->data + ETH_ALEN + ETH_ALEN;
+	u8 *ptp_loc = NULL;
+
+	*eth_type = be16_to_cpup(pos);
+
+	/* Check if outer vlan tag is here */
+	if (*eth_type == GFAR_VLAN_QINQ_1 ||
+			*eth_type == GFAR_VLAN_QINQ_2 ||
+			*eth_type == GFAR_VLAN_QINQ_3 ||
+			*eth_type == GFAR_VLAN_QINQ_4) {
+		pos += GFAR_VLAN_TAG_LEN;
+		*eth_type = be16_to_cpup(pos);
+	}
+
+	/* Check if inner tag is here */
+	if (*eth_type == ETH_P_8021Q) {
+		pos += GFAR_VLAN_TAG_LEN;
+		*eth_type = be16_to_cpup(pos);
+	}
+
+	/* set pos after ethertype */
+	pos += GFAR_ETHTYPE_LEN;
+
+	switch (*eth_type) {
+	case ETH_P_1588:
+		ptp_loc = pos;
+		if ((ptp_loc[0] & 0xF) <= 3) {
+			/* long enough ? */
+			if (skb->len >= ((ptp_loc - skb->data) +
+						GFAR_PTP_HEADER_SZE))
+				return ptp_loc;
+		}
+		break;
+	case ETH_P_IP:
+		iph = (struct iphdr *)pos;
+
+		if (ntohs(iph->protocol) != IPPROTO_UDP)
+			return NULL;
+
+		pos += iph->ihl * 4;
+		udph = (struct udphdr *)pos;
+
+		/* check the destination port address
+		 * ( 319 (0x013F) = PTP event port )
+		 */
+		if (ntohs(udph->dest) != GFAR_PTP_EVENT_PORT)
+			return NULL;
+
+		ptp_loc = pos + sizeof(struct udphdr);
+		/* long enough ? */
+		if (skb->len >= ((ptp_loc - skb->data)
+				+ GFAR_PTP_HEADER_SZE))
+			return ptp_loc;
+		break;
+	case ETH_P_IPV6:
+		ipv6h = (struct ipv6hdr *)pos;
+
+		if (ntohs(ipv6h->nexthdr) != IPPROTO_UDP)
+			return NULL;
+
+		pos += sizeof(struct ipv6hdr);
+		udph = (struct udphdr *)pos;
+
+		/* check the destination port address
+		 * ( 319 (0x013F) = PTP event port )
+		 */
+		if (ntohs(udph->dest) != GFAR_PTP_EVENT_PORT)
+			return NULL;
+
+		ptp_loc = pos + sizeof(struct udphdr);
+		/* long enough ? */
+		if (skb->len >= ((ptp_loc - skb->data)
+				+ GFAR_PTP_HEADER_SZE))
+			return ptp_loc;
+		break;
+	default:
+		break;
+	}
+
+	return NULL; /* no PTP frame */
+}
+
+/* Store the tx hardware timestamp with additional information
+ * in the gfar internal ringbuffer for timestamps.
+ */
+void gfar_ptp_store_txstamp(struct net_device *dev, struct sk_buff *skb,
+						struct gfar_ptp_time *tx_ts)
+{
+	struct gfar_ptp_data tmp_tx_time;
+	struct gfar_private *priv = netdev_priv(dev);
+	void  *ptp_loc;
+	u16 eth_type;
+
+	ptp_loc = gfar_ptp_parse_packet(skb, &eth_type);
+	if (ptp_loc == NULL)
+		return;
+
+	/* store identification data */
+	switch (eth_type) {
+	case ETH_P_IP:
+		tmp_tx_time.ident.netw_prot = GFAR_PTP_PROT_IPV4;
+		break;
+	case ETH_P_IPV6:
+		tmp_tx_time.ident.netw_prot = GFAR_PTP_PROT_IPV6;
+		break;
+	case ETH_P_1588:
+		tmp_tx_time.ident.netw_prot = GFAR_PTP_PROT_802_3;
+		break;
+	default:
+		return;
+	}
+
+	tmp_tx_time.ident.version = (*(u8 *)(ptp_loc + 1)) & 0X0F;
+	tmp_tx_time.ident.msg_type = (*(u8 *)(ptp_loc)) & 0x0F;
+	tmp_tx_time.ident.seq_id = be16_to_cpup(ptp_loc +
+					GFAR_PTP_HEADER_SEQ_OFFS);
+	memcpy(tmp_tx_time.ident.snd_port_id,
+	ptp_loc + GFAR_PTP_SPID_OFFS, GFAR_PTP_SOURCE_PORT_LENGTH);
+
+	/* store tx timestamp */
+	tmp_tx_time.ts = *tx_ts;
+
+	/* insert timestamp in circular buffer */
+	gfar_ptp_insert(&(priv->tx_timestamps), &tmp_tx_time);
+}
+
+/* Store the rx hardware timestamp with additional information
+ * in the gfar internal ringbuffer for timestamps.
+ */
+void gfar_ptp_store_rxstamp(struct net_device *dev, struct sk_buff *skb,
+						struct gfar_ptp_time *rx_ts)
+{
+	struct gfar_ptp_data tmp_rx_time;
+	struct gfar_private *priv = netdev_priv(dev);
+	void *ptp_loc;
+	u16 eth_type;
+
+	ptp_loc = gfar_ptp_parse_packet(skb, &eth_type);
+	if (ptp_loc == NULL)
+		return;
+
+	/* store identification data */
+	switch (eth_type) {
+	case ETH_P_IP:
+		tmp_rx_time.ident.netw_prot = GFAR_PTP_PROT_IPV4;
+		break;
+	case ETH_P_IPV6:
+		tmp_rx_time.ident.netw_prot = GFAR_PTP_PROT_IPV6;
+		break;
+	case ETH_P_1588:
+		tmp_rx_time.ident.netw_prot = GFAR_PTP_PROT_802_3;
+		break;
+	default:
+		return;
+	}
+
+	tmp_rx_time.ident.version = (*(u8 *)(ptp_loc + 1)) & 0X0F;
+	tmp_rx_time.ident.msg_type = (*(u8 *)(ptp_loc)) & 0x0F;
+	tmp_rx_time.ident.seq_id = be16_to_cpup(ptp_loc +
+					GFAR_PTP_HEADER_SEQ_OFFS);
+	memcpy(tmp_rx_time.ident.snd_port_id,
+		ptp_loc + GFAR_PTP_SPID_OFFS,
+		GFAR_PTP_SOURCE_PORT_LENGTH);
+
+	/* store rx timestamp */
+	tmp_rx_time.ts = *rx_ts;
+
+	/* insert timestamp in circular buffer */
+	gfar_ptp_insert(&(priv->rx_timestamps), &tmp_rx_time);
+}
+
+/* nominal_frequency - This function calculates the nominal frequency.
+ * nominal frequency is the desired clock frequency.
+ * @sysclock_freq: Timer Oscillator Frequency
+ *
+ * Description:
+ *  Returns the nominal frequency which is calculated on the following
+ *  basis.
+ *  nominal frequency should be less than the Timer Oscillator frequency.
+ *  nominal frequency should be a factor of 1000.
+ *
+ *  Eg If Timer Oscillator frequency is 400.
+ *     then nominal frequency can be 200.
+ *
+ *     If Timer Oscillator frequency is 600.
+ *     then nominal frequency can be 500.
+ *
+ *     If Timer Oscillator frequency is 333.
+ *     then nominal frequency can be 200.
+ */
+static u32 nominal_frequency(u32 sysclock_freq)
+{
+	u32 remainder = 0;
+
+	sysclock_freq /= 1000000;
+	remainder = sysclock_freq % 100;
+	if (remainder) {
+		sysclock_freq = sysclock_freq - remainder;
+		sysclock_freq += 100;
+	}
+
+	while ((1000 % (sysclock_freq -= 100)))
+		continue;
+
+	return sysclock_freq * 1000000;
+}
+
+static int gfar_ptp_cal_attr(u32 sysclk_freq)
+{
+	ptp_attr.sysclock_freq = sysclk_freq;
+
+	ptp_attr.nominal_freq =
+		nominal_frequency(ptp_attr.sysclock_freq);
+	ptp_attr.tmr_fiper1 = ONE_GIGA;
+
+	/* TCLK_PERIOD = 10^9/Nominal_Frequency in HZ */
+	ptp_attr.tclk_period =
+		1000000000 / ptp_attr.nominal_freq;
+
+	return 0;
+}
+
+/*set Fiper Trigger Alarm */
+static void gfar_set_fiper_alarm(struct net_device *dev,
+		struct gfar_ptp_time *alarm)
+{
+	struct gfar_private *priv = netdev_priv(dev);
+	u64 act_time;
+
+	act_time = alarm->sec * NANOSEC_PER_SEC + alarm->nsec;
+	gfar_write(&(priv->ptimer->tmr_alarm1_l), lower_32_bits(act_time));
+	gfar_write(&(priv->ptimer->tmr_alarm1_h), upper_32_bits(act_time));
+
+	gfar_write(&(priv->ptimer->tmr_fiper1), ptp_attr.tmr_fiper1
+			- ptp_attr.tclk_period);
+}
+
+/* Set the 1588 timer counter registers */
+static void gfar_set_1588cnt(struct net_device *dev,
+				struct gfar_ptp_time *gfar_time)
+{
+	struct gfar_private *priv = netdev_priv(dev);
+	u64 act_time;
+
+	act_time = gfar_time->sec * NANOSEC_PER_SEC + gfar_time->nsec;
+
+	/* We must write the tmr_cnt_l register first */
+	gfar_write(&priv->ptimer->tmr_cnt_l, lower_32_bits(act_time));
+	gfar_write(&priv->ptimer->tmr_cnt_h, upper_32_bits(act_time));
+
+	/* restart fiper two second later */
+	gfar_time->sec += 2;
+	gfar_time->nsec = 0;
+	gfar_set_fiper_alarm(dev, gfar_time);
+}
+
+void gfar_cnt_to_ptp_time(u32 high, u32 low, struct gfar_ptp_time *ptp_time)
+{
+	u64 sec;
+	u32 nsec;
+
+	sec = make64(high, low);
+	nsec = do_div(sec, NANOSEC_PER_SEC);
+	ptp_time->nsec = nsec;
+	ptp_time->sec = sec;
+}
+
+static void gfar_get_curr_cnt(struct gfar_regs_1588 __iomem *ptimer,
+					struct gfar_ptp_time *curr_time)
+{
+	u32 high1, high2, low1, low2;
+
+	/* do not accept a time at the rollover from low to high */
+	do {
+		high1 = gfar_read(&ptimer->tmr_cnt_h);
+		low1  = gfar_read(&ptimer->tmr_cnt_l);
+		high2 = gfar_read(&ptimer->tmr_cnt_h);
+		low2 = gfar_read(&ptimer->tmr_cnt_l);
+	} while (high1 != high2);
+
+	gfar_cnt_to_ptp_time(high2, low2, curr_time);
+}
+
+/* Get both the time-stamps and use the larger one */
+u64 gfar_get_tx_timestamp(struct gfar __iomem *regs)
+{
+	u32 high1, high2, low1, low2;
+	u64 ts1, ts2;
+
+	/* Read the nsec register first */
+	low1 = gfar_read(&regs->tmr_txts1_l);
+	high1 = gfar_read(&regs->tmr_txts1_h);
+	low2 = gfar_read(&regs->tmr_txts2_l);
+	high2 = gfar_read(&regs->tmr_txts2_h);
+
+	ts1 = make64(high1, low1);
+	ts2 = make64(high2, low2);
+
+	return (ts1 > ts2) ? ts1 : ts2;
+}
+
+int gfar_ioctl_1588(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct gfar_private *priv = netdev_priv(dev);
+	struct gfar_ptp_time act_time;
+	struct gfar_ptp_data ptp_ts_data;
+	struct gfar_ptp_data *ptp_dat_user;
+	struct gfar __iomem *regs = priv->gfargrp[0].regs;
+	u32 adj;
+
+	if (!(priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER))
+		return -ERANGE;
+
+	switch (cmd) {
+	case PTP_ENBL_TXTS_IOCTL:
+		priv->hwts_tx_en_ioctl = 1;
+		/* add RTPE bit - enable timestamp insertion on tx packets */
+		gfar_write(&(priv->ptimer->tmr_ctrl),
+			gfar_read(&(priv->ptimer->tmr_ctrl)) | TMR_RTPE);
+		break;
+	case PTP_DSBL_TXTS_IOCTL:
+		priv->hwts_tx_en_ioctl = 0;
+		/* remove RTPE bit - disable timestamp insertion
+		 * on tx packets
+		 */
+		gfar_write(&(priv->ptimer->tmr_ctrl),
+			gfar_read(&(priv->ptimer->tmr_ctrl)) & ~TMR_RTPE);
+		break;
+	case PTP_ENBL_RXTS_IOCTL:
+		priv->hwts_rx_en_ioctl = 1;
+		gfar_write(&regs->rctrl,
+				gfar_read(&regs->rctrl) | RCTRL_TS_ENABLE);
+		break;
+	case PTP_DSBL_RXTS_IOCTL:
+		priv->hwts_rx_en_ioctl = 0;
+		gfar_write(&regs->rctrl,
+				gfar_read(&regs->rctrl) & ~RCTRL_TS_ENABLE);
+		break;
+	case PTP_GET_TX_TIMESTAMP:
+		ptp_dat_user = (struct gfar_ptp_data *)ifr->ifr_data;
+		if (copy_from_user(&ptp_ts_data.ident,
+			&ptp_dat_user->ident, sizeof(ptp_ts_data.ident)))
+			return -EINVAL;
+
+		if (gfar_ptp_find_and_remove(&(priv->tx_timestamps),
+				&ptp_ts_data.ident, &ptp_ts_data.ts))
+			return -EAGAIN;
+
+		if (copy_to_user(&ptp_dat_user->ts, &ptp_ts_data.ts,
+						sizeof(ptp_ts_data.ts)))
+			return -EFAULT;
+		break;
+	case PTP_GET_RX_TIMESTAMP:
+		ptp_dat_user = (struct gfar_ptp_data *)ifr->ifr_data;
+		if (copy_from_user(&ptp_ts_data.ident,
+			&ptp_dat_user->ident, sizeof(ptp_ts_data.ident)))
+			return -EINVAL;
+
+		if (gfar_ptp_find_and_remove(&(priv->rx_timestamps),
+				&ptp_ts_data.ident, &ptp_ts_data.ts))
+			return -EAGAIN;
+
+		if (copy_to_user(&ptp_dat_user->ts,
+				&ptp_ts_data.ts, sizeof(ptp_ts_data.ts)))
+			return -EFAULT;
+		break;
+	case PTP_SET_TIME:
+		if (copy_from_user(&act_time, ifr->ifr_data,
+							sizeof(act_time)))
+			return -EINVAL;
+
+		gfar_set_1588cnt(dev, &act_time);
+		break;
+	case PTP_GET_TIME:
+		gfar_get_curr_cnt(priv->ptimer, &act_time);
+		if (copy_to_user(ifr->ifr_data, &act_time,
+					sizeof(act_time)))
+			return -EFAULT;
+		break;
+	case PTP_SET_FIPER_ALARM:
+		if (copy_from_user(&act_time, ifr->ifr_data,
+							sizeof(act_time)))
+			return -EINVAL;
+
+		gfar_set_fiper_alarm(dev, &act_time);
+		break;
+	case PTP_SET_ADJ:
+		if (copy_from_user(&adj, ifr->ifr_data, sizeof(adj)))
+			return -EINVAL;
+
+		/* assign new value directly */
+		gfar_write(&priv->ptimer->tmr_add, adj);
+		break;
+	case PTP_GET_ADJ:
+		/* return initial timer add value
+		 * to calculate drift correction
+		 */
+		if (copy_to_user(ifr->ifr_data, &ptp_attr.freq_comp,
+					sizeof(ptp_attr.freq_comp)))
+			return -EFAULT;
+		break;
+	case PTP_CLEANUP_TS:
+		/* reset tx-timestamping buffer */
+		priv->tx_timestamps.circ_buf.head = 0;
+		priv->tx_timestamps.circ_buf.tail = 0;
+		priv->tx_timestamps.size = DEFAULT_PTP_TX_BUF_SZ;
+		/* reset rx-timestamping buffer */
+		priv->rx_timestamps.circ_buf.head = 0;
+		priv->rx_timestamps.circ_buf.tail = 0;
+		priv->rx_timestamps.size = DEFAULT_PTP_RX_BUF_SZ;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int get_of_u32(struct device_node *node, char *str, u32 *val)
+{
+	int plen;
+	const u32 *prop = of_get_property(node, str, &plen);
+
+	if (!prop || plen != sizeof(*prop))
+		return -1;
+
+	*val = *prop;
+
+	return 0;
+}
+
+/* Resource required for accessing 1588 Timer Registers. There are few 1588
+ * modules registers which are present in eTSEC1 memory space only. The second
+ * reg entry there in denotes the 1588 regs.
+ */
+int gfar_ptp_init(struct device_node *np, struct gfar_private *priv)
+{
+	struct device_node *node;
+	const phandle *timer_handle;
+	u32 tmr_prsc, cksel, sysclk;
+	u64 freq_comp;
+
+	timer_handle = of_get_property(np, "ptimer-handle", NULL);
+	if (!timer_handle)
+		return -ENODEV;
+
+	node = of_find_node_by_phandle(*timer_handle);
+	if (!node)
+		return -ENODEV;
+
+	if (get_of_u32(node, "fsl,tmr-prsc", &tmr_prsc) ||
+		get_of_u32(node, "timer-frequency", &sysclk) ||
+		get_of_u32(node, "fsl,clock-source-select", &cksel))
+		return -ENODEV;
+
+	priv->ptimer = of_iomap(node, 0);
+	if (!priv->ptimer)
+		return -ENOMEM;
+
+	gfar_ptp_cal_attr(sysclk);
+
+	if (of_get_property(node, "fsl,ts-to-buffer", NULL))
+		priv->device_flags |= FSL_GIANFAR_DEV_HAS_TS_TO_BUFFER;
+
+	gfar_write(&(priv->ptimer->tmr_prsc), tmr_prsc);
+	gfar_write(&(priv->ptimer->tmr_fiper1), ptp_attr.tmr_fiper1
+		 - ptp_attr.tclk_period);
+
+	gfar_write(&(priv->ptimer->tmr_alarm1_l), TMR_ALARM1_L);
+	gfar_write(&(priv->ptimer->tmr_alarm1_h), TMR_ALARM1_H);
+
+	/* Need to mask the TCLK bits as they are initialized with 1 */
+	gfar_write(&(priv->ptimer->tmr_ctrl),
+		(gfar_read(&(priv->ptimer->tmr_ctrl))
+			 & ~TMR_CTRL_TCLK_MASK) | (ptp_attr.tclk_period << 16));
+
+	/* initialize TMR_ADD with the initial frequency compensation value:
+	 * freq_compensation = ceil(2^32 / frequency ratio)
+	 * frequency ratio = sysclock frequency / nominal frequency
+	 */
+	freq_comp = ((u64)2 << 31) * ptp_attr.nominal_freq;
+	if (do_div(freq_comp, ptp_attr.sysclock_freq))
+		freq_comp++;
+	ptp_attr.freq_comp = lower_32_bits(freq_comp);
+	gfar_write(&(priv->ptimer->tmr_add), ptp_attr.freq_comp);
+
+	gfar_write(&(priv->ptimer->tmr_ctrl),
+		(gfar_read(&(priv->ptimer->tmr_ctrl)) & ~TMR_CTRL_CKSEL_MASK) |
+		TMR_CTRL_ENABLE | cksel | TMR_CTRL_FIPER_START);
+
+	/* initialize circular buffer for tx timestamps */
+	if (gfar_ptp_init_circ(&(priv->tx_timestamps),
+					DEFAULT_PTP_TX_BUF_SZ))
+		goto txbuf;
+
+	/* initialize circular buffer for rx timestamps */
+	if (gfar_ptp_init_circ(&(priv->rx_timestamps),
+					DEFAULT_PTP_RX_BUF_SZ))
+		goto rxbuf;
+
+	gfar_1588_start(priv);
+
+	return 0;
+
+rxbuf:
+	vfree(priv->tx_timestamps.circ_buf.buf);
+txbuf:
+	iounmap(priv->ptimer);
+
+	return -EBUSY;
+}
+
+void gfar_ptp_cleanup(struct gfar_private *priv)
+{
+	if (priv->ptimer)
+		iounmap(priv->ptimer);
+
+	if (priv->tx_timestamps.circ_buf.buf)
+		vfree(priv->tx_timestamps.circ_buf.buf);
+
+	if (priv->rx_timestamps.circ_buf.buf)
+		vfree(priv->rx_timestamps.circ_buf.buf);
+}
+
+/* 1588 Module intialization */
+void gfar_1588_start(struct gfar_private *priv)
+{
+	if (!priv->ptimer)
+		return;
+
+	priv->hwts_tx_en = 0;
+	priv->hwts_rx_en = 0;
+	priv->hwts_tx_en_ioctl = 0;
+	priv->hwts_rx_en_ioctl = 0;
+
+	priv->device_flags |= FSL_GIANFAR_DEV_HAS_TIMER;
+}
+
+/* When PTP is disabled this routing is called */
+void gfar_1588_stop(struct gfar_private *priv)
+{
+	if (!(priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER))
+		return;
+
+	priv->hwts_tx_en = 0;
+	priv->hwts_rx_en = 0;
+	priv->hwts_tx_en_ioctl = 0;
+	priv->hwts_rx_en_ioctl = 0;
+
+	priv->device_flags &= ~FSL_GIANFAR_DEV_HAS_TIMER;
+}