From 67d56859d24864af530506c76523f0fc3c5cb502 Mon Sep 17 00:00:00 2001 From: Alison Wang Date: Thu, 4 Aug 2011 09:59:44 +0800 Subject: [PATCH 20/52] Add dual FEC 1588 timer support Add Modelo dual FEC 1588 function with IXXXAT statck. Signed-off-by: Alison Wang --- drivers/net/Kconfig | 6 + drivers/net/Makefile | 3 + drivers/net/fec.c | 153 ++++++++++++- drivers/net/fec.h | 25 ++ drivers/net/fec_1588.c | 626 ++++++++++++++++++++++++++++++++++++++++++++++++ drivers/net/fec_1588.h | 195 +++++++++++++++ 6 files changed, 1006 insertions(+), 2 deletions(-) create mode 100644 drivers/net/fec_1588.c create mode 100644 drivers/net/fec_1588.h --- a/drivers/net/Kconfig +++ b/drivers/net/Kconfig @@ -1958,6 +1958,12 @@ config FEC2 Say Y here if you want to use the second built-in 10/100 Fast ethernet controller on some Motorola ColdFire processors. +config FEC_1588 + bool "Enable 1588 interface(on some ColdFire designs)" + depends on M5441X && FEC + help + Say Y here if 1588 function is enabled. + config FEC_548x tristate "MCF547x/MCF548x Fast Ethernet Controller support" depends on M547X_8X --- a/drivers/net/Makefile +++ b/drivers/net/Makefile @@ -123,6 +123,9 @@ obj-$(CONFIG_PCMCIA_PCNET) += 8390.o obj-$(CONFIG_HP100) += hp100.o obj-$(CONFIG_SMC9194) += smc9194.o obj-$(CONFIG_FEC) += fec.o +ifeq ($(CONFIG_FEC_1588), y) +obj-$(CONFIG_FEC) += fec_1588.o +endif obj-$(CONFIG_FEC_548x) += fec_m547x.o obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx.o ifeq ($(CONFIG_FEC_MPC52xx_MDIO),y) --- a/drivers/net/fec.c +++ b/drivers/net/fec.c @@ -53,6 +53,7 @@ #endif #include "fec.h" +#include "fec_1588.h" #if defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28) #define FEC_ALIGNMENT 0xf @@ -135,8 +136,15 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet #define FEC_ENET_RXB ((uint)0x01000000) /* A buffer was received */ #define FEC_ENET_MII ((uint)0x00800000) /* MII interrupt */ #define FEC_ENET_EBERR ((uint)0x00400000) /* SDMA bus error */ +#define FEC_ENET_TS_AVAIL ((uint)0x00010000) +#define FEC_ENET_TS_TIMER ((uint)0x00008000) +#if defined(CONFIG_FEC_1588) +#define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII | \ + FEC_ENET_TS_AVAIL | FEC_ENET_TS_TIMER) +#else #define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII) +#endif /* The FEC stores dest/src/type, data, and checksum for receive packets. */ @@ -209,6 +217,10 @@ struct fec_enet_private { int link; int full_duplex; struct completion mdio_done; +#ifdef CONFIG_FEC_1588 + struct fec_ptp_private *ptp_priv; + uint ptimer_present; +#endif }; static irqreturn_t fec_enet_interrupt(int irq, void * dev_id); @@ -252,6 +264,9 @@ fec_enet_start_xmit(struct sk_buff *skb, struct bufdesc *bdp; void *bufaddr; unsigned short status; +#ifdef CONFIG_FEC_1588 + unsigned long estatus; +#endif unsigned long flags; if (!fep->link) { @@ -293,6 +308,17 @@ fec_enet_start_xmit(struct sk_buff *skb, bufaddr = fep->tx_bounce[index]; } +#ifdef CONFIG_FEC_1588 + if (fep->ptimer_present) { + if (fec_ptp_do_txstamp(skb)) + estatus = BD_ENET_TX_TS; + else + estatus = 0; + + bdp->cbd_esc = (estatus | BD_ENET_TX_INT); + bdp->cbd_bdu = 0; + } +#endif /* * Some design made an incorrect assumption on endian mode of * the system that it's running on. As the result, driver has to @@ -357,6 +383,9 @@ fec_enet_interrupt(int irq, void * dev_i { struct net_device *dev = dev_id; struct fec_enet_private *fep = netdev_priv(dev); +#ifdef CONFIG_FEC_1588 + struct fec_ptp_private *fpp = fep->ptp_priv; +#endif uint int_events; irqreturn_t ret = IRQ_NONE; @@ -364,6 +393,10 @@ fec_enet_interrupt(int irq, void * dev_i int_events = readl(fep->hwp + FEC_IEVENT); writel(int_events, fep->hwp + FEC_IEVENT); +#ifdef CONFIG_FEC_1588 + if (__raw_readb(MCF_DTIM1_DTER) & MCF_DTIM_DTER_REF) + __raw_writeb(MCF_DTIM_DTER_REF, MCF_DTIM1_DTER); +#endif if (int_events & FEC_ENET_RXF) { ret = IRQ_HANDLED; fec_enet_rx(dev); @@ -378,6 +411,19 @@ fec_enet_interrupt(int irq, void * dev_i fec_enet_tx(dev); } +#ifdef CONFIG_FEC_1588 + if (int_events & FEC_ENET_TS_AVAIL) { + ret = IRQ_HANDLED; + fec_ptp_store_txstamp(fep->ptp_priv); + } + + if (int_events & FEC_ENET_TS_TIMER) { + ret = IRQ_HANDLED; + if (fep->ptimer_present) + fpp->prtc++; + } +#endif + if (int_events & FEC_ENET_MII) { ret = IRQ_HANDLED; complete(&fep->mdio_done); @@ -394,6 +440,9 @@ fec_enet_tx(struct net_device *dev) struct fec_enet_private *fep; struct bufdesc *bdp; unsigned short status; +#ifdef CONFIG_FEC_1588 + unsigned long estatus; +#endif struct sk_buff *skb; fep = netdev_priv(dev); @@ -437,6 +486,13 @@ fec_enet_tx(struct net_device *dev) if (status & BD_ENET_TX_DEF) dev->stats.collisions++; +#if defined(CONFIG_FEC_1588) + if (fep->ptimer_present) { + estatus = bdp->cbd_esc; + if (estatus & BD_ENET_TX_TS) + fec_ptp_store_txstamp(fep->ptp_priv); + } +#endif /* Free the sk buffer associated with this last transmit */ dev_kfree_skb_any(skb); fep->tx_skbuff[fep->skb_dirty] = NULL; @@ -470,6 +526,9 @@ static void fec_enet_rx(struct net_device *dev) { struct fec_enet_private *fep = netdev_priv(dev); +#ifdef CONFIG_FEC_1588 + struct fec_ptp_private *fpp = fep->ptp_priv; +#endif const struct platform_device_id *id_entry = platform_get_device_id(fep->pdev); struct bufdesc *bdp; @@ -554,6 +613,12 @@ fec_enet_rx(struct net_device *dev) skb_put(skb, pkt_len - 4); /* Make room */ skb_copy_to_linear_data(skb, data, pkt_len - 4); skb->protocol = eth_type_trans(skb, dev); + +#ifdef CONFIG_FEC_1588 + /* 1588 messeage TS handle */ + if (fep->ptimer_present) + fec_ptp_store_rxstamp(fpp, skb, bdp); +#endif netif_rx(skb); } @@ -567,6 +632,11 @@ rx_processing_done: status |= BD_ENET_RX_EMPTY; bdp->cbd_sc = status; +#ifdef CONFIG_FEC_1588 + bdp->cbd_esc = BD_ENET_RX_INT; + bdp->cbd_prot = 0; + bdp->cbd_bdu = 0; +#endif /* Update BD pointer to next entry */ if (status & BD_ENET_RX_WRAP) bdp = fep->rx_bd_base; @@ -669,8 +739,11 @@ static void fec_enet_adjust_link(struct fec_stop(dev); if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) - writel(2, fep->hwp + FEC_ECNTRL); - +#ifdef CONFIG_FEC_1588 + writel(0x00000012, fep->hwp + FEC_ECNTRL); +#else + writel(0x00000002, fep->hwp + FEC_ECNTRL); +#endif status_change = 1; } @@ -983,6 +1056,10 @@ static int fec_enet_alloc_buffers(struct bdp->cbd_bufaddr = dma_map_single(&dev->dev, skb->data, FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE); bdp->cbd_sc = BD_ENET_RX_EMPTY; + +#ifdef CONFIG_FEC_1588 + bdp->cbd_esc = BD_ENET_RX_INT; +#endif bdp++; } @@ -996,6 +1073,9 @@ static int fec_enet_alloc_buffers(struct bdp->cbd_sc = 0; bdp->cbd_bufaddr = 0; +#ifdef CONFIG_FEC_1588 + bdp->cbd_esc = BD_ENET_TX_INT; +#endif bdp++; } @@ -1256,8 +1336,12 @@ fec_restart(struct net_device *dev, int writel(cpu_to_be32(temp_mac[1]), fep->hwp + FEC_ADDR_HIGH); } +#ifdef CONFIG_FEC_1588 + writel(0x7fff8000, fep->hwp + FEC_IEVENT); +#else /* Clear any outstanding interrupt. */ writel(0xffc00000, fep->hwp + FEC_IEVENT); +#endif /* Reset all multicast. */ writel(0, fep->hwp + FEC_GRP_HASH_TABLE_HIGH); @@ -1342,8 +1426,25 @@ fec_restart(struct net_device *dev, int #endif } +#ifdef CONFIG_FEC_1588 + if (fep->ptimer_present) { + int ret; + /* Set Timer count */ + ret = fec_ptp_start(fep->ptp_priv); + if (ret) { + fep->ptimer_present = 0; + writel(2, fep->hwp + FEC_ECNTRL); + } else { + val = readl(fep->hwp + FEC_ECNTRL); + val |= 0x00000012; + writel(val, fep->hwp + FEC_ECNTRL); + } + } else + writel(2, fep->hwp + FEC_ECNTRL); +#else /* And last, enable the transmit and receive processing */ writel(2, fep->hwp + FEC_ECNTRL); +#endif writel(0, fep->hwp + FEC_R_DES_ACTIVE); /* Enable interrupts we wish to service */ @@ -1367,6 +1468,10 @@ fec_stop(struct net_device *dev) writel(1, fep->hwp + FEC_ECNTRL); udelay(10); writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED); +#ifdef CONFIG_FEC_1588 + if (fep->ptimer_present) + fec_ptp_stop(fep->ptp_priv); +#endif writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK); } @@ -1428,6 +1533,24 @@ fec_probe(struct platform_device *pdev) } } +#ifdef CONFIG_FEC_1588 + i = (pdev->id) ? (64 + 64 + 64 + 7) : (64 + 64 + 64); + if (request_irq(i + 48, fec_enet_interrupt, IRQF_DISABLED, + "1588 TS AVAIL", ndev) != 0) + printk(KERN_ERR "FEC: Could not alloc FEC %x 1588 TS AVAIL " + "IRQ(%d)!\n", pdev->id, i + 48); + + if (pdev->id == 0) { + printk("setup TS timer interrupt through DMA timer1\n"); + __raw_writew(MCF_DTIM_DTMR_RST_RST, MCF_DTIM1_DTMR); + + if (request_irq(64 + 33, fec_enet_interrupt, IRQF_DISABLED, + "1588 TS TIMER", ndev) != 0) + printk(KERN_ERR "FEC: Could not alloc FEC %x 1588 TS" + " TIMER IRQ(%d)!\n", pdev->id, 64 + 33); + } +#endif + fep->clk = clk_get(&pdev->dev, "fec_clk"); if (IS_ERR(fep->clk)) { ret = PTR_ERR(fep->clk); @@ -1443,6 +1566,20 @@ fec_probe(struct platform_device *pdev) if (ret) goto failed_mii_init; +#ifdef CONFIG_FEC_1588 + fep->ptp_priv = kzalloc(sizeof(struct fec_ptp_private), + GFP_KERNEL); + if (fep->ptp_priv) { + fep->ptp_priv->hwp = fep->hwp; + ret = fec_ptp_init(fep->ptp_priv, pdev->id); + if (ret) + printk(KERN_ERR "IEEE1588: ptp-timer init failed\n"); + else + fep->ptimer_present = 1; + } else + printk(KERN_ERR "IEEE1588: failed to malloc memory\n"); +#endif + /* Carrier starts down, phylib will bring it up */ netif_carrier_off(ndev); @@ -1454,6 +1591,12 @@ fec_probe(struct platform_device *pdev) failed_register: fec_enet_mii_remove(fep); +#ifdef CONFIG_FEC_1588 + if (fep->ptimer_present) + fec_ptp_cleanup(fep->ptp_priv); + + kfree(fep->ptp_priv); +#endif failed_mii_init: failed_init: clk_disable(fep->clk); @@ -1485,6 +1628,12 @@ fec_drv_remove(struct platform_device *p clk_disable(fep->clk); clk_put(fep->clk); iounmap((void __iomem *)ndev->base_addr); +#ifdef CONFIG_FEC_1588 + if (fep->ptimer_present) + fec_ptp_cleanup(fep->ptp_priv); + + kfree(fep->ptp_priv); +#endif unregister_netdev(ndev); free_netdev(ndev); return 0; --- a/drivers/net/fec.h +++ b/drivers/net/fec.h @@ -50,6 +50,16 @@ #define FEC_MIIGSK_CFGR 0x300 /* MIIGSK Configuration reg */ #define FEC_MIIGSK_ENR 0x308 /* MIIGSK Enable reg */ +#if defined(CONFIG_FEC_1588) +#define FEC_ATIME_CTRL 0x400 +#define FEC_ATIME 0x404 +#define FEC_ATIME_EVT_OFFSET 0x408 +#define FEC_ATIME_EVT_PERIOD 0x40c +#define FEC_ATIME_CORR 0x410 +#define FEC_ATIME_INC 0x414 +#define FEC_TS_TIMESTAMP 0x418 +#endif + #else #define FEC_ECNTRL 0x000 /* Ethernet control reg */ @@ -78,6 +88,9 @@ #endif /* CONFIG_M5272 */ +#if defined(CONFIG_FEC_1588) +#define FEC_ENHANCED_MODE 1 +#endif /* * Define the buffer descriptor structure. @@ -93,6 +106,14 @@ struct bufdesc { unsigned short cbd_sc; /* Control and status info */ unsigned short cbd_datlen; /* Data length */ unsigned long cbd_bufaddr; /* Buffer address */ + +#ifdef FEC_ENHANCED_MODE + unsigned long cbd_esc; + unsigned long cbd_prot; + unsigned long cbd_bdu; + unsigned long ts; + unsigned short res0[4]; +#endif }; #endif @@ -128,6 +149,7 @@ struct bufdesc { #define BD_ENET_RX_OV ((ushort)0x0002) #define BD_ENET_RX_CL ((ushort)0x0001) #define BD_ENET_RX_STATS ((ushort)0x013f) /* All status bits */ +#define BD_ENET_RX_INT 0x00800000 /* Buffer descriptor control/status used by Ethernet transmit. */ @@ -146,6 +168,9 @@ struct bufdesc { #define BD_ENET_TX_CSL ((ushort)0x0001) #define BD_ENET_TX_STATS ((ushort)0x03ff) /* All status bits */ +#define BD_ENET_TX_TS 0x20000000 +#define BD_ENET_TX_INT 0x40000000 +#define BD_ENET_TX_BDU 0x80000000 /****************************************************************************/ #endif /* FEC_H */ --- /dev/null +++ b/drivers/net/fec_1588.c @@ -0,0 +1,626 @@ +/* + * drivers/net/fec_1588.c + * + * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved. + * Copyright (C) 2009 IXXAT Automation, GmbH + * + * FEC 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. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "fec_1588.h" + +static DECLARE_WAIT_QUEUE_HEAD(ptp_rx_ts_wait); +#define PTP_GET_RX_TIMEOUT (HZ/10) +#define COLDFIRE_DTIM1_INT (64+32+1) + +static struct fec_ptp_private *ptp_private[2]; + +static void init_DTIM1_for_1588(struct fec_ptp_private *priv) +{ + printk(KERN_INFO "Initializing DTIM1 for 1588 TS timer\n"); + + __raw_writew(MCF_DTIM_DTMR_RST_RST, MCF_DTIM1_DTMR); + + /*Enable 1588*/ + + __raw_writeb(MCF_DTIM_DTXMR_1588EN, MCF_DTIM1_DTXMR); + + /*Compare to the 1588 timerbase*/ + __raw_writel(FEC_T_PERIOD_ONE_SEC - FEC_T_INC_40MHZ, MCF_DTIM1_DTRR); + + __raw_writeb(MCF_DTIM_DTER_REF, MCF_DTIM1_DTER); + + MCF_GPIO_PAR_TIMER = (MCF_GPIO_PAR_TIMER & MCF_GPIO_PAR_TIMER_T1IN_MASK) + | MCF_GPIO_PAR_TIMER_T1IN_T1OUT; +} + +static void start_DTIM1(void) +{ + __raw_writew(MCF_DTIM_DTMR_RST_EN | MCF_DTIM_DTMR_ORRI | + MCF_DTIM_DTMR_OM, MCF_DTIM1_DTMR); +} + +static void stop_DTIM1(void) +{ + __raw_writew(MCF_DTIM_DTMR_RST_RST, MCF_DTIM1_DTMR); +} + +/* Alloc the ring resource */ +static int fec_ptp_init_circ(struct circ_buf *ptp_buf) +{ + ptp_buf->buf = vmalloc(DEFAULT_PTP_RX_BUF_SZ * + sizeof(struct fec_ptp_data_t)); + + if (!ptp_buf->buf) + return 1; + ptp_buf->head = 0; + ptp_buf->tail = 0; + + return 0; +} + +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) +{ + return (buf->head == buf->tail); +} + +static int fec_ptp_nelems(struct circ_buf *buf) +{ + const int front = buf->head; + const int end = buf->tail; + const int size = DEFAULT_PTP_RX_BUF_SZ; + int n_items; + + if (end > front) + n_items = end - front; + else if (end < front) + n_items = size - (front - end); + else + n_items = 0; + + return n_items; +} + +static int fec_ptp_is_full(struct circ_buf *buf) +{ + if (fec_ptp_nelems(buf) == + (DEFAULT_PTP_RX_BUF_SZ - 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) +{ + struct fec_ptp_data_t *tmp; + + if (fec_ptp_is_full(ptp_buf)) + return 1; + + spin_lock(&priv->ptp_lock); + tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + ptp_buf->tail; + + tmp->key = data->key; + tmp->ts_time.sec = data->ts_time.sec; + tmp->ts_time.nsec = data->ts_time.nsec; + + ptp_buf->tail = fec_ptp_calc_index(DEFAULT_PTP_RX_BUF_SZ, + ptp_buf->tail, 1); + spin_unlock(&priv->ptp_lock); + + return 0; +} + +static int fec_ptp_find_and_remove(struct circ_buf *ptp_buf, + int key, + struct fec_ptp_data_t *data, + struct fec_ptp_private *priv) +{ + int i; + int size = DEFAULT_PTP_RX_BUF_SZ; + int end = ptp_buf->tail; + unsigned long flags; + struct fec_ptp_data_t *tmp; + + if (fec_ptp_is_empty(ptp_buf)) + return 1; + + i = ptp_buf->head; + while (i != end) { + tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + i; + if (tmp->key == key) + break; + i = fec_ptp_calc_index(size, i, 1); + } + + spin_lock_irqsave(&priv->ptp_lock, flags); + if (i == end) { + ptp_buf->head = end; + spin_unlock_irqrestore(&priv->ptp_lock, flags); + 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); + + return 0; +} + +/* 1588 Module intialization */ +int fec_ptp_start(struct fec_ptp_private *priv) +{ + struct fec_ptp_private *fpp = priv; + + MCF_CCM_MISCCR3 = 0x0000; + + init_DTIM1_for_1588(priv); + + /* Select 1588 Timer source and enable module for starting Tmr Clock */ + fec_writel(FEC_T_CTRL_RESTART, fpp->hwp + FEC_ATIME_CTRL); + fec_writel(FEC_T_INC_40MHZ << FEC_T_INC_OFFSET, + fpp->hwp + FEC_ATIME_INC); + fec_writel(FEC_T_PERIOD_ONE_SEC, fpp->hwp + FEC_ATIME_EVT_PERIOD); + /* start counter */ + fec_writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE | + FEC_T_CTRL_PINPER, fpp->hwp + FEC_ATIME_CTRL); + + start_DTIM1(); + + return 0; +} + +/* Cleanup routine for 1588 module. + * When PTP is disabled this routing is called */ +void fec_ptp_stop(struct fec_ptp_private *priv) +{ + struct fec_ptp_private *fpp = priv; + + fec_writel(0, fpp->hwp + FEC_ATIME_CTRL); + fec_writel(FEC_T_CTRL_RESTART, fpp->hwp + FEC_ATIME_CTRL); + stop_DTIM1(); +} + +static void fec_get_curr_cnt(struct fec_ptp_private *priv, + struct ptp_rtc_time *curr_time) +{ + u32 tempval; + + fec_writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL); + fec_writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL); + curr_time->rtc_time.nsec = fec_readl(priv->hwp + FEC_ATIME); + curr_time->rtc_time.sec = priv->prtc; + + fec_writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL); + tempval = fec_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 */ +static void fec_set_1588cnt(struct fec_ptp_private *priv, + struct ptp_rtc_time *fec_time) +{ + u32 tempval; + unsigned long flags; + + spin_lock_irqsave(&priv->cnt_lock, flags); + + priv->prtc = fec_time->rtc_time.sec; + + tempval = fec_time->rtc_time.nsec; + fec_writel(tempval, priv->hwp + FEC_ATIME); + spin_unlock_irqrestore(&priv->cnt_lock, flags); +} + +/* 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->source) == 319) + return 1; + } + + return 0; +} + +void fec_ptp_store_txstamp(struct fec_ptp_private *priv) +{ + struct fec_ptp_private *fpp = priv; + unsigned int reg; + + reg = fec_readl(fpp->hwp + FEC_TS_TIMESTAMP); + fpp->txstamp.nsec = reg; + fpp->txstamp.sec = fpp->prtc; +} + +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_private *fpp = priv; + struct iphdr *iph; + struct udphdr *udph; + + /* 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->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 = ntohs(seq_id); + 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); + break; + + case PTP_MSG_DEL_REQ: + fec_ptp_insert(&(priv->rx_time_del_req), &tmp_rx_time, priv); + 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); + break; + case PTP_MSG_P_DEL_RESP: + fec_ptp_insert(&(priv->rx_time_pdel_resp), + &tmp_rx_time, priv); + break; + default: + break; + } + break; + default: + break; + } + + wake_up_interruptible(&ptp_rx_ts_wait); +} + +static void fec_get_tx_timestamp(struct fec_ptp_private *priv, + struct ptp_time *tx_time) +{ + tx_time->sec = priv->txstamp.sec; + tx_time->nsec = priv->txstamp.nsec; +} + +static uint8_t fec_get_rx_time(struct fec_ptp_private *priv, + struct ptp_ts_data *pts, + struct ptp_time *rx_time) +{ + struct fec_ptp_data_t tmp; + int key, flag; + u8 mode; + + key = pts->seq_id; + mode = pts->message_type; + switch (mode) { + case PTP_MSG_SYNC: + flag = fec_ptp_find_and_remove(&(priv->rx_time_sync), + key, &tmp, priv); + break; + case PTP_MSG_DEL_REQ: + flag = fec_ptp_find_and_remove(&(priv->rx_time_del_req), + key, &tmp, priv); + break; + + case PTP_MSG_P_DEL_REQ: + flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_req), + key, &tmp, priv); + break; + case PTP_MSG_P_DEL_RESP: + flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_resp), + key, &tmp, priv); + break; + + default: + flag = 1; + printk(KERN_ERR "ERROR\n"); + break; + } + + 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); + + switch (mode) { + case PTP_MSG_SYNC: + flag = fec_ptp_find_and_remove(&(priv->rx_time_sync), + key, &tmp, priv); + break; + case PTP_MSG_DEL_REQ: + flag = fec_ptp_find_and_remove( + &(priv->rx_time_del_req), key, &tmp, priv); + break; + case PTP_MSG_P_DEL_REQ: + flag = fec_ptp_find_and_remove( + &(priv->rx_time_pdel_req), key, &tmp, priv); + break; + case PTP_MSG_P_DEL_RESP: + flag = fec_ptp_find_and_remove( + &(priv->rx_time_pdel_resp), key, &tmp, priv); + break; + } + + if (flag == 0) { + rx_time->sec = tmp.ts_time.sec; + rx_time->nsec = tmp.ts_time.nsec; + return 0; + } + + return -1; + } +} + +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 = fec_readl(fpp->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK; + tmp |= corr_ns << FEC_T_INC_CORR_OFFSET; + fec_writel(tmp, fpp->hwp + FEC_ATIME_INC); + + fec_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_unlocked_ioctl( + struct file *file, + unsigned int cmd, + unsigned long arg) +{ + 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; + struct inode *inode = file->f_mapping->host; + unsigned int minor = MINOR(inode->i_rdev); + long retval = 0; + + priv = (struct fec_ptp_private *) ptp_private[minor]; + switch (cmd) { + case PTP_GET_RX_TIMESTAMP: + p_ts = (struct ptp_ts_data *)arg; + retval = fec_get_rx_time(priv, p_ts, &rx_time); + if (retval == 0) + copy_to_user((void __user *)(&(p_ts->ts)), &rx_time, + sizeof(rx_time)); + break; + case PTP_GET_TX_TIMESTAMP: + p_ts = (struct ptp_ts_data *)arg; + fec_get_tx_timestamp(priv, &tx_time); + copy_to_user((void __user *)(&(p_ts->ts)), &tx_time, + sizeof(tx_time)); + break; + case PTP_GET_CURRENT_TIME: + fec_get_curr_cnt(priv, &curr_time); + copy_to_user((void __user *)arg, &curr_time, sizeof(curr_time)); + break; + case PTP_SET_RTC_TIME: + cnt = (struct ptp_rtc_time *)arg; + fec_set_1588cnt(priv, cnt); + break; + case PTP_FLUSH_TIMESTAMP: + /* reset sync buffer */ + priv->rx_time_sync.head = 0; + priv->rx_time_sync.tail = 0; + /* reset delay_req buffer */ + priv->rx_time_del_req.head = 0; + priv->rx_time_del_req.tail = 0; + /* reset pdelay_req buffer */ + priv->rx_time_pdel_req.head = 0; + priv->rx_time_pdel_req.tail = 0; + /* reset pdelay_resp buffer */ + priv->rx_time_pdel_resp.head = 0; + priv->rx_time_pdel_resp.tail = 0; + break; + case PTP_SET_COMPENSATION: + p_comp = (struct ptp_set_comp *)arg; + fec_set_drift(priv, p_comp); + break; + case PTP_GET_ORIG_COMP: + ((struct ptp_get_comp *)arg)->dw_origcomp = FEC_PTP_ORIG_COMP; + break; + default: + return -EINVAL; + } + return retval; +} + +static const struct file_operations ptp_fops = { + .owner = THIS_MODULE, + .llseek = NULL, + .read = NULL, + .write = NULL, + .unlocked_ioctl = ptp_unlocked_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 deivce as char\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)); + fec_ptp_init_circ(&(priv->rx_time_del_req)); + fec_ptp_init_circ(&(priv->rx_time_pdel_req)); + fec_ptp_init_circ(&(priv->rx_time_pdel_resp)); + + spin_lock_init(&priv->ptp_lock); + spin_lock_init(&priv->cnt_lock); + ptp_private[id] = priv; + 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); + + ptp_free(); +} +EXPORT_SYMBOL(fec_ptp_cleanup); --- /dev/null +++ b/drivers/net/fec_1588.h @@ -0,0 +1,195 @@ +/* + * drivers/net/fec_1588.h + * + * Copyright (C) 2010-2011 Freescale Semiconductor, Inc. All Rights Reserved. + * + * 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. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + * + */ + +#ifndef FEC_1588_H +#define FEC_1588_H + +#include +#include "fec.h" + +#define fec_readl(addr) \ + ({ unsigned int __v = (*(volatile unsigned int *) (addr)); __v; }) + +#define fec_writel(b, addr) (void)((*(volatile unsigned int *) (addr)) = (b)) + +#define FALSE 0 +#define TRUE 1 + +/* FEC 1588 register bits */ +#define FEC_T_CTRL_CAPTURE 0x00000800 +#define FEC_T_CTRL_RESTART 0x00000200 +#define FEC_T_CTRL_PERIOD_RST 0x00000010 +#define FEC_T_CTRL_PINPER 0x00000080 +#define FEC_T_CTRL_ENABLE 0x00000001 + +#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 8 +#define FEC_ATIME_40MHZ 125000000 + +#define FEC_T_PERIOD_ONE_SEC 0x3B9ACA00 + +/* 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 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 0x0 +#define FEC_PTP_UDP_OFFS 0x14 +#define FEC_PTP_MSG_TYPE_OFFS 0x1C +#define FEC_PTP_SEQ_ID_OFFS 0x3A +#define FEC_PTP_COR_NS 0x24 +#define FEC_PTP_CTRL_OFFS 0x3C +#define FEC_PACKET_TYPE_UDP 0x11 + +#define FEC_PTP_ORIG_COMP 0x15555 + +/* PTP standard time representation structure */ +struct ptp_time { + u64 sec; /* seconds */ + u32 nsec; /* nanoseconds */ +}; + +/* Structure for PTP Time Stamp */ +struct fec_ptp_data_t { + int key; + struct ptp_time ts_time; +}; + +/* 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; + /* PTP timestamp */ + struct ptp_time ts; +}; + +/* interface for PTP driver command SET_RTC_TIME/GET_CURRENT_TIME */ +struct ptp_rtc_time { + struct ptp_time rtc_time; +}; + +/* interface for PTP driver command SET_COMPENSATION */ +struct ptp_set_comp { + u32 drift; + u32 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 + +struct fec_ptp_private { + void __iomem *hwp; + + 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; + spinlock_t ptp_lock; + spinlock_t cnt_lock; + + u64 prtc; + struct ptp_time txstamp; +}; + +#ifdef CONFIG_FEC_1588 +extern int fec_ptp_init(struct fec_ptp_private *priv, int id); +extern void fec_ptp_cleanup(struct fec_ptp_private *priv); +extern int fec_ptp_start(struct fec_ptp_private *priv); +extern void fec_ptp_stop(struct fec_ptp_private *priv); +extern int fec_ptp_do_txstamp(struct sk_buff *skb); +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); +#else +static inline int fec_ptp_init(struct fec_ptp_private *priv, int id) +{ + return 1; +} +static inline void fec_ptp_cleanup(struct fec_ptp_private *priv) { } +static inline int fec_ptp_start(struct fec_ptp_private *priv) +{ + return 1; +} +static inline void fec_ptp_stop(struct fec_ptp_private *priv) {} +static inline int fec_ptp_do_txstamp(struct sk_buff *skb) +{ + return 0; +} +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) {} +#endif /* 1588 */ + +#endif