ENGR00124997 MX28: Add drift setting for ENET IEEE1588 Timer

The ENET IEEE1588 Timer have a correction counter, and it can
slow down or speed up the timer.

Signed-off-by: Xie Xiaobo <X.Xie@freescale.com>

3 files changed, 135 insertions, 23 deletions

diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index b176bd4f20e9..3e4281a4483c 100644
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -296,6 +296,17 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		bufaddr = fep->tx_bounce[index];
 	}
 
+	if (fep->ptimer_present) {
+		if (fec_ptp_do_txstamp(skb))
+			estatus = BD_ENET_TX_TS;
+		else
+			estatus = 0;
+#ifdef CONFIG_FEC_1588
+		bdp->cbd_esc = (estatus | BD_ENET_TX_INT);
+		bdp->cbd_bdu = 0;
+#endif
+	}
+
 #ifdef CONFIG_ARCH_MXS
 	swap_buffer(bufaddr, skb->len);
 #endif
@@ -318,16 +329,6 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			| BD_ENET_TX_LAST | BD_ENET_TX_TC);
 	bdp->cbd_sc = status;
 
-	if (fep->ptimer_present) {
-		if (fec_ptp_do_txstamp(skb))
-			estatus = BD_ENET_TX_TS;
-		else
-			estatus = 0;
-#ifdef CONFIG_FEC_1588
-		bdp->cbd_esc = (estatus | BD_ENET_TX_INT);
-		bdp->cbd_bdu = 0;
-#endif
-	}
 	dev->trans_start = jiffies;
 
 	/* Trigger transmission start */
diff --git a/drivers/net/fec_1588.c b/drivers/net/fec_1588.c
index 37b1b01778fd..c4bf278c7f19 100644
--- a/drivers/net/fec_1588.c
+++ b/drivers/net/fec_1588.c
@@ -178,12 +178,19 @@ void fec_ptp_stop(struct fec_ptp_private *priv)
 static void fec_get_curr_cnt(struct fec_ptp_private *priv,
 			struct ptp_rtc_time *curr_time)
 {
+	u32 tempval;
+
+	writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL);
 	writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL);
 	curr_time->rtc_time.nsec = readl(priv->hwp + FEC_ATIME);
 	curr_time->rtc_time.sec = priv->prtc;
+
 	writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL);
-	if (readl(priv->hwp + FEC_ATIME) < curr_time->rtc_time.nsec)
-		curr_time->rtc_time.sec++;
+	tempval = readl(priv->hwp + FEC_ATIME);
+	if (tempval < curr_time->rtc_time.nsec) {
+		curr_time->rtc_time.nsec = tempval;
+		curr_time->rtc_time.sec = priv->prtc;
+	}
 }
 
 /* Set the 1588 timer counter registers */
@@ -214,7 +221,7 @@ int fec_ptp_do_txstamp(struct sk_buff *skb)
 			return 0;
 
 		udph = udp_hdr(skb);
-		if (udph != NULL && udph->source == 319)
+		if (udph != NULL && ntohs(udph->source) == 319)
 			return 1;
 	}
 
@@ -247,13 +254,13 @@ void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 		return;
 
 	udph = (struct udphdr *)(skb->data + FEC_PTP_UDP_OFFS);
-	if (udph->source != 319)
+	if (ntohs(udph->source) != 319)
 		return;
 
 	seq_id = *((u16 *)(skb->data + FEC_PTP_SEQ_ID_OFFS));
 	control = *((u8 *)(skb->data + FEC_PTP_CTRL_OFFS));
 
-	tmp_rx_time.key = seq_id;
+	tmp_rx_time.key = ntohs(seq_id);
 	tmp_rx_time.ts_time.sec = fpp->prtc;
 	tmp_rx_time.ts_time.nsec = bdp->ts;
 
@@ -370,6 +377,75 @@ static uint8_t fec_get_rx_time(struct fec_ptp_private *priv,
 	}
 }
 
+static void fec_handle_ptpdrift(struct ptp_set_comp *comp,
+				struct ptp_time_correct *ptc)
+{
+	u32 ndrift;
+	u32 i;
+	u32 tmp, tmp_ns, tmp_prid;
+	u32 min_ns, min_prid, miss_ns;
+
+	ndrift = comp->drift;
+	if (ndrift == 0) {
+		ptc->corr_inc = 0;
+		ptc->corr_period = 0;
+		return;
+	}
+
+	if (ndrift >= FEC_ATIME_40MHZ) {
+		ptc->corr_inc = (u32)(ndrift / FEC_ATIME_40MHZ);
+		ptc->corr_period = 1;
+		return;
+	}
+
+	min_ns = 1;
+	tmp = FEC_ATIME_40MHZ % ndrift;
+	tmp_prid = (u32)(FEC_ATIME_40MHZ / ndrift);
+	min_prid = tmp_prid;
+	miss_ns = tmp / tmp_prid;
+	for (i = 2; i <= FEC_T_INC_40MHZ; i++) {
+		tmp = (FEC_ATIME_40MHZ * i) % ndrift;
+		tmp_prid = (FEC_ATIME_40MHZ * i) / ndrift;
+		tmp_ns = tmp / tmp_prid;
+		if (tmp_ns <= 10) {
+			min_ns = i;
+			min_prid = tmp_prid;
+			break;
+		}
+		if (tmp_ns < miss_ns) {
+			min_ns = i;
+			min_prid = tmp_prid;
+			miss_ns = tmp_ns;
+		}
+	}
+
+	ptc->corr_inc = min_ns;
+	ptc->corr_period = min_prid;
+}
+
+static void fec_set_drift(struct fec_ptp_private *priv,
+			  struct ptp_set_comp *comp)
+{
+	struct ptp_time_correct	tc;
+	struct fec_ptp_private *fpp = priv;
+	u32 tmp, corr_ns;
+
+	fec_handle_ptpdrift(comp, &tc);
+	if (tc.corr_inc == 0)
+		return;
+
+	if (comp->o_ops == TRUE)
+		corr_ns = FEC_T_INC_40MHZ + tc.corr_inc;
+	else
+		corr_ns = FEC_T_INC_40MHZ - tc.corr_inc;
+
+	tmp = readl(fpp->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK;
+	tmp |= corr_ns << FEC_T_INC_CORR_OFFSET;
+	writel(tmp, fpp->hwp + FEC_ATIME_INC);
+
+	writel(tc.corr_period, fpp->hwp + FEC_ATIME_CORR);
+}
+
 static int ptp_open(struct inode *inode, struct file *file)
 {
 	return 0;
@@ -390,6 +466,7 @@ static int ptp_ioctl(
 	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(inode->i_rdev);
 	int retval = 0;
@@ -432,10 +509,11 @@ static int ptp_ioctl(
 		priv->rx_time_pdel_resp.tail = 0;
 		break;
 	case PTP_SET_COMPENSATION:
-		/* TBD */
+		p_comp = (struct ptp_set_comp *)arg;
+		fec_set_drift(priv, p_comp);
 		break;
 	case PTP_GET_ORIG_COMP:
-		/* TBD */
+		((struct ptp_get_comp *)arg)->dw_origcomp = FEC_PTP_ORIG_COMP;
 		break;
 	default:
 		return -EINVAL;
diff --git a/drivers/net/fec_1588.h b/drivers/net/fec_1588.h
index a503527eadbb..800ff310668f 100644
--- a/drivers/net/fec_1588.h
+++ b/drivers/net/fec_1588.h
@@ -24,6 +24,9 @@
 
 #include <linux/circ_buf.h>
 
+#define FALSE			0
+#define TRUE			1
+
 /* FEC 1588 register bits */
 #define FEC_T_CTRL_CAPTURE		0x00000800
 #define FEC_T_CTRL_RESTART		0x00000200
@@ -32,8 +35,11 @@
 
 #define FEC_T_INC_MASK			0x0000007f
 #define FEC_T_INC_OFFSET		0
+#define FEC_T_INC_CORR_MASK		0x00007f00
+#define FEC_T_INC_CORR_OFFSET		8
 
 #define FEC_T_INC_40MHZ			25
+#define FEC_ATIME_40MHZ			40000000
 
 #define FEC_T_PERIOD_ONE_SEC		0x3B9ACA00
 
@@ -52,7 +58,7 @@
 #define PTP_MSG_ALL_OTHER		0x5
 
 #define PTP_GET_TX_TIMESTAMP		0x1
-#define PTP_GET_RX_TIMESTAMP		0x2
+#define PTP_GET_RX_TIMESTAMP		0x9
 #define PTP_SET_RTC_TIME		0x3
 #define PTP_SET_COMPENSATION		0x4
 #define PTP_GET_CURRENT_TIME		0x5
@@ -64,13 +70,15 @@
 #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_SEQ_ID_OFFS		0x48
-#define FEC_PTP_CTRL_OFFS		0x4A
+#define FEC_PTP_IP_OFFS			0x0
+#define FEC_PTP_UDP_OFFS		0x14
+#define FEC_PTP_MSG_TYPE_OFFS		0x1C
+#define FEC_PTP_SEQ_ID_OFFS		0x3A
+#define FEC_PTP_CTRL_OFFS		0x3C
 #define FEC_PACKET_TYPE_UDP		0x11
 
+#define FEC_PTP_ORIG_COMP		0x15555555
+
 /* PTP standard time representation structure */
 struct ptp_time{
 	u64 sec;	/* seconds */
@@ -102,6 +110,31 @@ struct ptp_rtc_time {
 	struct ptp_time rtc_time;
 };
 
+/* interface for PTP driver command SET_COMPENSATION */
+struct ptp_set_comp {
+	u32 drift;
+	bool o_ops;
+};
+
+/* interface for PTP driver command GET_ORIG_COMP */
+struct ptp_get_comp {
+	/* the initial compensation value */
+	u32 dw_origcomp;
+	/* the minimum compensation value */
+	u32 dw_mincomp;
+	/*the max compensation value*/
+	u32 dw_maxcomp;
+	/*the min drift applying min compensation value in ppm*/
+	u32 dw_mindrift;
+	/*the max drift applying max compensation value in ppm*/
+	u32 dw_maxdrift;
+};
+
+struct ptp_time_correct {
+	u32 corr_period;
+	u32 corr_inc;
+};
+
 /* PTP message version */
 #define PTP_1588_MSG_VER_1	1
 #define PTP_1588_MSG_VER_2	2