#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))
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 {
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);
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);
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;
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");
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 =
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)) {
}
}
+ 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);
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);
}
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)
}
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);
/*
* 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,
#include <linux/workqueue.h>
#include <linux/bitops.h>
#include <linux/io.h>
+#include <linux/vmalloc.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
#include <linux/irq.h>
#include <linux/clk.h>
#include <linux/platform_device.h>
#include "fec.h"
+
/* FEC 1588 register bits */
#define FEC_T_CTRL_SLAVE 0x00002000
#define FEC_T_CTRL_CAPTURE 0x00000800
#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
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;
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
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);
+}
+
projects / karo-tx-linux.git / commitdiff