From 244281f46581a1716e99b6b6fe0aaf019e0d8b65 Mon Sep 17 00:00:00 2001 From: Luwei Zhou Date: Thu, 15 Aug 2013 13:45:23 +0800 Subject: 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_ptp.c | 641 +++++++++++++++++++++++++++++-- 1 file changed, 600 insertions(+), 41 deletions(-) (limited to 'drivers/net/ethernet/freescale/fec_ptp.c') 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); +} + -- cgit v1.2.2