Patchwork MX28 fec clock frequency

login
register
mail settings
Submitter Peter Rusko
Date Jan. 9, 2012, 2:07 p.m.
Message ID <4F0AF4B0.4030403@prolan.hu>
Download mbox | patch
Permalink /patch/135026/
State New
Headers show

Comments

Peter Rusko - Jan. 9, 2012, 2:07 p.m.
On 2012-01-08 04:32, Shawn Guo wrote:
> Hi Peter,
>
> On Fri, Jan 06, 2012 at 12:08:51PM +0100, Peter Rusko wrote:
>> Hi all,
>>
>> I'm trying to get the fec clock frequency on the i.MX28 processor. I0
>> need it for a PTP clock and it seems that I get a wrong value:
>> clk_get_rate returns 151578947.
>>
>
> The ptp clock (CLK_ENET_TIME) was missed from the initial imx28 clock
> support.  The fec_clk which is one child of hbus_clk (151 MHz) is
> taken as MDIO clock in fec driver.
>

I'm working on a ptp driver now and I can't deal with the clock.

I've added the CLK_ENET_TIME clock and set it to 40MHz. It should give a
25ns period for the counter in ptp. However the ptp clock is too slow.

If i increase the number (up to the maximum, which is 0x7f), the clock
is still slower then normal. It's like the CLK_ENET_TIME frequency is
less then I think. What can be the problem?

I've attached a patch.

Regards,
Shawn Guo - Jan. 10, 2012, 8:21 a.m.
On Mon, Jan 09, 2012 at 03:07:44PM +0100, Peter Rusko wrote:
> +	/* ENET_CLK setup */
> +	enet_clk.set_parent(&enet_clk, &pll0_clk);
> +	__raw_writel(BF_CLKCTRL_ENET_DIV(12),
> +		       	CLKCTRL_BASE_ADDR + HW_CLKCTRL_ENET);
> +
I'm not sure it's causing the problem you are seeing.  But from i.MX28
spec, it seems that bit BUSY_TIME of register HW_CLKCTRL_ENET should
be polled for new divider setting?
Peter Rusko - Jan. 10, 2012, 1:08 p.m.
> I'm not sure it's causing the problem you are seeing.  But from i.MX28
> spec, it seems that bit BUSY_TIME of register HW_CLKCTRL_ENET should
> be polled for new divider setting?
>

Thank you, I've added the check, but unfortunately it didn't help :(
Shawn Guo - Jan. 10, 2012, 2 p.m.
On Tue, Jan 10, 2012 at 02:08:31PM +0100, Peter Rusko wrote:
> >I'm not sure it's causing the problem you are seeing.  But from i.MX28
> >spec, it seems that bit BUSY_TIME of register HW_CLKCTRL_ENET should
> >be polled for new divider setting?
> >
> 
> Thank you, I've added the check, but unfortunately it didn't help :(
> 
Another point worth checking is bit field ATIME_INC of register
HW_ENET_MAC_ATIME_INC.  It should be 25 if your CLK_ENET_TIME runs
at 40 MHz.
Peter Rusko - Jan. 10, 2012, 3:39 p.m.
On 2012-01-10 15:00, Shawn Guo wrote:
> Another point worth checking is bit field ATIME_INC of register
> HW_ENET_MAC_ATIME_INC.  It should be 25 if your CLK_ENET_TIME runs
> at 40 MHz.
>

No, that was set correctly. Finally, I've found my mistake. I've 
overwritten the other bits when writing the divider so xtal clock was 
selected automatically.
Silly little mistake, isn't it? :) Anyway, thank you for your help.

Patch

diff --git a/arch/arm/mach-mxs/clock-mx28.c b/arch/arm/mach-mxs/clock-mx28.c
index 229ae34..4991e76 100644
--- a/arch/arm/mach-mxs/clock-mx28.c
+++ b/arch/arm/mach-mxs/clock-mx28.c
@@ -267,6 +267,7 @@  _CLK_GET_RATE1(gpmi_clk, GPMI)
 _CLK_GET_RATE1(lcdif_clk, DIS_LCDIF)
 _CLK_GET_RATE1(saif0_clk, SAIF0)
 _CLK_GET_RATE1(saif1_clk, SAIF1)
+_CLK_GET_RATE1(enet_clk, ENET)
 
 #define _CLK_GET_RATE_STUB(name)					\
 static unsigned long name##_get_rate(struct clk *clk)			\
@@ -523,6 +524,36 @@  _CLK_SET_PARENT_STUB(fec_clk)
 _CLK_SET_PARENT_STUB(can0_clk)
 _CLK_SET_PARENT_STUB(can1_clk)
 
+static int enet_clk_set_parent(struct clk *clk, struct clk *parent)
+{
+	int src;
+
+	do {
+		if (parent == clk->parent)
+			return 0;
+		if (parent == &ref_xtal_clk) {
+			src = 0x00;
+			break;
+		}
+		if (parent == &pll0_clk) {
+			src = 0x01;
+			break;
+		}
+		if (parent == &pll2_clk) {
+			src = 0x03;
+			break;
+		}
+		return -EINVAL;
+	} while (0);
+
+
+	__raw_writel(BF_CLKCTRL_ENET_TIME_SEL(src),
+		 CLKCTRL_BASE_ADDR + HW_CLKCTRL_ENET);
+	clk->parent = parent;
+
+	return 0;
+}
+
 /*
  * clk definition
  */
@@ -571,6 +602,12 @@  static struct clk usb1_clk = {
 	.parent = &pll1_clk,
 };
 
+static struct clk enet_clk = {
+	.get_rate	= enet_clk_get_rate,
+	.set_parent	= enet_clk_set_parent,
+	.parent		= &pll2_clk,
+};
+
 #define _DEFINE_CLOCK(name, er, es, p)					\
 	static struct clk name = {					\
 		.enable_reg	= CLKCTRL_BASE_ADDR + HW_CLKCTRL_##er,	\
@@ -614,6 +651,8 @@  static struct clk_lookup lookups[] = {
 	_REGISTER_CLOCK("duart", NULL, uart_clk)
 	_REGISTER_CLOCK("imx28-fec.0", NULL, fec_clk)
 	_REGISTER_CLOCK("imx28-fec.1", NULL, fec_clk)
+	_REGISTER_CLOCK("imx28-fec.0", "enet_clk", enet_clk)
+	_REGISTER_CLOCK("imx28-fec.1", "enet_clk", enet_clk)
 	_REGISTER_CLOCK("mxs-auart.0", NULL, uart_clk)
 	_REGISTER_CLOCK("mxs-auart.1", NULL, uart_clk)
 	_REGISTER_CLOCK("mxs-auart.2", NULL, uart_clk)
@@ -672,6 +711,11 @@  static int clk_misc_init(void)
 	saif1_clk.parent = (reg & BM_CLKCTRL_CLKSEQ_BYPASS_SAIF1) ?
 			&ref_xtal_clk : &pll0_clk;
 
+	/* ENET_CLK setup */
+	enet_clk.set_parent(&enet_clk, &pll0_clk);
+	__raw_writel(BF_CLKCTRL_ENET_DIV(12),
+		       	CLKCTRL_BASE_ADDR + HW_CLKCTRL_ENET);
+
 	/* Use int div over frac when both are available */
 	__raw_writel(BM_CLKCTRL_CPU_DIV_XTAL_FRAC_EN,
 			CLKCTRL_BASE_ADDR + HW_CLKCTRL_CPU_CLR);
diff --git a/drivers/net/ethernet/freescale/Makefile b/drivers/net/ethernet/freescale/Makefile
index 1752488..4b2af6d 100644
--- a/drivers/net/ethernet/freescale/Makefile
+++ b/drivers/net/ethernet/freescale/Makefile
@@ -3,6 +3,7 @@ 
 #
 
 obj-$(CONFIG_FEC) += fec.o
+obj-$(CONFIG_PTP_1588_CLOCK_FEC) += fec_ptp.o
 obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx.o
 ifeq ($(CONFIG_FEC_MPC52xx_MDIO),y)
 	obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx_phy.o
diff --git a/drivers/net/ethernet/freescale/fec.c b/drivers/net/ethernet/freescale/fec.c
index 1124ce0..4be9cea 100644
--- a/drivers/net/ethernet/freescale/fec.c
+++ b/drivers/net/ethernet/freescale/fec.c
@@ -49,6 +49,8 @@ 
 #include <linux/of_gpio.h>
 #include <linux/of_net.h>
 
+#include <mach/clock.h>
+
 #include <asm/cacheflush.h>
 
 #ifndef CONFIG_ARM
@@ -57,6 +59,9 @@ 
 #endif
 
 #include "fec.h"
+#ifdef CONFIG_PTP_1588_CLOCK_FEC
+#include "fec_ptp.h"
+#endif
 
 #if defined(CONFIG_ARM)
 #define FEC_ALIGNMENT	0xf
@@ -138,25 +143,6 @@  MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #endif
 #endif /* CONFIG_M5272 */
 
-/* The number of Tx and Rx buffers.  These are allocated from the page
- * pool.  The code may assume these are power of two, so it it best
- * to keep them that size.
- * We don't need to allocate pages for the transmitter.  We just use
- * the skbuffer directly.
- */
-#define FEC_ENET_RX_PAGES	8
-#define FEC_ENET_RX_FRSIZE	2048
-#define FEC_ENET_RX_FRPPG	(PAGE_SIZE / FEC_ENET_RX_FRSIZE)
-#define RX_RING_SIZE		(FEC_ENET_RX_FRPPG * FEC_ENET_RX_PAGES)
-#define FEC_ENET_TX_FRSIZE	2048
-#define FEC_ENET_TX_FRPPG	(PAGE_SIZE / FEC_ENET_TX_FRSIZE)
-#define TX_RING_SIZE		16	/* Must be power of two */
-#define TX_RING_MOD_MASK	15	/*   for this to work */
-
-#if (((RX_RING_SIZE + TX_RING_SIZE) * 8) > PAGE_SIZE)
-#error "FEC: descriptor ring size constants too large"
-#endif
-
 /* Interrupt events/masks. */
 #define FEC_ENET_HBERR	((uint)0x80000000)	/* Heartbeat error */
 #define FEC_ENET_BABR	((uint)0x40000000)	/* Babbling receiver */
@@ -177,9 +163,6 @@  MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define PKT_MINBUF_SIZE		64
 #define PKT_MAXBLR_SIZE		1520
 
-/* This device has up to three irqs on some platforms */
-#define FEC_IRQ_NUM		3
-
 /*
  * The 5270/5271/5280/5282/532x RX control register also contains maximum frame
  * size bits. Other FEC hardware does not, so we need to take that into
@@ -192,59 +175,6 @@  MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define	OPT_FRAME_SIZE	0
 #endif
 
-/* The FEC buffer descriptors track the ring buffers.  The rx_bd_base and
- * tx_bd_base always point to the base of the buffer descriptors.  The
- * cur_rx and cur_tx point to the currently available buffer.
- * The dirty_tx tracks the current buffer that is being sent by the
- * controller.  The cur_tx and dirty_tx are equal under both completely
- * empty and completely full conditions.  The empty/ready indicator in
- * the buffer descriptor determines the actual condition.
- */
-struct fec_enet_private {
-	/* Hardware registers of the FEC device */
-	void __iomem *hwp;
-
-	struct net_device *netdev;
-
-	struct clk *clk;
-
-	/* The saved address of a sent-in-place packet/buffer, for skfree(). */
-	unsigned char *tx_bounce[TX_RING_SIZE];
-	struct	sk_buff* tx_skbuff[TX_RING_SIZE];
-	struct	sk_buff* rx_skbuff[RX_RING_SIZE];
-	ushort	skb_cur;
-	ushort	skb_dirty;
-
-	/* CPM dual port RAM relative addresses */
-	dma_addr_t	bd_dma;
-	/* Address of Rx and Tx buffers */
-	struct bufdesc	*rx_bd_base;
-	struct bufdesc	*tx_bd_base;
-	/* The next free ring entry */
-	struct bufdesc	*cur_rx, *cur_tx;
-	/* The ring entries to be free()ed */
-	struct bufdesc	*dirty_tx;
-
-	uint	tx_full;
-	/* hold while accessing the HW like ringbuffer for tx/rx but not MAC */
-	spinlock_t hw_lock;
-
-	struct	platform_device *pdev;
-
-	int	opened;
-
-	/* Phylib and MDIO interface */
-	struct	mii_bus *mii_bus;
-	struct	phy_device *phy_dev;
-	int	mii_timeout;
-	uint	phy_speed;
-	phy_interface_t	phy_interface;
-	int	link;
-	int	full_duplex;
-	struct	completion mdio_done;
-	int	irq[FEC_IRQ_NUM];
-};
-
 /* FEC MII MMFR bits definition */
 #define FEC_MMFR_ST		(1 << 30)
 #define FEC_MMFR_OP_READ	(2 << 28)
@@ -385,9 +315,11 @@  fec_restart(struct net_device *ndev, int duplex)
 	u32 rcntl = OPT_FRAME_SIZE | 0x04;
 	u32 ecntl = 0x2; /* ETHEREN */
 
+	mxs_ptp_store(fep); // Store ptp registers before reset
 	/* Whack a reset.  We should wait for this. */
 	writel(1, fep->hwp + FEC_ECNTRL);
 	udelay(10);
+	mxs_ptp_restore(fep); // restore registers
 
 	/*
 	 * enet-mac reset will reset mac address registers too,
@@ -524,9 +456,11 @@  fec_stop(struct net_device *ndev)
 			printk("fec_stop : Graceful transmit stop did not complete !\n");
 	}
 
+	mxs_ptp_store(fep); // Store ptp registers before reset
 	/* Whack a reset.  We should wait for this. */
 	writel(1, fep->hwp + FEC_ECNTRL);
 	udelay(10);
+	mxs_ptp_restore(fep); // restore registers
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
 	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 
@@ -773,6 +707,14 @@  fec_enet_interrupt(int irq, void *dev_id)
 			ret = IRQ_HANDLED;
 			complete(&fep->mdio_done);
 		}
+
+#ifdef CONFIG_PTP_1588_CLOCK_FEC
+		/* PTP interrupts */
+		if (int_events & FEC_ENET_PTP_IRMASK) {
+			ret = mxs_ptp_interrupt(int_events &
+					FEC_ENET_PTP_IRMASK, fep);
+		}
+#endif
 	} while (int_events);
 
 	return ret;
@@ -1521,6 +1463,7 @@  fec_probe(struct platform_device *pdev)
 	int i, irq, ret = 0;
 	struct resource *r;
 	const struct of_device_id *of_id;
+struct clk *pclk;
 
 	of_id = of_match_device(fec_dt_ids, &pdev->dev);
 	if (of_id)
@@ -1605,6 +1548,8 @@  fec_probe(struct platform_device *pdev)
 	if (ret)
 		goto failed_register;
 
+	mxs_ptp_init(fep);
+
 	return 0;
 
 failed_register:
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index 8b2c6d7..2493ff5 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -13,6 +13,16 @@ 
 #define	FEC_H
 /****************************************************************************/
 
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/netdevice.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include <linux/spinlock_types.h>
+#include <linux/types.h>
+#include <linux/ptp_clock_kernel.h>
+
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
     defined(CONFIG_M520x) || defined(CONFIG_M532x) || \
     defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28)
@@ -143,6 +153,99 @@  struct bufdesc {
 #define BD_ENET_TX_CSL          ((ushort)0x0001)
 #define BD_ENET_TX_STATS        ((ushort)0x03ff)        /* All status bits */
 
+/* The number of Tx and Rx buffers.  These are allocated from the page
+ * pool.  The code may assume these are power of two, so it it best
+ * to keep them that size.
+ * We don't need to allocate pages for the transmitter.  We just use
+ * the skbuffer directly.
+ */
+
+#define FEC_ENET_RX_PAGES	8
+#define FEC_ENET_RX_FRSIZE	2048
+#define FEC_ENET_RX_FRPPG	(PAGE_SIZE / FEC_ENET_RX_FRSIZE)
+#define RX_RING_SIZE		(FEC_ENET_RX_FRPPG * FEC_ENET_RX_PAGES)
+#define FEC_ENET_TX_FRSIZE	2048
+#define FEC_ENET_TX_FRPPG	(PAGE_SIZE / FEC_ENET_TX_FRSIZE)
+#define TX_RING_SIZE		16	/* Must be power of two */
+#define TX_RING_MOD_MASK	15	/*   for this to work */
+
+#if (((RX_RING_SIZE + TX_RING_SIZE) * 8) > PAGE_SIZE)
+#error "FEC: descriptor ring size constants too large"
+#endif
+
+/* This device has up to three irqs on some platforms */
+#define FEC_IRQ_NUM		3
+
+/* The FEC buffer descriptors track the ring buffers.  The rx_bd_base and
+ * tx_bd_base always point to the base of the buffer descriptors.  The
+ * cur_rx and cur_tx point to the currently available buffer.
+ * The dirty_tx tracks the current buffer that is being sent by the
+ * controller.  The cur_tx and dirty_tx are equal under both completely
+ * empty and completely full conditions.  The empty/ready indicator in
+ * the buffer descriptor determines the actual condition.
+ */
+struct fec_enet_private {
+	/* Hardware registers of the FEC device */
+	void __iomem *hwp;
+
+	struct net_device *netdev;
+
+	struct clk *clk;
+
+	/* The saved address of a sent-in-place packet/buffer, for skfree(). */
+	unsigned char *tx_bounce[TX_RING_SIZE];
+	struct	sk_buff* tx_skbuff[TX_RING_SIZE];
+	struct	sk_buff* rx_skbuff[RX_RING_SIZE];
+	ushort	skb_cur;
+	ushort	skb_dirty;
+
+	/* CPM dual port RAM relative addresses */
+	dma_addr_t	bd_dma;
+	/* Address of Rx and Tx buffers */
+	struct bufdesc	*rx_bd_base;
+	struct bufdesc	*tx_bd_base;
+	/* The next free ring entry */
+	struct bufdesc	*cur_rx, *cur_tx;
+	/* The ring entries to be free()ed */
+	struct bufdesc	*dirty_tx;
+
+	uint	tx_full;
+	/* hold while accessing the HW like ringbuffer for tx/rx but not MAC */
+	spinlock_t hw_lock;
+
+	struct	platform_device *pdev;
+
+	int	opened;
+
+	/* Phylib and MDIO interface */
+	struct	mii_bus *mii_bus;
+	struct	phy_device *phy_dev;
+	int	mii_timeout;
+	uint	phy_speed;
+	phy_interface_t	phy_interface;
+	int	link;
+	int	full_duplex;
+	struct	completion mdio_done;
+	int	irq[FEC_IRQ_NUM];
+	
+#ifdef CONFIG_PTP_1588_CLOCK_FEC
+	/* PTP support */
+	struct clk *enet_clk;
+
+	struct ptp_clock	*ptp_clock;
+	struct ptp_clock_info	clock_info;
+
+	// Software clock representing seconds
+	long	ptp_sec;
+
+	u32	ptp_rate;
+	int	ptp_pps_enabled;
+
+#define PTP_REG_SIZE 7
+	u32 reg_store[PTP_REG_SIZE];
+#endif
+};
 
 /****************************************************************************/
 #endif /* FEC_H */
+
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
new file mode 100644
index 0000000..95998e3
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -0,0 +1,223 @@ 
+#include <linux/module.h>
+#include <linux/irqreturn.h>
+#include <linux/clk.h>
+#include <linux/time.h>
+#include <linux/ptp_clock_kernel.h>
+
+#include "fec.h"
+#include "fec_ptp.h"
+
+#define ENET_MAC_ATIME_CTRL 0x400
+#define BP_CAPTURE 11
+#define BP_EVT_PERIOD_RST 5
+#define BP_EVT_PERIOD_ENA 4
+#define BP_EVT_OFFSET_RST 3
+#define BP_EVT_OFFSET_ENA 2
+#define BP_ENABLE 0
+
+#define ENET_MAC_ATIME 0x404
+
+#define ENET_MAC_ATIME_OFFSET 0x408
+
+#define ENET_MAC_ATIME_CORR 0x410
+
+#define ENET_MAC_ATIME_INC 0x414
+#define BM_ATIME_INC_CORR 0x00007F00
+#define BP_ATIME_INC_CORR 8
+#define BF_ATIME_INC_CORR(v)  \
+		(((v) << BP_ATIME_INC_CORR) & BM_ATIME_INC_CORR)
+#define BM_ATIME_INC 0x0000007F
+#define BP_ATIME_INC 0
+#define BF_ATIME_INC(v)  \
+		(((v) << BP_ATIME_INC) & BM_ATIME_INC)
+
+#undef to_fec_enet_private
+#define to_fec_enet_private(i) container_of(i, struct fec_enet_private,	\
+		clock_info)
+
+u32 mxs_ptp_capture(struct fec_enet_private *fep) {
+	int i;
+
+	writel(1<<BP_CAPTURE, fep->hwp + ENET_MAC_ATIME_CTRL);
+	for (i=100000; i && (readl(fep->hwp + ENET_MAC_ATIME_CTRL) & (1<<BP_CAPTURE)); i--)
+		cpu_relax();
+	if (!i) {
+		pr_info("Read timeout\n");
+		return 0;
+	}
+
+	return readl(fep->hwp + ENET_MAC_ATIME);
+}
+
+int mxs_ptp_interrupt(u32 reg, struct fec_enet_private *fep)
+{
+	if (reg & FEC_ENET_TS_AVAIL) {
+	}
+	if (reg & FEC_ENET_TS_TIMER) {
+		fep->ptp_sec++;
+		if (fep->ptp_pps_enabled)
+		{
+			struct ptp_clock_event event;
+
+			event.type = PTP_CLOCK_PPS;
+
+			ptp_clock_event(fep->ptp_clock, &event);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void calc_atime(unsigned long new_rate, u32 *atime_corr, u32 *atime_norm_inc, u32 *atime_corr_inc)
+{
+	// TODO: complete calculation
+	*atime_corr = 0;
+	*atime_corr_inc = 0;
+	*atime_norm_inc = DIV_ROUND_CLOSEST (NSEC_PER_SEC, new_rate);
+}
+
+static int mxs_ptp_adjfreq(struct ptp_clock_info *ptp, s32 delta)
+{
+	struct fec_enet_private *fep = to_fec_enet_private(ptp);
+	u32 atime_corr, atime_norm_inc, atime_corr_inc;
+	u32 reg;
+
+	fep->ptp_rate += delta;
+	calc_atime(clk_get_rate(fep->enet_clk) + fep->ptp_rate, &atime_corr, &atime_norm_inc, &atime_corr_inc);
+
+	writel(atime_corr, fep->hwp + ENET_MAC_ATIME_CORR);
+	writel(BF_ATIME_INC(atime_norm_inc) | BF_ATIME_INC_CORR(atime_corr_inc),
+			fep->hwp + ENET_MAC_ATIME_INC);
+
+	return 0;
+}
+
+static int mxs_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct fec_enet_private *fep = to_fec_enet_private(ptp);
+	u32 reg;
+
+	// for now we only allow negative change
+	if (delta >= NSEC_PER_SEC || delta < 0) // 1 sec
+		return -EINVAL;
+
+	writel(NSEC_PER_SEC-delta, fep->hwp + ENET_MAC_ATIME_OFFSET);
+
+	reg = readl(fep->hwp + ENET_MAC_ATIME_CTRL);
+	writel(reg | (1<<BP_EVT_OFFSET_RST) | (1<<BP_EVT_OFFSET_ENA), fep->hwp + ENET_MAC_ATIME_CTRL);
+
+	return 0;
+}
+
+
+static int mxs_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+	struct fec_enet_private *fep = to_fec_enet_private(ptp);
+	u32 reg;
+
+	reg = mxs_ptp_capture(fep);
+
+	ts->tv_nsec = reg;
+	ts->tv_sec = fep->ptp_sec;
+
+	return 0;
+}
+
+static int mxs_ptp_settime(struct ptp_clock_info *ptp, const struct timespec *ts)
+{
+	struct fec_enet_private *fep = to_fec_enet_private(ptp);
+
+	fep->ptp_sec = ts->tv_sec;
+	writel(ts->tv_nsec, fep->hwp + ENET_MAC_ATIME);
+
+	return 0;
+}
+
+static int mxs_ptp_enable(struct ptp_clock_info *ptp,
+	      struct ptp_clock_request *request, int on)
+{
+	struct fec_enet_private *fep = to_fec_enet_private(ptp);
+
+	switch (request->type) {
+	case PTP_CLK_REQ_PPS:
+		fep->ptp_pps_enabled = on;
+		return 0;
+	default:
+		break;
+	}
+
+	return -EOPNOTSUPP;
+}
+
+static void mxs_ptp_registers_set(struct fec_enet_private *fep)
+{
+	u32 reg;
+
+	reg = readl(fep->hwp + FEC_ECNTRL);
+	writel(reg | 1<<4, fep->hwp + FEC_ECNTRL);
+
+	reg = readl(fep->hwp + FEC_IMASK);
+	writel(reg | (FEC_ENET_TS_TIMER), fep->hwp + FEC_IMASK);
+};
+
+static struct ptp_clock_info mxs_ptp_info = {
+	.owner = THIS_MODULE,
+	.name = "mxs-ptpclock",
+	.max_adj = NSEC_PER_SEC,
+	.n_alarm = 1,
+	.n_ext_ts = 4,
+	.n_per_out = 4,
+	.pps = 1,
+
+	.adjfreq = mxs_ptp_adjfreq,
+	.adjtime = mxs_ptp_adjtime,
+	.gettime = mxs_ptp_gettime,
+	.settime = mxs_ptp_settime,
+	.enable = mxs_ptp_enable,
+};
+
+int mxs_ptp_init(struct fec_enet_private *fep)
+{
+	struct ptp_clock *clk;
+	u32 atime_corr, atime_norm_inc, atime_corr_inc;
+
+	fep->enet_clk = clk_get(&fep->pdev->dev, "enet_clk");
+
+	mxs_ptp_registers_set(fep);
+
+	calc_atime(clk_get_rate(fep->enet_clk) + fep->ptp_rate, &atime_corr, &atime_norm_inc, &atime_corr_inc);
+	writel(BF_ATIME_INC_CORR(atime_corr) | BF_ATIME_INC(atime_norm_inc), fep->hwp + ENET_MAC_ATIME_INC);
+
+	writel((1<<BP_EVT_PERIOD_RST) | (1<<BP_EVT_PERIOD_ENA) |
+		       (1<<BP_ENABLE), fep->hwp + ENET_MAC_ATIME_CTRL);
+
+	fep->clock_info = mxs_ptp_info;
+
+	clk = ptp_clock_register(&fep->clock_info);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+	fep->ptp_clock = clk;
+
+	return 0;
+}
+
+
+void mxs_ptp_store(struct fec_enet_private *fep)
+{
+	int i;
+
+	for (i=0; i<PTP_REG_SIZE; i++) {
+		fep->reg_store[i] = readl(fep->hwp + ENET_MAC_ATIME_CTRL + i*4);
+	}
+}
+
+void mxs_ptp_restore(struct fec_enet_private *fep)
+{
+	int i;
+
+	mxs_ptp_registers_set(fep);
+
+	for (i=0; i<PTP_REG_SIZE; i++) {
+		writel(fep->reg_store[i], fep->hwp + ENET_MAC_ATIME_CTRL + i*4);
+	}
+}
diff --git a/drivers/net/ethernet/freescale/fec_ptp.h b/drivers/net/ethernet/freescale/fec_ptp.h
new file mode 100644
index 0000000..0d41a98
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fec_ptp.h
@@ -0,0 +1,15 @@ 
+#ifndef FEC_PTP_H
+#define FEC_PTP_H
+
+#include <linux/ptp_clock_kernel.h>
+
+#define FEC_ENET_TS_AVAIL	((uint)0x00010000) /* Timestamp available */
+#define FEC_ENET_TS_TIMER	((uint)0x00008000) /* Timer interrupt */
+#define FEC_ENET_PTP_IRMASK	(FEC_ENET_TS_AVAIL | FEC_ENET_TS_TIMER)
+
+int mxs_ptp_interrupt(u32 reg, struct fec_enet_private *fep);
+int mxs_ptp_init(struct fec_enet_private *fep);
+void mxs_ptp_store(struct fec_enet_private *fep);
+void mxs_ptp_restore(struct fec_enet_private *fep);
+
+#endif
diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index 68d7201..af7b95d 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -72,4 +72,17 @@  config DP83640_PHY
 	  In order for this to work, your MAC driver must also
 	  implement the skb_tx_timetamp() function.
 
+config PTP_1588_CLOCK_FEC
+	tristate "Freescale FEC as PTP clock"
+	depends on PTP_1588_CLOCK
+	depends on FEC
+	help
+	  This driver adds support for using the FEC as a PTP
+	  clock. This clock is only useful if your PTP programs are
+	  getting hardware time stamps on the PTP Ethernet packets
+	  using the SO_TIMESTAMPING API.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called fec_ptp.
+
 endmenu