summaryrefslogtreecommitdiff
path: root/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_1588.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/net/ethernet/freescale/sdk_dpaa/dpaa_1588.c')
-rw-r--r--drivers/net/ethernet/freescale/sdk_dpaa/dpaa_1588.c580
1 files changed, 580 insertions, 0 deletions
diff --git a/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_1588.c b/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_1588.c
new file mode 100644
index 0000000..3bf8cbc
--- /dev/null
+++ b/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_1588.c
@@ -0,0 +1,580 @@
+/* Copyright (C) 2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2009 IXXAT Automation, GmbH
+ *
+ * DPAA Ethernet Driver -- IEEE 1588 interface functionality
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/vmalloc.h>
+#include <linux/spinlock.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/udp.h>
+#include <asm/div64.h>
+#include "dpaa_eth.h"
+#include "dpaa_eth_common.h"
+#include "dpaa_1588.h"
+#include "mac.h"
+
+static int dpa_ptp_init_circ(struct dpa_ptp_circ_buf *ptp_buf, u32 size)
+{
+ struct circ_buf *circ_buf = &ptp_buf->circ_buf;
+
+ circ_buf->buf = vmalloc(sizeof(struct dpa_ptp_data) * size);
+ if (!circ_buf->buf)
+ return 1;
+
+ circ_buf->head = 0;
+ circ_buf->tail = 0;
+ ptp_buf->size = size;
+ spin_lock_init(&ptp_buf->ptp_lock);
+
+ return 0;
+}
+
+static void dpa_ptp_reset_circ(struct dpa_ptp_circ_buf *ptp_buf, u32 size)
+{
+ struct circ_buf *circ_buf = &ptp_buf->circ_buf;
+
+ circ_buf->head = 0;
+ circ_buf->tail = 0;
+ ptp_buf->size = size;
+}
+
+static int dpa_ptp_insert(struct dpa_ptp_circ_buf *ptp_buf,
+ struct dpa_ptp_data *data)
+{
+ struct circ_buf *circ_buf = &ptp_buf->circ_buf;
+ int size = ptp_buf->size;
+ struct dpa_ptp_data *tmp;
+ unsigned long flags;
+ int head, tail;
+
+ spin_lock_irqsave(&ptp_buf->ptp_lock, flags);
+
+ head = circ_buf->head;
+ tail = circ_buf->tail;
+
+ if (CIRC_SPACE(head, tail, size) <= 0)
+ circ_buf->tail = (tail + 1) & (size - 1);
+
+ tmp = (struct dpa_ptp_data *)(circ_buf->buf) + head;
+ memcpy(tmp, data, sizeof(struct dpa_ptp_data));
+
+ circ_buf->head = (head + 1) & (size - 1);
+
+ spin_unlock_irqrestore(&ptp_buf->ptp_lock, flags);
+
+ return 0;
+}
+
+static int dpa_ptp_is_ident_match(struct dpa_ptp_ident *dst,
+ struct dpa_ptp_ident *src)
+{
+ int ret;
+
+ if ((dst->version != src->version) || (dst->msg_type != src->msg_type))
+ return 0;
+
+ if ((dst->netw_prot == src->netw_prot)
+ || src->netw_prot == DPA_PTP_PROT_DONTCARE) {
+ if (dst->seq_id != src->seq_id)
+ return 0;
+
+ ret = memcmp(dst->snd_port_id, src->snd_port_id,
+ DPA_PTP_SOURCE_PORT_LENGTH);
+ if (ret)
+ return 0;
+ else
+ return 1;
+ }
+
+ return 0;
+}
+
+static int dpa_ptp_find_and_remove(struct dpa_ptp_circ_buf *ptp_buf,
+ struct dpa_ptp_ident *ident,
+ struct dpa_ptp_time *ts)
+{
+ struct circ_buf *circ_buf = &ptp_buf->circ_buf;
+ int size = ptp_buf->size;
+ int head, tail, idx;
+ unsigned long flags;
+ struct dpa_ptp_data *tmp, *tmp2;
+ struct dpa_ptp_ident *tmp_ident;
+
+ spin_lock_irqsave(&ptp_buf->ptp_lock, flags);
+
+ head = circ_buf->head;
+ tail = idx = circ_buf->tail;
+
+ if (CIRC_CNT(head, tail, size) == 0) {
+ spin_unlock_irqrestore(&ptp_buf->ptp_lock, flags);
+ return 1;
+ }
+
+ while (idx != head) {
+ tmp = (struct dpa_ptp_data *)(circ_buf->buf) + idx;
+ tmp_ident = &tmp->ident;
+ if (dpa_ptp_is_ident_match(tmp_ident, ident))
+ break;
+ idx = (idx + 1) & (size - 1);
+ }
+
+ if (idx == head) {
+ spin_unlock_irqrestore(&ptp_buf->ptp_lock, flags);
+ return 1;
+ }
+
+ ts->sec = tmp->ts.sec;
+ ts->nsec = tmp->ts.nsec;
+
+ if (idx != tail) {
+ if (CIRC_CNT(idx, tail, size) > TS_ACCUMULATION_THRESHOLD) {
+ tail = circ_buf->tail =
+ (idx - TS_ACCUMULATION_THRESHOLD) & (size - 1);
+ }
+
+ while (CIRC_CNT(idx, tail, size) > 0) {
+ tmp = (struct dpa_ptp_data *)(circ_buf->buf) + idx;
+ idx = (idx - 1) & (size - 1);
+ tmp2 = (struct dpa_ptp_data *)(circ_buf->buf) + idx;
+ *tmp = *tmp2;
+ }
+ }
+ circ_buf->tail = (tail + 1) & (size - 1);
+
+ spin_unlock_irqrestore(&ptp_buf->ptp_lock, flags);
+
+ return 0;
+}
+
+/* Parse the PTP packets
+ *
+ * The PTP header can be found in an IPv4 packet, IPv6 patcket or in
+ * an IEEE802.3 ethernet frame. This function returns the position of
+ * the PTP packet or NULL if no PTP found
+ */
+static u8 *dpa_ptp_parse_packet(struct sk_buff *skb, u16 *eth_type)
+{
+ u8 *pos = skb->data + ETH_ALEN + ETH_ALEN;
+ u8 *ptp_loc = NULL;
+ u8 msg_type;
+ u32 access_len = ETH_ALEN + ETH_ALEN + DPA_ETYPE_LEN;
+ struct iphdr *iph;
+ struct udphdr *udph;
+ struct ipv6hdr *ipv6h;
+
+ /* when we can receive S/G frames we need to check the data we want to
+ * access is in the linear skb buffer
+ */
+ if (!pskb_may_pull(skb, access_len))
+ return NULL;
+
+ *eth_type = *((u16 *)pos);
+
+ /* Check if inner tag is here */
+ if (*eth_type == ETH_P_8021Q) {
+ access_len += DPA_VLAN_TAG_LEN;
+
+ if (!pskb_may_pull(skb, access_len))
+ return NULL;
+
+ pos += DPA_VLAN_TAG_LEN;
+ *eth_type = *((u16 *)pos);
+ }
+
+ pos += DPA_ETYPE_LEN;
+
+ switch (*eth_type) {
+ /* Transport of PTP over Ethernet */
+ case ETH_P_1588:
+ ptp_loc = pos;
+
+ if (!pskb_may_pull(skb, access_len + PTP_OFFS_MSG_TYPE + 1))
+ return NULL;
+
+ msg_type = *((u8 *)(ptp_loc + PTP_OFFS_MSG_TYPE)) & 0xf;
+ if ((msg_type == PTP_MSGTYPE_SYNC)
+ || (msg_type == PTP_MSGTYPE_DELREQ)
+ || (msg_type == PTP_MSGTYPE_PDELREQ)
+ || (msg_type == PTP_MSGTYPE_PDELRESP))
+ return ptp_loc;
+ break;
+ /* Transport of PTP over IPv4 */
+ case ETH_P_IP:
+ iph = (struct iphdr *)pos;
+ access_len += sizeof(struct iphdr);
+
+ if (!pskb_may_pull(skb, access_len))
+ return NULL;
+
+ if (ntohs(iph->protocol) != IPPROTO_UDP)
+ return NULL;
+
+ access_len += iph->ihl * 4 - sizeof(struct iphdr) +
+ sizeof(struct udphdr);
+
+ if (!pskb_may_pull(skb, access_len))
+ return NULL;
+
+ pos += iph->ihl * 4;
+ udph = (struct udphdr *)pos;
+ if (ntohs(udph->dest) != 319)
+ return NULL;
+ ptp_loc = pos + sizeof(struct udphdr);
+ break;
+ /* Transport of PTP over IPv6 */
+ case ETH_P_IPV6:
+ ipv6h = (struct ipv6hdr *)pos;
+
+ access_len += sizeof(struct ipv6hdr) + sizeof(struct udphdr);
+
+ if (ntohs(ipv6h->nexthdr) != IPPROTO_UDP)
+ return NULL;
+
+ pos += sizeof(struct ipv6hdr);
+ udph = (struct udphdr *)pos;
+ if (ntohs(udph->dest) != 319)
+ return NULL;
+ ptp_loc = pos + sizeof(struct udphdr);
+ break;
+ default:
+ break;
+ }
+
+ return ptp_loc;
+}
+
+static int dpa_ptp_store_stamp(const struct dpa_priv_s *priv,
+ struct sk_buff *skb, void *data, enum port_type rx_tx,
+ struct dpa_ptp_data *ptp_data)
+{
+ u64 nsec;
+ u32 mod;
+ u8 *ptp_loc;
+ u16 eth_type;
+
+ ptp_loc = dpa_ptp_parse_packet(skb, &eth_type);
+ if (!ptp_loc)
+ return -EINVAL;
+
+ switch (eth_type) {
+ case ETH_P_IP:
+ ptp_data->ident.netw_prot = DPA_PTP_PROT_IPV4;
+ break;
+ case ETH_P_IPV6:
+ ptp_data->ident.netw_prot = DPA_PTP_PROT_IPV6;
+ break;
+ case ETH_P_1588:
+ ptp_data->ident.netw_prot = DPA_PTP_PROT_802_3;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ if (!pskb_may_pull(skb, ptp_loc - skb->data + PTP_OFFS_SEQ_ID + 2))
+ return -EINVAL;
+
+ ptp_data->ident.version = *(ptp_loc + PTP_OFFS_VER_PTP) & 0xf;
+ ptp_data->ident.msg_type = *(ptp_loc + PTP_OFFS_MSG_TYPE) & 0xf;
+ ptp_data->ident.seq_id = *((u16 *)(ptp_loc + PTP_OFFS_SEQ_ID));
+ memcpy(ptp_data->ident.snd_port_id, ptp_loc + PTP_OFFS_SRCPRTID,
+ DPA_PTP_SOURCE_PORT_LENGTH);
+
+ nsec = dpa_get_timestamp_ns(priv, rx_tx, data);
+ mod = do_div(nsec, NANOSEC_PER_SECOND);
+ ptp_data->ts.sec = nsec;
+ ptp_data->ts.nsec = mod;
+
+ return 0;
+}
+
+void dpa_ptp_store_txstamp(const struct dpa_priv_s *priv,
+ struct sk_buff *skb, void *data)
+{
+ struct dpa_ptp_tsu *tsu = priv->tsu;
+ struct dpa_ptp_data ptp_tx_data;
+
+ if (dpa_ptp_store_stamp(priv, skb, data, TX, &ptp_tx_data))
+ return;
+
+ dpa_ptp_insert(&tsu->tx_timestamps, &ptp_tx_data);
+}
+
+void dpa_ptp_store_rxstamp(const struct dpa_priv_s *priv,
+ struct sk_buff *skb, void *data)
+{
+ struct dpa_ptp_tsu *tsu = priv->tsu;
+ struct dpa_ptp_data ptp_rx_data;
+
+ if (dpa_ptp_store_stamp(priv, skb, data, RX, &ptp_rx_data))
+ return;
+
+ dpa_ptp_insert(&tsu->rx_timestamps, &ptp_rx_data);
+}
+
+static uint8_t dpa_get_tx_timestamp(struct dpa_ptp_tsu *ptp_tsu,
+ struct dpa_ptp_ident *ident,
+ struct dpa_ptp_time *ts)
+{
+ struct dpa_ptp_tsu *tsu = ptp_tsu;
+ struct dpa_ptp_time tmp;
+ int flag;
+
+ flag = dpa_ptp_find_and_remove(&tsu->tx_timestamps, ident, &tmp);
+ if (!flag) {
+ ts->sec = tmp.sec;
+ ts->nsec = tmp.nsec;
+ return 0;
+ }
+
+ return -1;
+}
+
+static uint8_t dpa_get_rx_timestamp(struct dpa_ptp_tsu *ptp_tsu,
+ struct dpa_ptp_ident *ident,
+ struct dpa_ptp_time *ts)
+{
+ struct dpa_ptp_tsu *tsu = ptp_tsu;
+ struct dpa_ptp_time tmp;
+ int flag;
+
+ flag = dpa_ptp_find_and_remove(&tsu->rx_timestamps, ident, &tmp);
+ if (!flag) {
+ ts->sec = tmp.sec;
+ ts->nsec = tmp.nsec;
+ return 0;
+ }
+
+ return -1;
+}
+
+static void dpa_set_fiper_alarm(struct dpa_ptp_tsu *tsu,
+ struct dpa_ptp_time *cnt_time)
+{
+ struct mac_device *mac_dev = tsu->dpa_priv->mac_dev;
+ u64 tmp, fiper;
+
+ if (mac_dev->fm_rtc_disable)
+ mac_dev->fm_rtc_disable(get_fm_handle(tsu->dpa_priv->net_dev));
+
+ /* TMR_FIPER1 will pulse every second after ALARM1 expired */
+ tmp = (u64)cnt_time->sec * NANOSEC_PER_SECOND + (u64)cnt_time->nsec;
+ fiper = NANOSEC_PER_SECOND - DPA_PTP_NOMINAL_FREQ_PERIOD_NS;
+ if (mac_dev->fm_rtc_set_alarm)
+ mac_dev->fm_rtc_set_alarm(get_fm_handle(tsu->dpa_priv->net_dev),
+ 0, tmp);
+ if (mac_dev->fm_rtc_set_fiper)
+ mac_dev->fm_rtc_set_fiper(get_fm_handle(tsu->dpa_priv->net_dev),
+ 0, fiper);
+
+ if (mac_dev->fm_rtc_enable)
+ mac_dev->fm_rtc_enable(get_fm_handle(tsu->dpa_priv->net_dev));
+}
+
+static void dpa_get_curr_cnt(struct dpa_ptp_tsu *tsu,
+ struct dpa_ptp_time *curr_time)
+{
+ struct mac_device *mac_dev = tsu->dpa_priv->mac_dev;
+ u64 tmp;
+ u32 mod;
+
+ if (mac_dev->fm_rtc_get_cnt)
+ mac_dev->fm_rtc_get_cnt(get_fm_handle(tsu->dpa_priv->net_dev),
+ &tmp);
+
+ mod = do_div(tmp, NANOSEC_PER_SECOND);
+ curr_time->sec = (u32)tmp;
+ curr_time->nsec = mod;
+}
+
+static void dpa_set_1588cnt(struct dpa_ptp_tsu *tsu,
+ struct dpa_ptp_time *cnt_time)
+{
+ struct mac_device *mac_dev = tsu->dpa_priv->mac_dev;
+ u64 tmp;
+
+ tmp = (u64)cnt_time->sec * NANOSEC_PER_SECOND + (u64)cnt_time->nsec;
+
+ if (mac_dev->fm_rtc_set_cnt)
+ mac_dev->fm_rtc_set_cnt(get_fm_handle(tsu->dpa_priv->net_dev),
+ tmp);
+
+ /* Restart fiper two seconds later */
+ cnt_time->sec += 2;
+ cnt_time->nsec = 0;
+ dpa_set_fiper_alarm(tsu, cnt_time);
+}
+
+static void dpa_get_drift(struct dpa_ptp_tsu *tsu, u32 *addend)
+{
+ struct mac_device *mac_dev = tsu->dpa_priv->mac_dev;
+ u32 drift;
+
+ if (mac_dev->fm_rtc_get_drift)
+ mac_dev->fm_rtc_get_drift(get_fm_handle(tsu->dpa_priv->net_dev),
+ &drift);
+
+ *addend = drift;
+}
+
+static void dpa_set_drift(struct dpa_ptp_tsu *tsu, u32 addend)
+{
+ struct mac_device *mac_dev = tsu->dpa_priv->mac_dev;
+
+ if (mac_dev->fm_rtc_set_drift)
+ mac_dev->fm_rtc_set_drift(get_fm_handle(tsu->dpa_priv->net_dev),
+ addend);
+}
+
+static void dpa_flush_timestamp(struct dpa_ptp_tsu *tsu)
+{
+ dpa_ptp_reset_circ(&tsu->rx_timestamps, DEFAULT_PTP_RX_BUF_SZ);
+ dpa_ptp_reset_circ(&tsu->tx_timestamps, DEFAULT_PTP_TX_BUF_SZ);
+}
+
+int dpa_ioctl_1588(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+ struct dpa_priv_s *priv = netdev_priv(dev);
+ struct dpa_ptp_tsu *tsu = priv->tsu;
+ struct mac_device *mac_dev = priv->mac_dev;
+ struct dpa_ptp_data ptp_data;
+ struct dpa_ptp_data *ptp_data_user;
+ struct dpa_ptp_time act_time;
+ u32 addend;
+ int retval = 0;
+
+ if (!tsu || !tsu->valid)
+ return -ENODEV;
+
+ switch (cmd) {
+ case PTP_ENBL_TXTS_IOCTL:
+ tsu->hwts_tx_en_ioctl = 1;
+ if (mac_dev->fm_rtc_enable)
+ mac_dev->fm_rtc_enable(get_fm_handle(dev));
+ if (mac_dev->ptp_enable)
+ mac_dev->ptp_enable(mac_dev->get_mac_handle(mac_dev));
+ break;
+ case PTP_DSBL_TXTS_IOCTL:
+ tsu->hwts_tx_en_ioctl = 0;
+ if (mac_dev->fm_rtc_disable)
+ mac_dev->fm_rtc_disable(get_fm_handle(dev));
+ if (mac_dev->ptp_disable)
+ mac_dev->ptp_disable(mac_dev->get_mac_handle(mac_dev));
+ break;
+ case PTP_ENBL_RXTS_IOCTL:
+ tsu->hwts_rx_en_ioctl = 1;
+ break;
+ case PTP_DSBL_RXTS_IOCTL:
+ tsu->hwts_rx_en_ioctl = 0;
+ break;
+ case PTP_GET_RX_TIMESTAMP:
+ ptp_data_user = (struct dpa_ptp_data *)ifr->ifr_data;
+ if (copy_from_user(&ptp_data.ident,
+ &ptp_data_user->ident, sizeof(ptp_data.ident)))
+ return -EINVAL;
+
+ if (dpa_get_rx_timestamp(tsu, &ptp_data.ident, &ptp_data.ts))
+ return -EAGAIN;
+
+ if (copy_to_user((void __user *)&ptp_data_user->ts,
+ &ptp_data.ts, sizeof(ptp_data.ts)))
+ return -EFAULT;
+ break;
+ case PTP_GET_TX_TIMESTAMP:
+ ptp_data_user = (struct dpa_ptp_data *)ifr->ifr_data;
+ if (copy_from_user(&ptp_data.ident,
+ &ptp_data_user->ident, sizeof(ptp_data.ident)))
+ return -EINVAL;
+
+ if (dpa_get_tx_timestamp(tsu, &ptp_data.ident, &ptp_data.ts))
+ return -EAGAIN;
+
+ if (copy_to_user((void __user *)&ptp_data_user->ts,
+ &ptp_data.ts, sizeof(ptp_data.ts)))
+ return -EFAULT;
+ break;
+ case PTP_GET_TIME:
+ dpa_get_curr_cnt(tsu, &act_time);
+ if (copy_to_user(ifr->ifr_data, &act_time, sizeof(act_time)))
+ return -EFAULT;
+ break;
+ case PTP_SET_TIME:
+ if (copy_from_user(&act_time, ifr->ifr_data, sizeof(act_time)))
+ return -EINVAL;
+ dpa_set_1588cnt(tsu, &act_time);
+ break;
+ case PTP_GET_ADJ:
+ dpa_get_drift(tsu, &addend);
+ if (copy_to_user(ifr->ifr_data, &addend, sizeof(addend)))
+ return -EFAULT;
+ break;
+ case PTP_SET_ADJ:
+ if (copy_from_user(&addend, ifr->ifr_data, sizeof(addend)))
+ return -EINVAL;
+ dpa_set_drift(tsu, addend);
+ break;
+ case PTP_SET_FIPER_ALARM:
+ if (copy_from_user(&act_time, ifr->ifr_data, sizeof(act_time)))
+ return -EINVAL;
+ dpa_set_fiper_alarm(tsu, &act_time);
+ break;
+ case PTP_CLEANUP_TS:
+ dpa_flush_timestamp(tsu);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return retval;
+}
+
+int dpa_ptp_init(struct dpa_priv_s *priv)
+{
+ struct dpa_ptp_tsu *tsu;
+
+ /* Allocate memory for PTP structure */
+ tsu = kzalloc(sizeof(struct dpa_ptp_tsu), GFP_KERNEL);
+ if (!tsu)
+ return -ENOMEM;
+
+ tsu->valid = TRUE;
+ tsu->dpa_priv = priv;
+
+ dpa_ptp_init_circ(&tsu->rx_timestamps, DEFAULT_PTP_RX_BUF_SZ);
+ dpa_ptp_init_circ(&tsu->tx_timestamps, DEFAULT_PTP_TX_BUF_SZ);
+
+ priv->tsu = tsu;
+
+ return 0;
+}
+EXPORT_SYMBOL(dpa_ptp_init);
+
+void dpa_ptp_cleanup(struct dpa_priv_s *priv)
+{
+ struct dpa_ptp_tsu *tsu = priv->tsu;
+
+ tsu->valid = FALSE;
+ vfree(tsu->rx_timestamps.circ_buf.buf);
+ vfree(tsu->tx_timestamps.circ_buf.buf);
+
+ kfree(tsu);
+}
+EXPORT_SYMBOL(dpa_ptp_cleanup);
generated by cgit v0.10.2 at 2024-06-10 09:28:57 (GMT)