From 8eeda5ac98beb9e43f5a76ae77af3251c0b97ec8 Mon Sep 17 00:00:00 2001 From: Luwei Zhou Date: Thu, 15 Aug 2013 13:45:23 +0800 Subject: [PATCH] ENGR00275371 net: fec: PTP: Add ptp support for IXXAT stack These patch add ptp support for IXXAT stack. Cherry picked from commit 1c8839574ac87826f53b96f987975a9bb1d72915. Signed-off-by: Luwei Zhou --- drivers/net/ethernet/freescale/fec.h | 112 +++- drivers/net/ethernet/freescale/fec_main.c | 55 +- drivers/net/ethernet/freescale/fec_ptp.c | 641 ++++++++++++++++++++-- 3 files changed, 753 insertions(+), 55 deletions(-) diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h index ea5c60d84889..888ff237338d 100644 --- a/drivers/net/ethernet/freescale/fec.h +++ b/drivers/net/ethernet/freescale/fec.h @@ -16,6 +16,9 @@ #include #include #include +#include +#include +#include #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \ defined(CONFIG_M520x) || defined(CONFIG_M532x) || \ @@ -256,6 +259,98 @@ struct bufdesc_ex { #define FLAG_RX_CSUM_ENABLED (BD_ENET_RX_ICE | BD_ENET_RX_PCR) #define FLAG_RX_CSUM_ERROR (BD_ENET_RX_ICE | BD_ENET_RX_PCR) +#define FALSE 0 +#define TRUE 1 + +/* IEEE 1588 definition */ +#define FEC_T_PERIOD_ONE_SEC 0x3B9ACA00 + +#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{ + u64 sec; /* seconds */ + u32 nsec; /* nanoseconds */ +}; + +/* 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 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; +}; + +/* interface for PTP driver command SET_COMPENSATION */ +struct ptp_set_comp { + u32 drift; + bool o_ops; + u32 freq_compensation; +}; + +struct ptp_time_correct { + u32 corr_period; + u32 corr_inc; +}; + struct fec_enet_delayed_work { struct delayed_work delay_work; bool timeout; @@ -312,13 +407,19 @@ struct fec_enet_private { int speed; struct completion mdio_done; int irq[FEC_IRQ_NUM]; + /* HW timestamping over ioctl enabled flag */ + int hwts_tx_en_ioctl; + int hwts_rx_en_ioctl; + struct fec_ptp_circular tx_timestamps; + struct fec_ptp_circular rx_timestamps; + u64 prtc; int bufdesc_ex; int pause_flag; struct napi_struct napi; int csum_flags; - int phy_reset_gpio; + int phy_reset_gpio; struct ptp_clock *ptp_clock; struct ptp_clock_info ptp_caps; @@ -339,6 +440,15 @@ struct fec_enet_private { void fec_ptp_init(struct platform_device *pdev); void fec_ptp_start_cyclecounter(struct net_device *ndev); int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd); +void fec_ptp_cleanup(struct fec_enet_private *priv); +void fec_ptp_stop(struct net_device *ndev); +int fec_ptp_do_txstamp(struct sk_buff *skb); +void fec_ptp_store_txstamp(struct fec_enet_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp); +void fec_ptp_store_rxstamp(struct fec_enet_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp); /****************************************************************************/ #endif /* FEC_H */ diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c index 73d2c270917e..b0cad4e20cac 100644 --- a/drivers/net/ethernet/freescale/fec_main.c +++ b/drivers/net/ethernet/freescale/fec_main.c @@ -197,6 +197,8 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address"); #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) #define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII) #define FEC_RX_DISABLED_IMASK (FEC_DEFAULT_IMASK & (~FEC_ENET_RXF)) @@ -369,7 +371,8 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev) struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp; ebdp->cbd_bdu = 0; if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP && - fep->hwts_tx_en)) { + fep->hwts_tx_en) || unlikely(fep->hwts_tx_en_ioctl && + fec_ptp_do_txstamp(skb))) { ebdp->cbd_esc = (BD_ENET_TX_TS | BD_ENET_TX_INT); skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; } else { @@ -645,8 +648,13 @@ fec_restart(struct net_device *ndev, int duplex) writel(ecntl, fep->hwp + FEC_ECNTRL); writel(0, fep->hwp + FEC_R_DES_ACTIVE); - if (fep->bufdesc_ex) + if (fep->bufdesc_ex) { fec_ptp_start_cyclecounter(ndev); + /* Enable interrupts we wish to service */ + writel(FEC_DEFAULT_IMASK | FEC_ENET_TS_AVAIL | + FEC_ENET_TS_TIMER, fep->hwp + FEC_IMASK); + } else + writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK); /* Enable interrupts we wish to service */ writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK); @@ -681,6 +689,11 @@ fec_stop(struct net_device *ndev) writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED); writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK); + if (fep->bufdesc_ex && (fep->hwts_tx_en_ioctl || + fep->hwts_rx_en_ioctl || fep->hwts_tx_en || + fep->hwts_rx_en)) + fec_ptp_stop(ndev); + /* We have to keep ENET enabled to have MII interrupt stay working */ if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) { writel(2, fep->hwp + FEC_ECNTRL); @@ -775,8 +788,8 @@ fec_enet_tx(struct net_device *ndev) ndev->stats.tx_bytes += bdp->cbd_datlen; } - if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS) && - fep->bufdesc_ex) { + if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS) && + fep->hwts_tx_en) && fep->bufdesc_ex) { struct skb_shared_hwtstamps shhwtstamps; unsigned long flags; struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp; @@ -787,7 +800,8 @@ fec_enet_tx(struct net_device *ndev) timecounter_cyc2time(&fep->tc, ebdp->ts)); spin_unlock_irqrestore(&fep->tmreg_lock, flags); skb_tstamp_tx(skb, &shhwtstamps); - } + } else if (unlikely(fep->hwts_tx_en_ioctl) && fep->bufdesc_ex) + fec_ptp_store_txstamp(fep, skb, bdp); if (status & BD_ENET_TX_READY) netdev_err(ndev, "HEY! Enet xmit interrupt and TX_READY\n"); @@ -944,8 +958,6 @@ fec_enet_rx(struct net_device *ndev, int budget) data + payload_offset, pkt_len - 4 - (2 * ETH_ALEN)); - skb->protocol = eth_type_trans(skb, ndev); - /* Get receive timestamp from the skb */ if (fep->hwts_rx_en && fep->bufdesc_ex) { struct skb_shared_hwtstamps *shhwtstamps = @@ -958,8 +970,11 @@ fec_enet_rx(struct net_device *ndev, int budget) shhwtstamps->hwtstamp = ns_to_ktime( timecounter_cyc2time(&fep->tc, ebdp->ts)); spin_unlock_irqrestore(&fep->tmreg_lock, flags); - } + } else if (unlikely(fep->hwts_rx_en_ioctl) && + fep->bufdesc_ex) + fec_ptp_store_rxstamp(fep, skb, bdp); + skb->protocol = eth_type_trans(skb, ndev); if (fep->bufdesc_ex && (fep->csum_flags & FLAG_RX_CSUM_ENABLED)) { if (!(ebdp->cbd_esc & FLAG_RX_CSUM_ERROR)) { @@ -1037,6 +1052,12 @@ fec_enet_interrupt(int irq, void *dev_id) } } + if ((int_events & FEC_ENET_TS_TIMER) && fep->bufdesc_ex) { + ret = IRQ_HANDLED; + if (fep->hwts_tx_en_ioctl || fep->hwts_rx_en_ioctl) + fep->prtc++; + } + if (int_events & FEC_ENET_MII) { ret = IRQ_HANDLED; complete(&fep->mdio_done); @@ -1656,8 +1677,11 @@ static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd) if (!phydev) return -ENODEV; - if (cmd == SIOCSHWTSTAMP && fep->bufdesc_ex) + if (((cmd == SIOCSHWTSTAMP) || ((cmd >= PTP_ENBL_TXTS_IOCTL) && + (cmd <= PTP_FLUSH_TIMESTAMP))) && fep->bufdesc_ex) return fec_ptp_ioctl(ndev, rq, cmd); + else if (fep->bufdesc_ex) + return -ENODEV; return phy_mii_ioctl(phydev, rq, cmd); } @@ -2255,7 +2279,10 @@ fec_drv_remove(struct platform_device *pdev) cancel_delayed_work_sync(&(fep->delay_work.delay_work)); unregister_netdev(ndev); fec_enet_mii_remove(fep); - del_timer_sync(&fep->time_keep); + if (fep->bufdesc_ex) { + fec_ptp_cleanup(fep); + del_timer_sync(&fep->time_keep); + } for (i = 0; i < FEC_IRQ_NUM; i++) { int irq = platform_get_irq(pdev, i); if (irq > 0) @@ -2263,9 +2290,11 @@ fec_drv_remove(struct platform_device *pdev) } if (fep->reg_phy) regulator_disable(fep->reg_phy); - clk_disable_unprepare(fep->clk_ptp); - if (fep->ptp_clock) - ptp_clock_unregister(fep->ptp_clock); + if (fep->bufdesc_ex) { + clk_disable_unprepare(fep->clk_ptp); + if (fep->ptp_clock) + ptp_clock_unregister(fep->ptp_clock); + } clk_disable_unprepare(fep->clk_enet_out); clk_disable_unprepare(fep->clk_ahb); clk_disable_unprepare(fep->clk_ipg); diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c index 5007e4f9fff9..06c679ebcd9c 100644 --- a/drivers/net/ethernet/freescale/fec_ptp.c +++ b/drivers/net/ethernet/freescale/fec_ptp.c @@ -1,7 +1,7 @@ /* * Fast Ethernet Controller (ENET) PTP driver for MX6x. * - * Copyright (C) 2012 Freescale Semiconductor, Inc. + * Copyright (C) 2012-2013 Freescale Semiconductor, Inc. * * This program is free software; you can redistribute it and/or modify it * under the terms and conditions of the GNU General Public License, @@ -37,6 +37,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -49,6 +52,7 @@ #include "fec.h" + /* FEC 1588 register bits */ #define FEC_T_CTRL_SLAVE 0x00002000 #define FEC_T_CTRL_CAPTURE 0x00000800 @@ -71,6 +75,446 @@ #define FEC_TS_TIMESTAMP 0x418 #define FEC_CC_MULT (1 << 31) + +/* Alloc the ring resource */ +static int fec_ptp_init_circ(struct fec_ptp_circular *buf, int size) +{ + buf->data_buf = (struct fec_ptp_ts_data *) + vmalloc(size * sizeof(struct fec_ptp_ts_data)); + + if (!buf->data_buf) + return 1; + buf->front = 0; + buf->end = 0; + buf->size = size; + 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 fec_ptp_circular *buf) +{ + return (buf->front == buf->end); +} + +static int fec_ptp_nelems(struct fec_ptp_circular *buf) +{ + const int front = buf->front; + const int end = buf->end; + const int size = buf->size; + 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 fec_ptp_circular *buf) +{ + if (fec_ptp_nelems(buf) == (buf->size - 1)) + return 1; + else + return 0; +} + +static int fec_ptp_insert(struct fec_ptp_circular *ptp_buf, + struct fec_ptp_ts_data *data) +{ + struct fec_ptp_ts_data *tmp; + + if (fec_ptp_is_full(ptp_buf)) + ptp_buf->end = fec_ptp_calc_index(ptp_buf->size, + ptp_buf->end, 1); + + 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 fec_ptp_circular *ptp_buf, + struct fec_ptp_ident *ident, struct ptp_time *ts) +{ + int i; + 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->front; + while (i != end) { + 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); + } + + /* not found ? */ + if (i == end) { + /* 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; + } + *ts = (ptp_buf->data_buf + i)->ts; + ptp_buf->front = fec_ptp_calc_index(size, ptp_buf->front, 1); + + return 0; +} + +/* 1588 Module intialization */ +void fec_ptp_start(struct net_device *ndev) +{ + struct fec_enet_private *fep = netdev_priv(ndev); + unsigned long flags; + int inc; + + inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(fep->clk_ptp); + + spin_lock_irqsave(&fep->tmreg_lock, flags); + /* Select 1588 Timer source and enable module for starting Tmr Clock */ + writel(FEC_T_CTRL_RESTART, fep->hwp + FEC_ATIME_CTRL); + writel(inc << FEC_T_INC_OFFSET, + fep->hwp + FEC_ATIME_INC); + writel(FEC_T_PERIOD_ONE_SEC, fep->hwp + FEC_ATIME_EVT_PERIOD); + /* start counter */ + writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE, + fep->hwp + FEC_ATIME_CTRL); + spin_unlock_irqrestore(&fep->tmreg_lock, flags); +} + +/* Cleanup routine for 1588 module. + * When PTP is disabled this routing is called */ +void fec_ptp_stop(struct net_device *ndev) +{ + struct fec_enet_private *priv = netdev_priv(ndev); + + writel(0, priv->hwp + FEC_ATIME_CTRL); + writel(FEC_T_CTRL_RESTART, priv->hwp + FEC_ATIME_CTRL); +} + +static void fec_get_curr_cnt(struct fec_enet_private *priv, + struct ptp_rtc_time *curr_time) +{ + u32 tempval; + + tempval = readl(priv->hwp + FEC_ATIME_CTRL); + tempval |= FEC_T_CTRL_CAPTURE; + + writel(tempval, priv->hwp + FEC_ATIME_CTRL); + curr_time->rtc_time.nsec = readl(priv->hwp + FEC_ATIME); + curr_time->rtc_time.sec = priv->prtc; + + writel(tempval, priv->hwp + FEC_ATIME_CTRL); + 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 */ +static void fec_set_1588cnt(struct fec_enet_private *priv, + struct ptp_rtc_time *fec_time) +{ + u32 tempval; + unsigned long flags; + + spin_lock_irqsave(&priv->tmreg_lock, flags); + priv->prtc = fec_time->rtc_time.sec; + + tempval = fec_time->rtc_time.nsec; + writel(tempval, priv->hwp + FEC_ATIME); + spin_unlock_irqrestore(&priv->tmreg_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 (ntohs(*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 == ntohs(*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) +{ + u8 *ptp_loc; + u16 eth_type; + + ptp_loc = fec_ptp_parse_packet(skb, ð_type); + if (ptp_loc != NULL) + return 1; + + return 0; +} + +void fec_ptp_store_txstamp(struct fec_enet_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp) +{ + struct fec_ptp_ts_data tmp_tx_time; + struct bufdesc_ex *bdp_ex = NULL; + u8 *ptp_loc; + u16 eth_type; + + bdp_ex = container_of(bdp, struct bufdesc_ex, desc); + ptp_loc = fec_ptp_parse_packet(skb, ð_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 ETH_P_IPV6: + tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV6; + break; + case ETH_P_1588: + tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_802_3; + break; + default: + return; + } + 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 = priv->prtc; + tmp_tx_time.ts.nsec = bdp_ex->ts; + /* insert timestamp in circular buffer */ + fec_ptp_insert(&(priv->tx_timestamps), &tmp_tx_time); + } +} + +void fec_ptp_store_rxstamp(struct fec_enet_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp) +{ + struct fec_ptp_ts_data tmp_rx_time; + struct bufdesc_ex *bdp_ex = NULL; + u8 *ptp_loc; + u16 eth_type; + + bdp_ex = container_of(bdp, struct bufdesc_ex, desc); + ptp_loc = fec_ptp_parse_packet(skb, ð_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 ETH_P_IPV6: + tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV6; + break; + case ETH_P_1588: + tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_802_3; + break; + default: + return; + } + 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 = priv->prtc; + tmp_rx_time.ts.nsec = bdp_ex->ts; + + /* insert timestamp in circular buffer */ + fec_ptp_insert(&(priv->rx_timestamps), &tmp_rx_time); + } +} + + +static void fec_handle_ptpdrift(struct fec_enet_private *priv, + struct ptp_set_comp *comp, struct ptp_time_correct *ptc) +{ + u32 ndrift; + u32 i, adj_inc, adj_period; + u32 tmp_current, tmp_winner; + u32 ptp_ts_clk, ptp_inc; + + ptp_ts_clk = clk_get_rate(priv->clk_ptp); + ptp_inc = FEC_T_PERIOD_ONE_SEC / ptp_ts_clk; + + ndrift = comp->drift; + + if (ndrift == 0) { + ptc->corr_inc = 0; + ptc->corr_period = 0; + return; + } else if (ndrift >= ptp_ts_clk) { + ptc->corr_inc = (u32)(ndrift / ptp_ts_clk); + ptc->corr_period = 1; + return; + } else { + tmp_winner = 0xFFFFFFFF; + adj_inc = 1; + + if (ndrift > (ptp_ts_clk / ptp_inc)) { + adj_inc = ptp_inc / FEC_PTP_SPINNER_2; + } else if (ndrift > (ptp_ts_clk / + (ptp_inc * FEC_PTP_SPINNER_4))) { + adj_inc = ptp_inc / FEC_PTP_SPINNER_4; + adj_period = FEC_PTP_SPINNER_2; + } else { + adj_inc = FEC_PTP_SPINNER_4; + adj_period = FEC_PTP_SPINNER_4; + } + + for (i = 1; i < adj_inc; i++) { + tmp_current = (ptp_ts_clk * i) % ndrift; + if (tmp_current == 0) { + ptc->corr_inc = i; + ptc->corr_period = (u32)((ptp_ts_clk * + adj_period * i) / ndrift); + break; + } else if (tmp_current < tmp_winner) { + ptc->corr_inc = i; + ptc->corr_period = (u32)((ptp_ts_clk * + adj_period * i) / ndrift); + tmp_winner = tmp_current; + } + } + } +} + +static void fec_set_drift(struct fec_enet_private *priv, + struct ptp_set_comp *comp) +{ + struct ptp_time_correct tc; + u32 tmp, corr_ns; + u32 ptp_inc; + + memset(&tc, 0, sizeof(struct ptp_time_correct)); + fec_handle_ptpdrift(priv, comp, &tc); + if (tc.corr_inc == 0) + return; + + ptp_inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(priv->clk_ptp); + if (comp->o_ops == TRUE) + corr_ns = ptp_inc + tc.corr_inc; + else + corr_ns = ptp_inc - tc.corr_inc; + + tmp = readl(priv->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK; + tmp |= corr_ns << FEC_T_INC_CORR_OFFSET; + writel(tmp, priv->hwp + FEC_ATIME_INC); + writel(tc.corr_period, priv->hwp + FEC_ATIME_CORR); +} + /** * fec_ptp_read - read raw cycle counter (to be used by time counter) * @cc: the cyclecounter structure @@ -106,18 +550,25 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev) unsigned long flags; int inc; - inc = 1000000000 / fep->cycle_speed; + inc = FEC_T_PERIOD_ONE_SEC / clk_get_rate(fep->clk_ptp); /* grab the ptp lock */ spin_lock_irqsave(&fep->tmreg_lock, flags); + writel(FEC_T_CTRL_RESTART, fep->hwp + FEC_ATIME_CTRL); /* 1ns counter */ writel(inc << FEC_T_INC_OFFSET, fep->hwp + FEC_ATIME_INC); - /* use free running count */ - writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD); - - writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL); + if (fep->hwts_rx_en_ioctl || fep->hwts_tx_en_ioctl) { + writel(FEC_T_PERIOD_ONE_SEC, fep->hwp + FEC_ATIME_EVT_PERIOD); + /* start counter */ + writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE, + fep->hwp + FEC_ATIME_CTRL); + } else if (fep->hwts_tx_en || fep->hwts_tx_en) { + /* use free running count */ + writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD); + writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL); + } memset(&fep->cc, 0, sizeof(fep->cc)); fep->cc.read = fec_ptp_read; @@ -277,50 +728,140 @@ static int fec_ptp_enable(struct ptp_clock_info *ptp, int fec_ptp_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd) { struct fec_enet_private *fep = netdev_priv(ndev); - struct hwtstamp_config config; - - if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) - return -EFAULT; - - /* reserved for future extensions */ - if (config.flags) - return -EINVAL; - - switch (config.tx_type) { - case HWTSTAMP_TX_OFF: + struct ptp_rtc_time curr_time; + struct ptp_time rx_time, tx_time; + 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; + + switch (cmd) { + case SIOCSHWTSTAMP: + if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) + return -EFAULT; + + /* reserved for future extensions */ + if (config.flags) + return -EINVAL; + + switch (config.tx_type) { + case HWTSTAMP_TX_OFF: + fep->hwts_tx_en = 0; + break; + case HWTSTAMP_TX_ON: + fep->hwts_tx_en = 1; + fep->hwts_tx_en_ioctl = 0; + break; + default: + return -ERANGE; + } + + switch (config.rx_filter) { + case HWTSTAMP_FILTER_NONE: + if (fep->hwts_rx_en) + fep->hwts_rx_en = 0; + config.rx_filter = HWTSTAMP_FILTER_NONE; + break; + + default: + /* + * register RXMTRL must be set in order + * to do V1 packets, therefore it is not + * possible to time stamp both V1 Sync and + * Delay_Req messages and hardware does not support + * timestamping all packets => return error + */ + fep->hwts_rx_en = 1; + fep->hwts_rx_en_ioctl = 0; + config.rx_filter = HWTSTAMP_FILTER_ALL; + break; + } + + fec_ptp_start_cyclecounter(ndev); + return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? + -EFAULT : 0; + break; + case PTP_ENBL_TXTS_IOCTL: + case PTP_ENBL_RXTS_IOCTL: + fep->hwts_rx_en_ioctl = 1; + fep->hwts_tx_en_ioctl = 1; + fep->hwts_rx_en = 0; fep->hwts_tx_en = 0; + fec_ptp_start_cyclecounter(ndev); break; - case HWTSTAMP_TX_ON: - fep->hwts_tx_en = 1; + case PTP_DSBL_RXTS_IOCTL: + case PTP_DSBL_TXTS_IOCTL: + fep->hwts_rx_en_ioctl = 0; + fep->hwts_tx_en_ioctl = 0; break; - default: - return -ERANGE; - } - - switch (config.rx_filter) { - case HWTSTAMP_FILTER_NONE: - if (fep->hwts_rx_en) - fep->hwts_rx_en = 0; - config.rx_filter = HWTSTAMP_FILTER_NONE; + case PTP_GET_RX_TIMESTAMP: + 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_ptp_find_and_remove(&fep->rx_timestamps, + &p_ts.ident, &rx_time); + if (retval == 0 && + copy_to_user((void __user *)(&p_ts_user->ts), + &rx_time, sizeof(rx_time))) + return -EFAULT; break; - - default: - /* - * register RXMTRL must be set in order to do V1 packets, - * therefore it is not possible to time stamp both V1 Sync and - * Delay_Req messages and hardware does not support - * timestamping all packets => return error - */ - fep->hwts_rx_en = 1; - config.rx_filter = HWTSTAMP_FILTER_ALL; + case PTP_GET_TX_TIMESTAMP: + 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_ptp_find_and_remove(&fep->tx_timestamps, + &p_ts.ident, &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(fep, &curr_time); + if (0 != copy_to_user(ifr->ifr_data, + &(curr_time.rtc_time), + sizeof(struct ptp_time))) + return -EFAULT; + break; + case PTP_SET_RTC_TIME: + if (0 != copy_from_user(&(curr_time.rtc_time), + ifr->ifr_data, + sizeof(struct ptp_time))) + return -EINVAL; + fec_set_1588cnt(fep, &curr_time); break; + case PTP_FLUSH_TIMESTAMP: + /* reset tx-timestamping buffer */ + fep->tx_timestamps.front = 0; + fep->tx_timestamps.end = 0; + fep->tx_timestamps.size = (DEFAULT_PTP_TX_BUF_SZ + 1); + /* reset rx-timestamping buffer */ + fep->rx_timestamps.front = 0; + fep->rx_timestamps.end = 0; + fep->rx_timestamps.size = (DEFAULT_PTP_RX_BUF_SZ + 1); + break; + case PTP_SET_COMPENSATION: + if (0 != copy_from_user(&p_comp, ifr->ifr_data, + sizeof(struct ptp_set_comp))) + return -EINVAL; + fec_set_drift(fep, &p_comp); + break; + case PTP_GET_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; } - return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? - -EFAULT : 0; + return retval; } - /** * fec_time_keep - call timecounter_read every second to avoid timer overrun * because ENET just support 32bit counter, will timeout in 4s @@ -383,4 +924,22 @@ void fec_ptp_init(struct platform_device *pdev) fep->ptp_clock = NULL; pr_err("ptp_clock_register failed\n"); } + + /* initialize circular buffer for tx timestamps */ + if (fec_ptp_init_circ(&(fep->tx_timestamps), + (DEFAULT_PTP_TX_BUF_SZ+1))) + pr_err("init tx circular buffer failed\n"); + /* initialize circular buffer for rx timestamps */ + if (fec_ptp_init_circ(&(fep->rx_timestamps), + (DEFAULT_PTP_RX_BUF_SZ+1))) + pr_err("init rx curcular buffer failed\n"); } + +void fec_ptp_cleanup(struct fec_enet_private *priv) +{ + if (priv->tx_timestamps.data_buf) + vfree(priv->tx_timestamps.data_buf); + if (priv->rx_timestamps.data_buf) + vfree(priv->rx_timestamps.data_buf); +} + -- 2.39.5