/* * Fast Ethernet Controller (ENET) PTP driver for MX6x. * * Copyright (C) 2012-2014 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, * version 2, as published by the Free Software Foundation. * * This program is distributed in the hope 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 St - Fifth Floor, Boston, MA 02110-1301 USA. */ #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "fec.h" /* FEC 1588 register bits */ #define FEC_T_CTRL_SLAVE 0x00002000 #define FEC_T_CTRL_CAPTURE 0x00000800 #define FEC_T_CTRL_RESTART 0x00000200 #define FEC_T_CTRL_PERIOD_RST 0x00000030 #define FEC_T_CTRL_PERIOD_EN 0x00000010 #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_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 #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; tmp = (ptp_buf->data_buf + ptp_buf->end); memcpy(tmp, data, sizeof(struct fec_ptp_ts_data)); if (fec_ptp_is_full(ptp_buf)) /* drop one in front */ ptp_buf->front = fec_ptp_calc_index(ptp_buf->size, ptp_buf->front, 1); 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; 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); del_timer_sync(&priv->time_keep); } static void fec_get_curr_cnt(struct fec_enet_private *priv, struct ptp_rtc_time *curr_time) { u32 tempval, old_sec; u32 timeout_event, timeout_ts = 0; const struct platform_device_id *id_entry = platform_get_device_id(priv->pdev); do { old_sec = priv->prtc; timeout_event = 0; tempval = readl(priv->hwp + FEC_ATIME_CTRL); tempval |= FEC_T_CTRL_CAPTURE; writel(tempval, priv->hwp + FEC_ATIME_CTRL); if (id_entry->driver_data & FEC_QUIRK_TKT210590) udelay(1); curr_time->rtc_time.nsec = readl(priv->hwp + FEC_ATIME); while (readl(priv->hwp + FEC_IEVENT) & FEC_ENET_TS_TIMER) { timeout_event++; udelay(20); if (timeout_event >= FEC_PTP_TIMEOUT_EVENT) break; } curr_time->rtc_time.sec = priv->prtc; timeout_ts++; if (timeout_event >= FEC_PTP_TIMEOUT_EVENT) pr_err("timeout: TS TIMER event\n"); } while (old_sec != curr_time->rtc_time.sec && timeout_ts < FEC_PTP_TIMEOUT_TS); if (timeout_ts >= FEC_PTP_TIMEOUT_TS) pr_err("timeout: current timestamp unmatched\n"); } /* 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; struct ptp_rtc_time curr_time; 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 */ fec_get_curr_cnt(priv, &curr_time); if (curr_time.rtc_time.nsec < bdp_ex->ts) tmp_tx_time.ts.sec = curr_time.rtc_time.sec - 1; else tmp_tx_time.ts.sec = curr_time.rtc_time.sec; 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; struct ptp_rtc_time curr_time; 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 */ fec_get_curr_cnt(priv, &curr_time); if (curr_time.rtc_time.nsec < bdp_ex->ts) tmp_rx_time.ts.sec = curr_time.rtc_time.sec - 1; else tmp_rx_time.ts.sec = curr_time.rtc_time.sec; 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; 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; } for (i = 1; i <= ptp_inc; i++) { if (((i * FEC_T_PERIOD_ONE_SEC) / ndrift) > ptp_inc) { ptc->corr_inc = i; ptc->corr_period = ((i * FEC_T_PERIOD_ONE_SEC) / (ptp_inc * ndrift)); break; } } /* not found ? */ if (i > ptp_inc) { /* * set it to high value - double speed * correct in every clock step. */ ptc->corr_inc = ptp_inc; ptc->corr_period = 1; } } 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 * * this function reads the cyclecounter registers and is called by the * cyclecounter structure used to construct a ns counter from the * arbitrary fixed point registers */ static cycle_t fec_ptp_read(const struct cyclecounter *cc) { struct fec_enet_private *fep = container_of(cc, struct fec_enet_private, cc); const struct platform_device_id *id_entry = platform_get_device_id(fep->pdev); u32 tempval; tempval = readl(fep->hwp + FEC_ATIME_CTRL); tempval |= FEC_T_CTRL_CAPTURE; writel(tempval, fep->hwp + FEC_ATIME_CTRL); if (id_entry->driver_data & FEC_QUIRK_TKT210590) udelay(1); return readl(fep->hwp + FEC_ATIME); } /** * fec_ptp_start_cyclecounter - create the cycle counter from hw * @ndev: network device * * this function initializes the timecounter and cyclecounter * structures for use in generated a ns counter from the arbitrary * fixed point cycles registers in the hardware. */ void fec_ptp_start_cyclecounter(struct net_device *ndev) { struct fec_enet_private *fep = netdev_priv(ndev); unsigned long flags; int inc; inc = FEC_T_PERIOD_ONE_SEC / fep->cycle_speed; /* 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); 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; fep->cc.mask = CLOCKSOURCE_MASK(32); fep->cc.shift = 31; fep->cc.mult = FEC_CC_MULT; /* reset the ns time counter */ timecounter_init(&fep->tc, &fep->cc, ktime_to_ns(ktime_get_real())); spin_unlock_irqrestore(&fep->tmreg_lock, flags); if (!timer_pending(&fep->time_keep) && fep->opened) add_timer(&fep->time_keep); } /** * fec_ptp_adjfreq - adjust ptp cycle frequency * @ptp: the ptp clock structure * @ppb: parts per billion adjustment from base * * Adjust the frequency of the ptp cycle counter by the * indicated ppb from the base frequency. * * Because ENET hardware frequency adjust is complex, * using software method to do that. */ static int fec_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb) { u64 diff; unsigned long flags; int neg_adj = 0; u32 mult = FEC_CC_MULT; struct fec_enet_private *fep = container_of(ptp, struct fec_enet_private, ptp_caps); if (ppb < 0) { ppb = -ppb; neg_adj = 1; } diff = mult; diff *= ppb; diff = div_u64(diff, 1000000000ULL); spin_lock_irqsave(&fep->tmreg_lock, flags); /* * dummy read to set cycle_last in tc to now. * So use adjusted mult to calculate when next call * timercounter_read. */ timecounter_read(&fep->tc); fep->cc.mult = neg_adj ? mult - diff : mult + diff; spin_unlock_irqrestore(&fep->tmreg_lock, flags); return 0; } /** * fec_ptp_adjtime * @ptp: the ptp clock structure * @delta: offset to adjust the cycle counter by * * adjust the timer by resetting the timecounter structure. */ static int fec_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) { struct fec_enet_private *fep = container_of(ptp, struct fec_enet_private, ptp_caps); unsigned long flags; u64 now; spin_lock_irqsave(&fep->tmreg_lock, flags); now = timecounter_read(&fep->tc); now += delta; /* reset the timecounter */ timecounter_init(&fep->tc, &fep->cc, now); spin_unlock_irqrestore(&fep->tmreg_lock, flags); return 0; } /** * fec_ptp_gettime * @ptp: the ptp clock structure * @ts: timespec structure to hold the current time value * * read the timecounter and return the correct value on ns, * after converting it into a struct timespec. */ static int fec_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts) { struct fec_enet_private *adapter = container_of(ptp, struct fec_enet_private, ptp_caps); u64 ns; u32 remainder; unsigned long flags; spin_lock_irqsave(&adapter->tmreg_lock, flags); ns = timecounter_read(&adapter->tc); spin_unlock_irqrestore(&adapter->tmreg_lock, flags); ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder); ts->tv_nsec = remainder; return 0; } /** * fec_ptp_settime * @ptp: the ptp clock structure * @ts: the timespec containing the new time for the cycle counter * * reset the timecounter to use a new base value instead of the kernel * wall timer value. */ static int fec_ptp_settime(struct ptp_clock_info *ptp, const struct timespec *ts) { struct fec_enet_private *fep = container_of(ptp, struct fec_enet_private, ptp_caps); u64 ns; unsigned long flags; ns = ts->tv_sec * 1000000000ULL; ns += ts->tv_nsec; spin_lock_irqsave(&fep->tmreg_lock, flags); timecounter_init(&fep->tc, &fep->cc, ns); spin_unlock_irqrestore(&fep->tmreg_lock, flags); return 0; } /** * fec_ptp_enable * @ptp: the ptp clock structure * @rq: the requested feature to change * @on: whether to enable or disable the feature * */ static int fec_ptp_enable(struct ptp_clock_info *ptp, struct ptp_clock_request *rq, int on) { return -EOPNOTSUPP; } /** * fec_ptp_hwtstamp_ioctl - control hardware time stamping * @ndev: pointer to net_device * @ifreq: ioctl data * @cmd: particular ioctl requested */ 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; 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 PTP_DSBL_RXTS_IOCTL: case PTP_DSBL_TXTS_IOCTL: fep->hwts_rx_en_ioctl = 0; fep->hwts_tx_en_ioctl = 0; break; 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; if (fec_ptp_find_and_remove(&fep->rx_timestamps, &p_ts.ident, &rx_time)) { usleep_range(4000, 5000); return -EAGAIN; } if (copy_to_user((void __user *)(&p_ts_user->ts), &rx_time, sizeof(rx_time))) return -EFAULT; break; 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; if (fec_ptp_find_and_remove(&fep->tx_timestamps, &p_ts.ident, &tx_time)) { usleep_range(4000, 5000); return -EAGAIN; } if (copy_to_user((void __user *)(&p_ts_user->ts), &tx_time, sizeof(tx_time))) return -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 retval; } /** * fec_time_keep - call timecounter_read every second to avoid timer overrun * because ENET just support 32bit counter, will timeout in 4s */ static void fec_time_keep(unsigned long _data) { struct fec_enet_private *fep = (struct fec_enet_private *)_data; u64 ns; unsigned long flags; spin_lock_irqsave(&fep->tmreg_lock, flags); ns = timecounter_read(&fep->tc); spin_unlock_irqrestore(&fep->tmreg_lock, flags); mod_timer(&fep->time_keep, jiffies + HZ); } /** * fec_ptp_init * @ndev: The FEC network adapter * * This function performs the required steps for enabling ptp * support. If ptp support has already been loaded it simply calls the * cyclecounter init routine and exits. */ void fec_ptp_init(struct platform_device *pdev) { struct net_device *ndev = platform_get_drvdata(pdev); struct fec_enet_private *fep = netdev_priv(ndev); fep->ptp_caps.owner = THIS_MODULE; snprintf(fep->ptp_caps.name, 16, "fec ptp"); fep->ptp_caps.max_adj = 250000000; fep->ptp_caps.n_alarm = 0; fep->ptp_caps.n_ext_ts = 0; fep->ptp_caps.n_per_out = 0; fep->ptp_caps.pps = 0; fep->ptp_caps.adjfreq = fec_ptp_adjfreq; fep->ptp_caps.adjtime = fec_ptp_adjtime; fep->ptp_caps.gettime = fec_ptp_gettime; fep->ptp_caps.settime = fec_ptp_settime; fep->ptp_caps.enable = fec_ptp_enable; fep->cycle_speed = clk_get_rate(fep->clk_ptp); spin_lock_init(&fep->tmreg_lock); fec_ptp_start_cyclecounter(ndev); init_timer(&fep->time_keep); fep->time_keep.data = (unsigned long)fep; fep->time_keep.function = fec_time_keep; fep->time_keep.expires = jiffies + HZ; fep->ptp_clock = ptp_clock_register(&fep->ptp_caps, &pdev->dev); if (IS_ERR(fep->ptp_clock)) { 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); }