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.

The IEEE 1588 driver can also be disabled/enabled via sysfs.
To enable 1588:
    - echo 1 > /sys/*/ethN/ptp_1588
To disable 1588
    - echo 0 > /sys/*/ethN/ptp_1588

Signed-off-by: Sandeep Gopalpet <sandeep.kumar@freescale.com>
Signed-off-by: Bhaskar Upadhaya <Bhaskar.Upadhaya@freescale.com>
Signed-off-by: Tang Yuantian <Yuantian.Tang@freescale.com>
Signed-off-by: Xie Xiaobo <X.Xie@freescale.com>
Change-Id: I3a1190eb6bce06417e0365aba06d73d3a9af5634
Reviewed-on: http://git.am.freescale.net:8181/866
Reviewed-by: Tang Yuantian-B29983 <yuantian.tang@freescale.com>
Reviewed-by: Manoil Claudiu-B08782 <claudiu.manoil@freescale.com>
Reviewed-by: Fleming Andrew-AFLEMING <AFLEMING@freescale.com>
Tested-by: Fleming Andrew-AFLEMING <AFLEMING@freescale.com>

5 files changed, 1053 insertions, 34 deletions

diff --git a/drivers/net/ethernet/freescale/Makefile b/drivers/net/ethernet/freescale/Makefile
index adc5b7f..beb91a5 100644
--- a/drivers/net/ethernet/freescale/Makefile
+++ b/drivers/net/ethernet/freescale/Makefile
@@ -17,6 +17,7 @@ obj-$(if $(CONFIG_DPA_ETH),y) += dpa/
 obj-$(CONFIG_PTP_1588_CLOCK_GIANFAR) += gianfar_ptp.o
 gianfar_driver-objs := gianfar.o \
 		gianfar_ethtool.o \
-		gianfar_sysfs.o
+		gianfar_sysfs.o \
+		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 d154c7a..9a4d700 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -9,7 +9,7 @@
  * Maintainer: Kumar Gala
  * Modifier: Sandeep Gopalpet <sandeep.kumar@freescale.com>
  *
- * Copyright 2002-2009, 2011 Freescale Semiconductor, Inc.
+ * Copyright 2002-2009, 2011-2012 Freescale Semiconductor, Inc.
  * Copyright 2007 MontaVista Software, Inc.
  *
  * This program is free software; you can redistribute  it and/or modify it
@@ -388,10 +388,11 @@ static void gfar_init_mac(struct net_device *ndev)
 	}
 
 	/* Insert receive time stamps into padding alignment bytes */
-	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER) {
+	if (priv->ptimer) {
 		rctrl &= ~RCTRL_PAL_MASK;
-		rctrl |= RCTRL_PADDING(8);
+		rctrl |= RCTRL_PADDING(8) | RCTRL_PRSDEP_INIT;
 		priv->padding = 8;
+		priv->uses_rxfcb = 1;
 	}
 
 	/* Enable HW time stamping if requested from user space */
@@ -775,8 +776,7 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 				     FSL_GIANFAR_DEV_HAS_CSUM |
 				     FSL_GIANFAR_DEV_HAS_VLAN |
 				     FSL_GIANFAR_DEV_HAS_MAGIC_PACKET |
-				     FSL_GIANFAR_DEV_HAS_EXTENDED_HASH |
-				     FSL_GIANFAR_DEV_HAS_TIMER;
+				     FSL_GIANFAR_DEV_HAS_EXTENDED_HASH;
 
 	ctype = of_get_property(np, "phy-connection-type", NULL);
 
@@ -794,6 +794,10 @@ 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);
 
+	/* Handle IEEE1588 node */
+	if (!gfar_ptp_init(np, priv))
+		dev_info(&ofdev->dev, "ptp 1588 is initialized.\n");
+
 	return 0;
 
 rx_alloc_failed:
@@ -811,6 +815,7 @@ static int gfar_hwtstamp_ioctl(struct net_device *netdev,
 {
 	struct hwtstamp_config config;
 	struct gfar_private *priv = netdev_priv(netdev);
+	struct gfar __iomem *regs = priv->gfargrp[0].regs;
 
 	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
 		return -EFAULT;
@@ -822,11 +827,25 @@ static int gfar_hwtstamp_ioctl(struct net_device *netdev,
 	switch (config.tx_type) {
 	case HWTSTAMP_TX_OFF:
 		priv->hwts_tx_en = 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 HWTSTAMP_TX_ON:
 		if (!(priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER))
 			return -ERANGE;
 		priv->hwts_tx_en = 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;
 	default:
 		return -ERANGE;
@@ -837,6 +856,8 @@ static int gfar_hwtstamp_ioctl(struct net_device *netdev,
 		if (priv->hwts_rx_en) {
 			stop_gfar(netdev);
 			priv->hwts_rx_en = 0;
+			gfar_write(&regs->rctrl,
+				gfar_read(&regs->rctrl) & ~RCTRL_TS_ENABLE);
 			startup_gfar(netdev);
 		}
 		break;
@@ -846,6 +867,8 @@ static int gfar_hwtstamp_ioctl(struct net_device *netdev,
 		if (!priv->hwts_rx_en) {
 			stop_gfar(netdev);
 			priv->hwts_rx_en = 1;
+			gfar_write(&regs->rctrl,
+				gfar_read(&regs->rctrl) | RCTRL_TS_ENABLE);
 			startup_gfar(netdev);
 		}
 		config.rx_filter = HWTSTAMP_FILTER_ALL;
@@ -870,7 +893,11 @@ static int gfar_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	if (!priv->phydev)
 		return -ENODEV;
 
-	return phy_mii_ioctl(priv->phydev, rq, cmd);
+	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);
 }
 
 static unsigned int reverse_bitmap(unsigned int bit_map, unsigned int max_qs)
@@ -1261,6 +1288,7 @@ static int gfar_probe(struct platform_device *ofdev)
 	return 0;
 
 register_fail:
+	gfar_ptp_cleanup(priv);
 	unmap_group_regs(priv);
 	free_tx_pointers(priv);
 	free_rx_pointers(priv);
@@ -2139,7 +2167,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* check if time stamp should be generated */
 	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
-		     priv->hwts_tx_en)) {
+		     priv->hwts_tx_en) || unlikely(priv->hwts_tx_en_ioctl)) {
 		do_tstamp = 1;
 		fcb_length = GMAC_FCB_LEN + GMAC_TXPAL_LEN;
 	}
@@ -2259,11 +2287,29 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Setup tx hardware time stamping if requested */
 	if (unlikely(do_tstamp)) {
+		u32 vlan_ctrl;
 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 		if (fcb == NULL)
 			fcb = gfar_add_fcb(skb);
+		/*
+		 * the timestamp overwrites the ethertype and the following
+		 * 2 bytes -> storing 4 bytes at the end of the control buffer
+		 * structure - will be recovered in the function
+		 * gfar_clean_tx_ring
+		 */
+		memcpy(skb->cb, (skb->data + GMAC_FCB_LEN +
+					ETH_ALEN + ETH_ALEN), 4);
 		fcb->ptp = 1;
 		lstatus |= BD_LFLAG(TXBD_TOE);
+		/*
+		 * SYMM: 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);
 	}
 
 	txbdp_start->bufPtr = dma_map_single(priv->dev, skb->data,
@@ -2613,13 +2659,44 @@ 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)) {
+			struct gfar __iomem *regs = priv->gfargrp[0].regs;
 			struct skb_shared_hwtstamps shhwtstamps;
-			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);
+			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);
+			/*
+			 * the timestamp overwrote the ethertype and the
+			 * following 2 bytes, 4 byters were stored in the
+			 * end of the control buffer in function
+			 * gfar_start_xmit to be recovered here
+			 */
+			memcpy((skb->data + ETH_ALEN + ETH_ALEN), skb->cb, 4);
+			/* 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);
 			bdp->lstatus &= BD_LFLAG(TXBD_WRAP);
 			bdp = next;
 		}
@@ -2809,16 +2886,30 @@ static void gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	}
 
 	/* Get receive timestamp from the skb */
-	if (priv->hwts_rx_en) {
-		struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
-		u64 *ns = (u64 *) skb->data;
-
-		memset(shhwtstamps, 0, sizeof(*shhwtstamps));
-		shhwtstamps->hwtstamp = ns_to_ktime(*ns);
-	}
-
-	if (priv->padding)
-		skb_pull(skb, priv->padding);
+	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);
 
 	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 475f88e..551ec37 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -9,7 +9,7 @@
  * Maintainer: Kumar Gala
  * Modifier: Sandeep Gopalpet <sandeep.kumar@freescale.com>
  *
- * Copyright 2002-2009, 2011 Freescale Semiconductor, Inc.
+ * Copyright 2002-2009, 2011, 2013 Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute  it and/or modify it
  * under  the terms of  the GNU General  Public License as published by the
@@ -46,6 +46,7 @@
 #include <linux/crc32.h>
 #include <linux/workqueue.h>
 #include <linux/ethtool.h>
+#include <linux/circ_buf.h>
 
 struct ethtool_flow_spec_container {
 	struct ethtool_rx_flow_spec fs;
@@ -146,6 +147,57 @@ extern const char gfar_driver_version[];
 		| SUPPORTED_Autoneg \
 		| SUPPORTED_MII)
 
+/* 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
+
+#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
+
 /* TBI register addresses */
 #define MII_TBICON		0x11
 
@@ -659,6 +711,78 @@ struct gfar_extra_stats {
 /* Number of stats exported via ethtool */
 #define GFAR_STATS_LEN (GFAR_RMON_LEN + GFAR_EXTRA_STATS_LEN)
 
+/* 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;
+};
+
 struct gfar {
 	u32	tsec_id;	/* 0x.000 - Controller ID register */
 	u32	tsec_id2;	/* 0x.004 - Controller ID2 register */
@@ -731,7 +855,15 @@ 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 */
-	u8	res10[192];
+	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];
 	u32	rctrl;		/* 0x.300 - Receive Control Register */
 	u32	rstat;		/* 0x.304 - Receive Status Register */
 	u8	res12[8];
@@ -782,7 +914,10 @@ 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 */
-	u8	res17[192];
+	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];
 	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 */
@@ -851,7 +986,8 @@ struct gfar {
 	u8	res23c[248];
 	u32	attr;		/* 0x.bf8 - Attributes Register */
 	u32	attreli;	/* 0x.bfc - Attributes Extract Length and Extract Index Register */
-	u8	res24[688];
+	u8	res24[512];
+	struct gfar_regs_1588 regs_1588;
 	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 */
@@ -890,6 +1026,7 @@ struct gfar {
 #define FSL_GIANFAR_DEV_HAS_BD_STASHING		0x00000200
 #define FSL_GIANFAR_DEV_HAS_BUF_STASHING	0x00000400
 #define FSL_GIANFAR_DEV_HAS_TIMER		0x00000800
+#define FSL_GIANFAR_DEV_HAS_TS_TO_BUFFER	0x00001000
 
 #if (MAXGROUPS == 2)
 #define DEFAULT_MAPPING 	0xAA
@@ -1076,9 +1213,11 @@ struct gfar_private {
 	u16 uses_rxfcb;
 	u16 padding;
 
-	/* HW time stamping enabled flag */
-	int hwts_rx_en;
-	int hwts_tx_en;
+	u32 device_flags;
+
+	/* HW TX timestamping enabled flag */
+	u16 hwts_tx_en;
+	u16 hwts_tx_en_ioctl;
 
 	struct gfar_priv_tx_q *tx_queue[MAX_TX_QS];
 	struct gfar_priv_rx_q *rx_queue[MAX_RX_QS];
@@ -1087,8 +1226,6 @@ struct gfar_private {
 	struct gfar_priv_recycle recycle;
 	struct gfar_priv_recycle *recycle_target;
 
-	u32 device_flags;
-
 	unsigned int mode;
 	unsigned int num_tx_queues;
 	unsigned int num_grps;
@@ -1096,6 +1233,15 @@ struct gfar_private {
 	/* Network Statistics */
 	struct gfar_extra_stats extra_stats;
 
+	/* 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;
+
 	/* PHY stuff */
 	phy_interface_t interface;
 	struct device_node *phy_node;
@@ -1201,6 +1347,17 @@ extern irqreturn_t gfar_receive(int irq, void *dev_id);
 extern int startup_gfar(struct net_device *dev);
 extern void stop_gfar(struct net_device *dev);
 extern void gfar_halt(struct net_device *dev);
+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);
 extern void gfar_phy_test(struct mii_bus *bus, struct phy_device *phydev,
 		int enable, u32 regnum, u32 read);
 extern 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..bd6f2b1
--- /dev/null
+++ b/drivers/net/ethernet/freescale/gianfar_1588.c
@@ -0,0 +1,728 @@
+/*
+ * 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/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;
+
+	return (CIRC_SPACE(circ_buf->head, circ_buf->tail,
+				buf->size) == 0);
+}
+
+static bool gfar_ptp_is_empty(struct gfar_ptp_circular *buf)
+{
+	struct circ_buf *circ_buf = &buf->circ_buf;
+
+	return (CIRC_CNT(circ_buf->head, circ_buf->tail,
+				buf->size) == 0);
+}
+
+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, idx;
+	struct gfar_ptp_data *tmp;
+	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;
+	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) {
+		circ_buf->tail = head;
+		spin_unlock_irqrestore(&buf->ptp_lock, flags);
+		return 1;
+	}
+
+	*ts = tmp->ts;
+
+	/* set tail pointer to postion after found */
+	circ_buf->tail = (idx + 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;
+}
diff --git a/drivers/net/ethernet/freescale/gianfar_sysfs.c b/drivers/net/ethernet/freescale/gianfar_sysfs.c
index 0c420e8..15973ca 100644
--- a/drivers/net/ethernet/freescale/gianfar_sysfs.c
+++ b/drivers/net/ethernet/freescale/gianfar_sysfs.c
@@ -10,7 +10,7 @@
  * Maintainer: Kumar Gala (galak@kernel.crashing.org)
  * Modifier: Sandeep Gopalpet <sandeep.kumar@freescale.com>
  *
- * Copyright 2002-2009 Freescale Semiconductor, Inc.
+ * Copyright 2002-2009, 2012 Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute  it and/or modify it
  * under  the terms of  the GNU General  Public License as published by the
@@ -391,6 +391,47 @@ static DEVICE_ATTR(recycle_target, 0644, gfar_show_recycle_target,
 		   gfar_set_recycle_target);
 
 
+static ssize_t gfar_show_ptp_1588(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct gfar_private *priv = netdev_priv(to_net_dev(dev));
+
+	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)
+		return sprintf(buf, "1\n");
+	else
+		return sprintf(buf, "0\n");
+}
+
+static ssize_t gfar_set_ptp_1588(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct net_device *ndev = to_net_dev(dev);
+	struct gfar_private *priv = netdev_priv(ndev);
+	unsigned int num;
+	unsigned long flags;
+
+	if (kstrtouint(buf, 0, &num) < 0)
+		return -EINVAL;
+
+	local_irq_save(flags);
+	lock_tx_qs(priv);
+	lock_rx_qs(priv);
+
+	if (num)
+		gfar_1588_start(priv);
+	else
+		gfar_1588_stop(priv);
+
+	unlock_rx_qs(priv);
+	unlock_tx_qs(priv);
+	local_irq_restore(flags);
+
+	return count;
+}
+
+static DEVICE_ATTR(ptp_1588, 0644, gfar_show_ptp_1588, gfar_set_ptp_1588);
+
 void gfar_init_sysfs(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
@@ -410,6 +451,7 @@ void gfar_init_sysfs(struct net_device *dev)
 	rc |= device_create_file(&dev->dev, &dev_attr_fifo_starve_off);
 	rc |= device_create_file(&dev->dev, &dev_attr_recycle);
 	rc |= device_create_file(&dev->dev, &dev_attr_recycle_target);
+	rc |= device_create_file(&dev->dev, &dev_attr_ptp_1588);
 	if (rc)
 		dev_err(&dev->dev, "Error creating gianfar sysfs files.\n");
 }