ENGR00232528 - IEEE1588:imx6:Upgrade driver to support IXXAT stack V1.05.03

Upgrade drivers to support IXXAT IEEE1588 stack V1.05.03.
Ptp message protocol support IPv4, IPv6, and IEEE802.3 L2.

Signed-off-by: Fugang Duan  <B38611@freescale.com>

3 files changed, 355 insertions, 490 deletions

diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 86e4ea3404b3..db48dfda84e2 100755
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -1197,7 +1197,9 @@ static struct ethtool_ops fec_enet_ethtool_ops = {
 static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct fec_ptp_private *priv = fep->ptp_priv;
 	struct phy_device *phydev = fep->phy_dev;
+	int retVal = 0;
 
 	if (!netif_running(ndev))
 		return -EINVAL;
@@ -1205,7 +1207,16 @@ static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 	if (!phydev)
 		return -ENODEV;
 
-	return phy_mii_ioctl(phydev, rq, cmd);
+	if ((cmd >= PTP_ENBL_TXTS_IOCTL) &&
+			(cmd <= PTP_FLUSH_TIMESTAMP)) {
+		if (fep->ptimer_present)
+			retVal = fec_ptp_ioctl(priv, rq, cmd);
+		else
+			retVal = -ENODEV;
+	} else
+		retVal = phy_mii_ioctl(phydev, rq, cmd);
+
+	return retVal;
 }
 
 static void fec_enet_free_buffers(struct net_device *ndev)
diff --git a/drivers/net/fec_1588.c b/drivers/net/fec_1588.c
index 43b60941cc99..74fa168f3f7d 100644
--- a/drivers/net/fec_1588.c
+++ b/drivers/net/fec_1588.c
@@ -26,18 +26,11 @@
 #include <linux/device.h>
 #include <linux/fs.h>
 #include <linux/vmalloc.h>
-#include <linux/spinlock.h>
 #include <linux/ip.h>
 #include <linux/udp.h>
 #include "fec.h"
 #include "fec_1588.h"
 
-static DECLARE_WAIT_QUEUE_HEAD(ptp_rx_ts_wait);
-static DECLARE_WAIT_QUEUE_HEAD(ptp_tx_ts_wait);
-
-#define PTP_GET_RX_TIMEOUT      (HZ)
-#define PTP_GET_TX_TIMEOUT      (HZ)
-
 #if defined(CONFIG_ARCH_MX28)
 static struct fec_ptp_private *ptp_private[2];
 #elif defined(CONFIG_ARCH_MX6)
@@ -45,15 +38,16 @@ static struct fec_ptp_private *ptp_private[1];
 #endif
 
 /* Alloc the ring resource */
-static int fec_ptp_init_circ(struct circ_buf *ptp_buf, int size)
+static int fec_ptp_init_circ(struct fec_ptp_circular *buf, int size)
 {
-	ptp_buf->buf = vmalloc(size * sizeof(struct fec_ptp_data_t));
+	buf->data_buf = (struct fec_ptp_ts_data *)
+		vmalloc(size * sizeof(struct fec_ptp_ts_data));
 
-	if (!ptp_buf->buf)
+	if (!buf->data_buf)
 		return 1;
-	ptp_buf->head = 0;
-	ptp_buf->tail = 0;
-
+	buf->front = 0;
+	buf->end = 0;
+	buf->size = size;
 	return 0;
 }
 
@@ -62,15 +56,16 @@ static inline int fec_ptp_calc_index(int size, int curr_index, int offset)
 	return (curr_index + offset) % size;
 }
 
-static int fec_ptp_is_empty(struct circ_buf *buf)
+static int fec_ptp_is_empty(struct fec_ptp_circular *buf)
 {
-	return (buf->head == buf->tail);
+	return (buf->front == buf->end);
 }
 
-static int fec_ptp_nelems(struct circ_buf *buf, int size)
+static int fec_ptp_nelems(struct fec_ptp_circular *buf)
 {
-	const int front = buf->head;
-	const int end = buf->tail;
+	const int front = buf->front;
+	const int end = buf->end;
+	const int size = buf->size;
 	int n_items;
 
 	if (end > front)
@@ -83,73 +78,76 @@ static int fec_ptp_nelems(struct circ_buf *buf, int size)
 	return n_items;
 }
 
-static int fec_ptp_is_full(struct circ_buf *buf, int size)
+static int fec_ptp_is_full(struct fec_ptp_circular *buf)
 {
-	if (fec_ptp_nelems(buf, size) ==
-				(size - 1))
+	if (fec_ptp_nelems(buf) == (buf->size - 1))
 		return 1;
 	else
 		return 0;
 }
 
-static int fec_ptp_insert(struct circ_buf *ptp_buf,
-			  struct fec_ptp_data_t *data,
-			  struct fec_ptp_private *priv,
-			  int size)
-{
-	struct fec_ptp_data_t *tmp;
-
-	if (fec_ptp_is_full(ptp_buf, size))
-		return 1;
+static int fec_ptp_insert(struct fec_ptp_circular *ptp_buf,
+			  struct fec_ptp_ts_data *data)
 
-	spin_lock(&priv->ptp_lock);
-	tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + ptp_buf->tail;
+{
+	struct fec_ptp_ts_data *tmp;
 
-	tmp->key = data->key;
-	memcpy(tmp->spid, data->spid, 10);
-	tmp->ts_time.sec = data->ts_time.sec;
-	tmp->ts_time.nsec = data->ts_time.nsec;
+	if (fec_ptp_is_full(ptp_buf))
+		ptp_buf->end = fec_ptp_calc_index(ptp_buf->size,
+						ptp_buf->end, 1);
 
-	ptp_buf->tail = fec_ptp_calc_index(size, ptp_buf->tail, 1);
-	spin_unlock(&priv->ptp_lock);
+	tmp = (ptp_buf->data_buf + ptp_buf->end);
+	memcpy(tmp, data, sizeof(struct fec_ptp_ts_data));
+	ptp_buf->end = fec_ptp_calc_index(ptp_buf->size, ptp_buf->end, 1);
 
 	return 0;
 }
 
-static int fec_ptp_find_and_remove(struct circ_buf *ptp_buf,
-				   struct fec_ptp_data_t *data,
-				   struct fec_ptp_private *priv,
-				   int size)
+static int fec_ptp_find_and_remove(struct fec_ptp_circular *ptp_buf,
+			struct fec_ptp_ident *ident, struct ptp_time *ts)
 {
 	int i;
-	int end = ptp_buf->tail;
-	unsigned long flags;
-	struct fec_ptp_data_t *tmp;
+	int size = ptp_buf->size, end = ptp_buf->end;
+	struct fec_ptp_ident *tmp_ident;
 
 	if (fec_ptp_is_empty(ptp_buf))
 		return 1;
 
-	i = ptp_buf->head;
+	i = ptp_buf->front;
 	while (i != end) {
-		tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + i;
-		if (tmp->key == data->key &&
-				!memcmp(tmp->spid, data->spid, 10))
-			break;
+		tmp_ident = &(ptp_buf->data_buf + i)->ident;
+		if (tmp_ident->version == ident->version) {
+			if (tmp_ident->message_type == ident->message_type) {
+				if ((tmp_ident->netw_prot == ident->netw_prot)
+				|| (ident->netw_prot ==
+					FEC_PTP_PROT_DONTCARE)) {
+					if (tmp_ident->seq_id ==
+							ident->seq_id) {
+						int ret =
+						memcmp(tmp_ident->spid,
+							ident->spid,
+							PTP_SOURCE_PORT_LENGTH);
+						if (0 == ret)
+							break;
+					}
+				}
+			}
+		}
+		/* get next */
 		i = fec_ptp_calc_index(size, i, 1);
 	}
 
-	spin_lock_irqsave(&priv->ptp_lock, flags);
+	/* not found ? */
 	if (i == end) {
-		ptp_buf->head = end;
-		spin_unlock_irqrestore(&priv->ptp_lock, flags);
+		/* buffer full ? */
+		if (fec_ptp_is_full(ptp_buf))
+			/* drop one in front */
+			ptp_buf->front =
+			fec_ptp_calc_index(size, ptp_buf->front, 1);
+
 		return 1;
 	}
-
-	data->ts_time.sec = tmp->ts_time.sec;
-	data->ts_time.nsec = tmp->ts_time.nsec;
-
-	ptp_buf->head = fec_ptp_calc_index(size, i, 1);
-	spin_unlock_irqrestore(&priv->ptp_lock, flags);
+	*ts = (ptp_buf->data_buf + i)->ts;
 
 	return 0;
 }
@@ -211,7 +209,6 @@ void fec_ptp_stop(struct fec_ptp_private *priv)
 	writel(FEC_T_CTRL_RESTART, fpp->hwp + FEC_ATIME_CTRL);
 	priv->ptp_active = 0;
 	priv->ptp_slave = 0;
-
 }
 
 static void fec_get_curr_cnt(struct fec_ptp_private *priv,
@@ -263,22 +260,100 @@ static void fec_set_1588cnt(struct fec_ptp_private *priv,
 	spin_unlock_irqrestore(&priv->cnt_lock, flags);
 }
 
+/**
+ * Parse packets if they are PTP.
+ * The PTP header can be found in an IPv4, IPv6 or in an IEEE802.3
+ * ethernet frame. The function returns the position of the PTP packet
+ * or NULL, if no PTP found
+ */
+u8 *fec_ptp_parse_packet(struct sk_buff *skb, u16 *eth_type)
+{
+	u8 *position = skb->data + ETH_ALEN + ETH_ALEN;
+	u8 *ptp_loc = NULL;
+
+	*eth_type = *((u16 *)position);
+	/* Check if outer vlan tag is here */
+	if (*eth_type == ETH_P_8021Q) {
+		position += FEC_VLAN_TAG_LEN;
+		*eth_type = *((u16 *)position);
+	}
+
+	/* set position after ethertype */
+	position += FEC_ETHTYPE_LEN;
+	if (ETH_P_1588 == *eth_type) {
+		ptp_loc = position;
+		/* IEEE1588 event message which needs timestamping */
+		if ((ptp_loc[0] & 0xF) <= 3) {
+			if (skb->len >=
+			((ptp_loc - skb->data) + PTP_HEADER_SZE))
+				return ptp_loc;
+		}
+	} else if (ETH_P_IP == ntohs(*eth_type)) {
+		u8 *ip_header, *prot, *udp_header;
+		u8 ip_version, ip_hlen;
+		ip_header = position;
+		ip_version = ip_header[0] >> 4; /* correct IP version? */
+		if (0x04 == ip_version) { /* IPv4 */
+			prot = ip_header + 9; /* protocol */
+			if (FEC_PACKET_TYPE_UDP == *prot) {
+				u16 udp_dstPort;
+				/* retrieve the size of the ip-header
+				 * with the first byte of the ip-header:
+				 * version ( 4 bits) + Internet header
+				 * length (4 bits)
+				 */
+				ip_hlen   = (*ip_header & 0xf) * 4;
+				udp_header = ip_header + ip_hlen;
+				udp_dstPort = *((u16 *)(udp_header + 2));
+				/* check the destination port address
+				 * ( 319 (0x013F) = PTP event port )
+				 */
+				if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
+					ptp_loc = udp_header + 8;
+					/* long enough ? */
+					if (skb->len >= ((ptp_loc - skb->data)
+							+ PTP_HEADER_SZE))
+						return ptp_loc;
+				}
+			}
+		}
+	} else if (ETH_P_IPV6 == ntohs(*eth_type)) {
+		u8 *ip_header, *udp_header, *prot;
+		u8 ip_version;
+		ip_header = position;
+		ip_version = ip_header[0] >> 4;
+		if (0x06 == ip_version) {
+			prot = ip_header + 6;
+			if (FEC_PACKET_TYPE_UDP == *prot) {
+				u16 udp_dstPort;
+				udp_header = ip_header + 40;
+				udp_dstPort = *((u16 *)(udp_header + 2));
+				/* check the destination port address
+				 * ( 319 (0x013F) = PTP event port )
+				 */
+				if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
+					ptp_loc = udp_header + 8;
+					/* long enough ? */
+					if (skb->len >= ((ptp_loc - skb->data)
+							+ PTP_HEADER_SZE))
+						return ptp_loc;
+				}
+			}
+		}
+	}
+
+	return NULL; /* no PTP frame */
+}
+
 /* Set the BD to ptp */
 int fec_ptp_do_txstamp(struct sk_buff *skb)
 {
-	struct iphdr *iph;
-	struct udphdr *udph;
-
-	if (skb->len > 44) {
-		/* Check if port is 319 for PTP Event, and check for UDP */
-		iph = ip_hdr(skb);
-		if (iph == NULL || iph->protocol != FEC_PACKET_TYPE_UDP)
-			return 0;
-
-		udph = udp_hdr(skb);
-		if (udph != NULL && ntohs(udph->dest) == 319)
-			return 1;
-	}
+	u8 *ptp_loc;
+	u16 eth_type;
+
+	ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
+	if (ptp_loc != NULL)
+		return 1;
 
 	return 0;
 }
@@ -287,288 +362,113 @@ void fec_ptp_store_txstamp(struct fec_ptp_private *priv,
 			   struct sk_buff *skb,
 			   struct bufdesc *bdp)
 {
-	int msg_type, seq_id, control;
-	struct fec_ptp_data_t tmp_tx_time;
+	struct fec_ptp_ts_data tmp_tx_time;
 	struct fec_ptp_private *fpp;
-	unsigned char *sp_id;
-	unsigned short portnum;
+	u8 *ptp_loc;
+	u16 eth_type;
 
 	if (!priv->ptp_slave)
 		fpp = priv;
 	else
 		fpp = ptp_private[0];
 
-	seq_id = *((u16 *)(skb->data + FEC_PTP_SEQ_ID_OFFS));
-	control = *((u8 *)(skb->data + FEC_PTP_CTRL_OFFS));
-	sp_id = skb->data + FEC_PTP_SPORT_ID_OFFS;
-	portnum = ntohs(*((unsigned short *)(sp_id + 8)));
-
-	tmp_tx_time.key = ntohs(seq_id);
-	memcpy(tmp_tx_time.spid, sp_id, 8);
-	memcpy(tmp_tx_time.spid + 8, (unsigned char *)&portnum, 2);
-	tmp_tx_time.ts_time.sec = fpp->prtc;
-	tmp_tx_time.ts_time.nsec = bdp->ts;
-
-	switch (control) {
-
-	case PTP_MSG_SYNC:
-		fec_ptp_insert(&(priv->tx_time_sync), &tmp_tx_time, priv,
-				DEFAULT_PTP_TX_BUF_SZ);
-		break;
-
-	case PTP_MSG_DEL_REQ:
-		fec_ptp_insert(&(priv->tx_time_del_req), &tmp_tx_time, priv,
-				DEFAULT_PTP_TX_BUF_SZ);
-		break;
-
-	/* clear transportSpecific field*/
-	case PTP_MSG_ALL_OTHER:
-		msg_type = (*((u8 *)(skb->data +
-				FEC_PTP_MSG_TYPE_OFFS))) & 0x0F;
-		switch (msg_type) {
-		case PTP_MSG_P_DEL_REQ:
-			fec_ptp_insert(&(priv->tx_time_pdel_req), &tmp_tx_time,
-					priv, DEFAULT_PTP_TX_BUF_SZ);
+	ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
+	if (ptp_loc != NULL) {
+		/* store identification data */
+		switch (ntohs(eth_type)) {
+		case ETH_P_IP:
+			tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
 			break;
-		case PTP_MSG_P_DEL_RESP:
-			fec_ptp_insert(&(priv->tx_time_pdel_resp), &tmp_tx_time,
-					priv, DEFAULT_PTP_TX_BUF_SZ);
+		case ETH_P_IPV6:
+			tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
 			break;
-		default:
+		case ETH_P_1588:
+			tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
 			break;
+		default:
+			return;
 		}
-		break;
-	default:
-		break;
+		tmp_tx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
+		tmp_tx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
+		tmp_tx_time.ident.seq_id =
+			ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
+		memcpy(tmp_tx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
+						PTP_SOURCE_PORT_LENGTH);
+		/* store tx timestamp */
+		tmp_tx_time.ts.sec = fpp->prtc;
+		tmp_tx_time.ts.nsec = bdp->ts;
+		/* insert timestamp in circular buffer */
+		fec_ptp_insert(&(priv->tx_timestamps), &tmp_tx_time);
 	}
-
-	wake_up_interruptible(&ptp_tx_ts_wait);
 }
 
 void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 			   struct sk_buff *skb,
 			   struct bufdesc *bdp)
 {
-	int msg_type, seq_id, control;
-	struct fec_ptp_data_t tmp_rx_time;
+	struct fec_ptp_ts_data tmp_rx_time;
 	struct fec_ptp_private *fpp;
-	struct iphdr *iph;
-	struct udphdr *udph;
-	unsigned char *sp_id;
-	unsigned short portnum;
+	u8 *ptp_loc;
+	u16 eth_type;
 
 	if (!priv->ptp_slave)
 		fpp = priv;
 	else
 		fpp = ptp_private[0];
 
-	/* Check for UDP, and Check if port is 319 for PTP Event */
-	iph = (struct iphdr *)(skb->data + FEC_PTP_IP_OFFS);
-	if (iph->protocol != FEC_PACKET_TYPE_UDP)
-		return;
-
-	udph = (struct udphdr *)(skb->data + FEC_PTP_UDP_OFFS);
-	if (ntohs(udph->dest) != 319)
-		return;
-
-	seq_id = *((u16 *)(skb->data + FEC_PTP_SEQ_ID_OFFS));
-	control = *((u8 *)(skb->data + FEC_PTP_CTRL_OFFS));
-	sp_id = skb->data + FEC_PTP_SPORT_ID_OFFS;
-	portnum = ntohs(*((unsigned short *)(sp_id + 8)));
-
-	tmp_rx_time.key = ntohs(seq_id);
-	memcpy(tmp_rx_time.spid, sp_id, 8);
-	memcpy(tmp_rx_time.spid + 8, (unsigned char *)&portnum, 2);
-	tmp_rx_time.ts_time.sec = fpp->prtc;
-	tmp_rx_time.ts_time.nsec = bdp->ts;
-
-	switch (control) {
-
-	case PTP_MSG_SYNC:
-		fec_ptp_insert(&(priv->rx_time_sync), &tmp_rx_time, priv,
-				DEFAULT_PTP_RX_BUF_SZ);
-		break;
-
-	case PTP_MSG_DEL_REQ:
-		fec_ptp_insert(&(priv->rx_time_del_req), &tmp_rx_time, priv,
-				DEFAULT_PTP_RX_BUF_SZ);
-		break;
-
-	/* clear transportSpecific field*/
-	case PTP_MSG_ALL_OTHER:
-		msg_type = (*((u8 *)(skb->data +
-				FEC_PTP_MSG_TYPE_OFFS))) & 0x0F;
-		switch (msg_type) {
-		case PTP_MSG_P_DEL_REQ:
-			fec_ptp_insert(&(priv->rx_time_pdel_req), &tmp_rx_time,
-					priv, DEFAULT_PTP_RX_BUF_SZ);
+	ptp_loc = fec_ptp_parse_packet(skb, &eth_type);
+	if (ptp_loc != NULL) {
+		/* store identification data */
+		tmp_rx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
+		tmp_rx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
+		switch (ntohs(eth_type)) {
+		case ETH_P_IP:
+			tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
 			break;
-		case PTP_MSG_P_DEL_RESP:
-			fec_ptp_insert(&(priv->rx_time_pdel_resp), &tmp_rx_time,
-					priv, DEFAULT_PTP_RX_BUF_SZ);
+		case ETH_P_IPV6:
+			tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
 			break;
-		default:
+		case ETH_P_1588:
+			tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
 			break;
+		default:
+			return;
 		}
-		break;
-	default:
-		break;
+		tmp_rx_time.ident.seq_id =
+			ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
+		memcpy(tmp_rx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
+						PTP_SOURCE_PORT_LENGTH);
+		/* store rx timestamp */
+		tmp_rx_time.ts.sec = fpp->prtc;
+		tmp_rx_time.ts.nsec = bdp->ts;
+
+		/* insert timestamp in circular buffer */
+		fec_ptp_insert(&(fpp->rx_timestamps), &tmp_rx_time);
 	}
-
-	wake_up_interruptible(&ptp_rx_ts_wait);
 }
 
 static uint8_t fec_get_tx_timestamp(struct fec_ptp_private *priv,
-				    struct ptp_ts_data *pts,
+				    struct fec_ptp_ts_data *pts,
 				    struct ptp_time *tx_time)
 {
-	struct fec_ptp_data_t tmp;
-	int flag;
-	u8 mode;
-
-	tmp.key = pts->seq_id;
-	memcpy(tmp.spid, pts->spid, 10);
-	mode = pts->message_type;
-
-	switch (mode) {
-	case PTP_MSG_SYNC:
-		flag = fec_ptp_find_and_remove(&(priv->tx_time_sync), &tmp,
-						priv, DEFAULT_PTP_TX_BUF_SZ);
-		break;
-	case PTP_MSG_DEL_REQ:
-		flag = fec_ptp_find_and_remove(&(priv->tx_time_del_req), &tmp,
-						priv, DEFAULT_PTP_TX_BUF_SZ);
-		break;
+	int ret = 0;
 
-	case PTP_MSG_P_DEL_REQ:
-		flag = fec_ptp_find_and_remove(&(priv->tx_time_pdel_req), &tmp,
-						priv, DEFAULT_PTP_TX_BUF_SZ);
-		break;
-	case PTP_MSG_P_DEL_RESP:
-		flag = fec_ptp_find_and_remove(&(priv->tx_time_pdel_resp), &tmp,
-						priv, DEFAULT_PTP_TX_BUF_SZ);
-		break;
-
-	default:
-		flag = 1;
-		printk(KERN_ERR "ERROR\n");
-		break;
-	}
-
-	if (!flag) {
-		tx_time->sec = tmp.ts_time.sec;
-		tx_time->nsec = tmp.ts_time.nsec;
-		return 0;
-	} else {
-		wait_event_interruptible_timeout(ptp_tx_ts_wait, 0,
-					PTP_GET_TX_TIMEOUT);
-
-		switch (mode) {
-		case PTP_MSG_SYNC:
-			flag = fec_ptp_find_and_remove(&(priv->tx_time_sync),
-					&tmp, priv, DEFAULT_PTP_TX_BUF_SZ);
-			break;
-		case PTP_MSG_DEL_REQ:
-			flag = fec_ptp_find_and_remove(&(priv->tx_time_del_req),
-					&tmp, priv, DEFAULT_PTP_TX_BUF_SZ);
-			break;
-		case PTP_MSG_P_DEL_REQ:
-			flag = fec_ptp_find_and_remove(
-				&(priv->tx_time_pdel_req), &tmp, priv,
-				DEFAULT_PTP_TX_BUF_SZ);
-			break;
-		case PTP_MSG_P_DEL_RESP:
-			flag = fec_ptp_find_and_remove(
-				&(priv->tx_time_pdel_resp), &tmp, priv,
-				DEFAULT_PTP_TX_BUF_SZ);
-			break;
-		}
-
-		if (flag == 0) {
-			tx_time->sec = tmp.ts_time.sec;
-			tx_time->nsec = tmp.ts_time.nsec;
-			return 0;
-		}
+	ret = fec_ptp_find_and_remove(&(priv->tx_timestamps),
+					&pts->ident, tx_time);
 
-		return -1;
-	}
+	return ret;
 }
 
 static uint8_t fec_get_rx_timestamp(struct fec_ptp_private *priv,
-				    struct ptp_ts_data *pts,
+				    struct fec_ptp_ts_data *pts,
 				    struct ptp_time *rx_time)
 {
-	struct fec_ptp_data_t tmp;
-	int flag;
-	u8 mode;
-
-	tmp.key = pts->seq_id;
-	memcpy(tmp.spid, pts->spid, 10);
-	mode = pts->message_type;
-
-	switch (mode) {
-	case PTP_MSG_SYNC:
-		flag = fec_ptp_find_and_remove(&(priv->rx_time_sync), &tmp,
-				priv, DEFAULT_PTP_RX_BUF_SZ);
-		break;
-	case PTP_MSG_DEL_REQ:
-		flag = fec_ptp_find_and_remove(&(priv->rx_time_del_req), &tmp,
-				priv, DEFAULT_PTP_RX_BUF_SZ);
-		break;
-
-	case PTP_MSG_P_DEL_REQ:
-		flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_req), &tmp,
-				priv, DEFAULT_PTP_RX_BUF_SZ);
-		break;
-	case PTP_MSG_P_DEL_RESP:
-		flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_resp), &tmp,
-				priv, DEFAULT_PTP_RX_BUF_SZ);
-		break;
-
-	default:
-		flag = 1;
-		printk(KERN_ERR "ERROR\n");
-		break;
-	}
+	int ret = 0;
 
-	if (!flag) {
-		rx_time->sec = tmp.ts_time.sec;
-		rx_time->nsec = tmp.ts_time.nsec;
-		return 0;
-	} else {
-		wait_event_interruptible_timeout(ptp_rx_ts_wait, 0,
-					PTP_GET_RX_TIMEOUT);
+	ret = fec_ptp_find_and_remove(&(priv->rx_timestamps),
+					&pts->ident, rx_time);
 
-		switch (mode) {
-		case PTP_MSG_SYNC:
-			flag = fec_ptp_find_and_remove(&(priv->rx_time_sync),
-				&tmp, priv, DEFAULT_PTP_RX_BUF_SZ);
-			break;
-		case PTP_MSG_DEL_REQ:
-			flag = fec_ptp_find_and_remove(
-				&(priv->rx_time_del_req), &tmp, priv,
-				DEFAULT_PTP_RX_BUF_SZ);
-			break;
-		case PTP_MSG_P_DEL_REQ:
-			flag = fec_ptp_find_and_remove(
-				&(priv->rx_time_pdel_req), &tmp, priv,
-				DEFAULT_PTP_RX_BUF_SZ);
-			break;
-		case PTP_MSG_P_DEL_RESP:
-			flag = fec_ptp_find_and_remove(
-				&(priv->rx_time_pdel_resp), &tmp, priv,
-				DEFAULT_PTP_RX_BUF_SZ);
-			break;
-		}
-
-		if (flag == 0) {
-			rx_time->sec = tmp.ts_time.sec;
-			rx_time->nsec = tmp.ts_time.nsec;
-			return 0;
-		}
-
-		return -1;
-	}
+	return ret;
 }
 
 static void fec_handle_ptpdrift(struct ptp_set_comp *comp,
@@ -593,13 +493,14 @@ static void fec_handle_ptpdrift(struct ptp_set_comp *comp,
 		adj_inc = 1;
 
 		if (ndrift > (FEC_ATIME_CLK / FEC_T_INC_CLK)) {
-			adj_inc = FEC_T_INC_CLK / 2;
-		} else if (ndrift > (FEC_ATIME_CLK / (FEC_T_INC_CLK * 4))) {
-			adj_inc = FEC_T_INC_CLK / 4;
-			adj_period = 2;
+			adj_inc = FEC_T_INC_CLK / FEC_PTP_SPINNER_2;
+		} else if (ndrift > (FEC_ATIME_CLK /
+			(FEC_T_INC_CLK * FEC_PTP_SPINNER_4))) {
+			adj_inc = FEC_T_INC_CLK / FEC_PTP_SPINNER_4;
+			adj_period = FEC_PTP_SPINNER_2;
 		} else {
-			adj_inc = 4;
-			adj_period = 4;
+			adj_inc = FEC_PTP_SPINNER_4;
+			adj_period = FEC_PTP_SPINNER_4;
 		}
 
 		for (i = 1; i < adj_inc; i++) {
@@ -647,86 +548,79 @@ static void fec_set_drift(struct fec_ptp_private *priv,
 	writel(tc.corr_period, fpp->hwp + FEC_ATIME_CORR);
 }
 
-static int ptp_open(struct inode *inode, struct file *file)
-{
-	return 0;
-}
-
-static int ptp_release(struct inode *inode, struct file *file)
-{
-	return 0;
-}
-
-static long ptp_ioctl(
-	struct file *file,
-	unsigned int cmd,
-	unsigned long arg)
+int fec_ptp_ioctl(struct fec_ptp_private *priv, struct ifreq *ifr, int cmd)
 {
-	struct ptp_rtc_time *cnt;
 	struct ptp_rtc_time curr_time;
 	struct ptp_time rx_time, tx_time;
-	struct ptp_ts_data *p_ts;
-	struct ptp_set_comp *p_comp;
-	struct fec_ptp_private *priv;
-	unsigned int minor = MINOR(file->f_path.dentry->d_inode->i_rdev);
+	struct fec_ptp_ts_data p_ts;
+	struct fec_ptp_ts_data *p_ts_user;
+	struct ptp_set_comp p_comp;
+	u32 freq_compensation;
 	int retval = 0;
 
-	priv = (struct fec_ptp_private *) ptp_private[minor];
-
 	switch (cmd) {
+	case PTP_ENBL_TXTS_IOCTL:
+	case PTP_DSBL_TXTS_IOCTL:
+	case PTP_ENBL_RXTS_IOCTL:
+	case PTP_DSBL_RXTS_IOCTL:
+		break;
 	case PTP_GET_RX_TIMESTAMP:
-		p_ts = (struct ptp_ts_data *)arg;
-		retval = fec_get_rx_timestamp(priv, p_ts, &rx_time);
-		if (retval == 0 &&
-			copy_to_user((void __user *)(&(p_ts->ts)), &rx_time,
-					sizeof(rx_time)))
-			retval = -EFAULT;
+		p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
+		if (0 != copy_from_user(&p_ts.ident,
+			&p_ts_user->ident, sizeof(p_ts.ident)))
+			return -EINVAL;
+		if (fec_get_rx_timestamp(priv, &p_ts, &rx_time) != 0)
+			return -EAGAIN;
+		if (copy_to_user((void __user *)(&p_ts_user->ts),
+			&rx_time, sizeof(rx_time)))
+			return -EFAULT;
 		break;
 	case PTP_GET_TX_TIMESTAMP:
-		p_ts = (struct ptp_ts_data *)arg;
-		fec_get_tx_timestamp(priv, p_ts, &tx_time);
-		if (copy_to_user((void __user *)(&(p_ts->ts)), &tx_time,
-				sizeof(tx_time)))
+		p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
+		if (0 != copy_from_user(&p_ts.ident,
+			&p_ts_user->ident, sizeof(p_ts.ident)))
+			return -EINVAL;
+		retval = fec_get_tx_timestamp(priv, &p_ts, &tx_time);
+		if (retval == 0 &&
+			copy_to_user((void __user *)(&p_ts_user->ts),
+				&tx_time, sizeof(tx_time)))
 			retval = -EFAULT;
 		break;
 	case PTP_GET_CURRENT_TIME:
 		fec_get_curr_cnt(priv, &curr_time);
-		if (copy_to_user((void __user *)arg, &curr_time,
-			sizeof(curr_time)))
-			retval = -EFAULT;;
+		if (0 != copy_to_user(ifr->ifr_data,
+					&(curr_time.rtc_time),
+					sizeof(struct ptp_time)))
+			return -EFAULT;
 		break;
 	case PTP_SET_RTC_TIME:
-		cnt = (struct ptp_rtc_time *)arg;
-		fec_set_1588cnt(priv, cnt);
+		if (0 != copy_from_user(&(curr_time.rtc_time),
+					ifr->ifr_data,
+					sizeof(struct ptp_time)))
+			return -EINVAL;
+		fec_set_1588cnt(priv, &curr_time);
 		break;
 	case PTP_FLUSH_TIMESTAMP:
-		/* reset sync buffer */
-		priv->rx_time_sync.head = 0;
-		priv->rx_time_sync.tail = 0;
-		priv->tx_time_sync.head = 0;
-		priv->tx_time_sync.tail = 0;
-		/* reset delay_req buffer */
-		priv->rx_time_del_req.head = 0;
-		priv->rx_time_del_req.tail = 0;
-		priv->tx_time_del_req.head = 0;
-		priv->tx_time_del_req.tail = 0;
-		/* reset pdelay_req buffer */
-		priv->rx_time_pdel_req.head = 0;
-		priv->rx_time_pdel_req.tail = 0;
-		priv->tx_time_pdel_req.head = 0;
-		priv->tx_time_pdel_req.tail = 0;
-		/* reset pdelay_resp buffer */
-		priv->rx_time_pdel_resp.head = 0;
-		priv->rx_time_pdel_resp.tail = 0;
-		priv->tx_time_pdel_resp.head = 0;
-		priv->tx_time_pdel_resp.tail = 0;
+		/* reset tx-timestamping buffer */
+		priv->tx_timestamps.front = 0;
+		priv->tx_timestamps.end = 0;
+		priv->tx_timestamps.size = (DEFAULT_PTP_TX_BUF_SZ + 1);
+		/* reset rx-timestamping buffer */
+		priv->rx_timestamps.front = 0;
+		priv->rx_timestamps.end = 0;
+		priv->rx_timestamps.size = (DEFAULT_PTP_RX_BUF_SZ + 1);
 		break;
 	case PTP_SET_COMPENSATION:
-		p_comp = (struct ptp_set_comp *)arg;
-		fec_set_drift(priv, p_comp);
+		if (0 != copy_from_user(&p_comp, ifr->ifr_data,
+			sizeof(struct ptp_set_comp)))
+			return -EINVAL;
+		fec_set_drift(priv, &p_comp);
 		break;
 	case PTP_GET_ORIG_COMP:
-		((struct ptp_get_comp *)arg)->dw_origcomp = FEC_PTP_ORIG_COMP;
+		freq_compensation = FEC_PTP_ORIG_COMP;
+		if (copy_to_user(ifr->ifr_data, &freq_compensation,
+					sizeof(freq_compensation)) > 0)
+			return -EFAULT;
 		break;
 	default:
 		return -EINVAL;
@@ -734,76 +628,32 @@ static long ptp_ioctl(
 	return retval;
 }
 
-static const struct file_operations ptp_fops = {
-	.owner	= THIS_MODULE,
-	.llseek	= NULL,
-	.read	= NULL,
-	.write	= NULL,
-	.unlocked_ioctl = ptp_ioctl,
-	.open	= ptp_open,
-	.release = ptp_release,
-};
-
-static int init_ptp(void)
-{
-	if (register_chrdev(PTP_MAJOR, "ptp", &ptp_fops))
-		printk(KERN_ERR "Unable to register PTP device as char\n");
-	else
-		printk(KERN_INFO "Register PTP device as char /dev/ptp\n");
-
-	return 0;
-}
-
-static void ptp_free(void)
-{
-	/*unregister the PTP device*/
-	unregister_chrdev(PTP_MAJOR, "ptp");
-}
-
 /*
  * Resource required for accessing 1588 Timer Registers.
  */
 int fec_ptp_init(struct fec_ptp_private *priv, int id)
 {
-	fec_ptp_init_circ(&(priv->rx_time_sync), DEFAULT_PTP_RX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->rx_time_del_req), DEFAULT_PTP_RX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->rx_time_pdel_req), DEFAULT_PTP_RX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->rx_time_pdel_resp), DEFAULT_PTP_RX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->tx_time_sync), DEFAULT_PTP_TX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->tx_time_del_req), DEFAULT_PTP_TX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->tx_time_pdel_req), DEFAULT_PTP_TX_BUF_SZ);
-	fec_ptp_init_circ(&(priv->tx_time_pdel_resp), DEFAULT_PTP_TX_BUF_SZ);
-
-	spin_lock_init(&priv->ptp_lock);
+	/* initialize circular buffer for tx timestamps */
+	if (fec_ptp_init_circ(&(priv->tx_timestamps),
+			(DEFAULT_PTP_TX_BUF_SZ+1)))
+		return 1;
+	/* initialize circular buffer for rx timestamps */
+	if (fec_ptp_init_circ(&(priv->rx_timestamps),
+			(DEFAULT_PTP_RX_BUF_SZ+1)))
+		return 1;
+
 	spin_lock_init(&priv->cnt_lock);
 	ptp_private[id] = priv;
 	priv->dev_id = id;
-	if (id == 0)
-		init_ptp();
 	return 0;
 }
 EXPORT_SYMBOL(fec_ptp_init);
 
 void fec_ptp_cleanup(struct fec_ptp_private *priv)
 {
-
-	if (priv->rx_time_sync.buf)
-		vfree(priv->rx_time_sync.buf);
-	if (priv->rx_time_del_req.buf)
-		vfree(priv->rx_time_del_req.buf);
-	if (priv->rx_time_pdel_req.buf)
-		vfree(priv->rx_time_pdel_req.buf);
-	if (priv->rx_time_pdel_resp.buf)
-		vfree(priv->rx_time_pdel_resp.buf);
-	if (priv->tx_time_sync.buf)
-		vfree(priv->tx_time_sync.buf);
-	if (priv->tx_time_del_req.buf)
-		vfree(priv->tx_time_del_req.buf);
-	if (priv->tx_time_pdel_req.buf)
-		vfree(priv->tx_time_pdel_req.buf);
-	if (priv->tx_time_pdel_resp.buf)
-		vfree(priv->tx_time_pdel_resp.buf);
-
-	ptp_free();
+	if (priv->tx_timestamps.data_buf)
+		vfree(priv->tx_timestamps.data_buf);
+	if (priv->rx_timestamps.data_buf)
+		vfree(priv->rx_timestamps.data_buf);
 }
 EXPORT_SYMBOL(fec_ptp_cleanup);
diff --git a/drivers/net/fec_1588.h b/drivers/net/fec_1588.h
index 8e21519ec45d..bd3565ac5194 100644
--- a/drivers/net/fec_1588.h
+++ b/drivers/net/fec_1588.h
@@ -41,7 +41,6 @@
 #define FEC_T_INC_CORR_MASK		0x00007f00
 #define FEC_T_INC_CORR_OFFSET		8
 
-
 #define FEC_T_INC_50MHZ			20
 #define FEC_ATIME_50MHZ			50000000
 #define FEC_T_INC_CLK			FEC_T_INC_50MHZ
@@ -51,41 +50,45 @@
 
 /* IEEE 1588 definition */
 #define FEC_ECNTRL_TS_EN	0x10
-#define PTP_MAJOR		232	/*the temporary major number
-						 *used by PTP driver, the major
-						 *number 232~239 is unassigned*/
-
-#define DEFAULT_PTP_RX_BUF_SZ		2048
-#define DEFAULT_PTP_TX_BUF_SZ		16
-#define PTP_MSG_SYNC			0x0
-#define PTP_MSG_DEL_REQ			0x1
-#define PTP_MSG_P_DEL_REQ		0x2
-#define PTP_MSG_P_DEL_RESP		0x3
-#define PTP_MSG_DEL_RESP		0x4
-#define PTP_MSG_ALL_OTHER		0x5
-
-#define PTP_GET_TX_TIMESTAMP		0x1
-#define PTP_GET_RX_TIMESTAMP		0x9
-#define PTP_SET_RTC_TIME		0x3
-#define PTP_SET_COMPENSATION		0x4
-#define PTP_GET_CURRENT_TIME		0x5
-#define PTP_FLUSH_TIMESTAMP		0x6
-#define PTP_ADJ_ADDEND			0x7
-#define PTP_GET_ORIG_COMP		0x8
-#define PTP_GET_ADDEND			0xB
-#define PTP_GET_RX_TIMESTAMP_PDELAY_REQ		0xC
-#define PTP_GET_RX_TIMESTAMP_PDELAY_RESP	0xD
-
-#define FEC_PTP_DOMAIN_DLFT		0xe0000181
-#define FEC_PTP_IP_OFFS			0xE
-#define FEC_PTP_UDP_OFFS		0x22
-#define FEC_PTP_MSG_TYPE_OFFS		0x2A
-#define FEC_PTP_SPORT_ID_OFFS		0x3E
-#define FEC_PTP_SEQ_ID_OFFS		0x48
-#define FEC_PTP_CTRL_OFFS		0x4A
+
+#define DEFAULT_PTP_RX_BUF_SZ		64
+#define DEFAULT_PTP_TX_BUF_SZ		64
+
+/* 1588stack API defines */
+#define PTP_ENBL_TXTS_IOCTL	SIOCDEVPRIVATE
+#define PTP_DSBL_TXTS_IOCTL	(SIOCDEVPRIVATE + 1)
+#define PTP_ENBL_RXTS_IOCTL	(SIOCDEVPRIVATE + 2)
+#define PTP_DSBL_RXTS_IOCTL	(SIOCDEVPRIVATE + 3)
+#define PTP_GET_TX_TIMESTAMP	(SIOCDEVPRIVATE + 4)
+#define PTP_GET_RX_TIMESTAMP	(SIOCDEVPRIVATE + 5)
+#define PTP_SET_RTC_TIME	(SIOCDEVPRIVATE + 6)
+#define PTP_GET_CURRENT_TIME	(SIOCDEVPRIVATE + 7)
+#define PTP_SET_COMPENSATION	(SIOCDEVPRIVATE + 9)
+#define PTP_GET_ORIG_COMP	(SIOCDEVPRIVATE + 10)
+#define PTP_FLUSH_TIMESTAMP	(SIOCDEVPRIVATE + 11)
+
+/* IEEE1588 ptp head format */
+#define PTP_CTRL_OFFS		0x52
+#define PTP_SOURCE_PORT_LENGTH	10
+#define	PTP_HEADER_SEQ_OFFS	30
+#define PTP_HEADER_CTL_OFFS	32
+#define PTP_SPID_OFFS		20
+#define PTP_HEADER_SZE		34
+#define PTP_EVENT_PORT		0x013F
+
+#define FEC_VLAN_TAG_LEN	0x04
+#define FEC_ETHTYPE_LEN		0x02
+
+/* 1588-2008 network protocol enumeration values */
+#define FEC_PTP_PROT_IPV4		1
+#define FEC_PTP_PROT_IPV6		2
+#define FEC_PTP_PROT_802_3		3
+#define FEC_PTP_PROT_DONTCARE		0xFFFF
 #define FEC_PACKET_TYPE_UDP		0x11
 
 #define FEC_PTP_ORIG_COMP		0x15555555
+#define FEC_PTP_SPINNER_2		2
+#define FEC_PTP_SPINNER_4		4
 
 /* PTP standard time representation structure */
 struct ptp_time{
@@ -93,27 +96,30 @@ struct ptp_time{
 	u32 nsec;	/* nanoseconds */
 };
 
-/* Structure for PTP Time Stamp */
-struct fec_ptp_data_t {
-	u8		spid[10];
-	int		key;
-	struct ptp_time	ts_time;
+/* struct needed to identify a timestamp */
+struct fec_ptp_ident {
+	u8	version;
+	u8	message_type;
+	u16	netw_prot;
+	u16	seq_id;
+	u8	spid[10];
 };
 
 /* interface for PTP driver command GET_TX_TIME */
-struct ptp_ts_data {
-	/* PTP version */
-	u8 version;
-	/* PTP source port ID */
-	u8 spid[10];
-	/* PTP sequence ID */
-	u16 seq_id;
-	/* PTP message type */
-	u8 message_type;
+struct fec_ptp_ts_data {
+	struct fec_ptp_ident ident;
 	/* PTP timestamp */
 	struct ptp_time ts;
 };
 
+/* circular buffer for ptp timestamps over ioctl */
+struct fec_ptp_circular {
+	int	front;
+	int	end;
+	int	size;
+	struct	fec_ptp_ts_data	*data_buf;
+};
+
 /* interface for PTP driver command SET_RTC_TIME/GET_CURRENT_TIME */
 struct ptp_rtc_time {
 	struct ptp_time rtc_time;
@@ -156,15 +162,8 @@ struct fec_ptp_private {
 	void __iomem *hwp;
 	int	dev_id;
 
-	struct	circ_buf rx_time_sync;
-	struct	circ_buf rx_time_del_req;
-	struct	circ_buf rx_time_pdel_req;
-	struct	circ_buf rx_time_pdel_resp;
-	struct	circ_buf tx_time_sync;
-	struct	circ_buf tx_time_del_req;
-	struct	circ_buf tx_time_pdel_req;
-	struct	circ_buf tx_time_pdel_resp;
-	spinlock_t ptp_lock;
+	struct fec_ptp_circular tx_timestamps;
+	struct fec_ptp_circular rx_timestamps;
 	spinlock_t cnt_lock;
 
 	u64	prtc;
@@ -190,6 +189,8 @@ extern void fec_ptp_store_txstamp(struct fec_ptp_private *priv,
 extern void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 				  struct sk_buff *skb,
 				  struct bufdesc *bdp);
+extern int fec_ptp_ioctl(struct fec_ptp_private *priv,
+				struct ifreq *ifr, int cmd);
 #else
 static inline int fec_ptp_malloc_priv(struct fec_ptp_private **priv)
 {
@@ -215,6 +216,9 @@ static inline void fec_ptp_store_txstamp(struct fec_ptp_private *priv,
 static inline void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 					 struct sk_buff *skb,
 					 struct bufdesc *bdp) {}
+static inline int fec_ptp_ioctl(struct fec_ptp_private *priv,
+				struct ifreq *ifr, int cmd) {}
+
 #endif /* 1588 */
 
 #endif