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>

1 files changed, 736 insertions, 0 deletions

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;
+}