stmmac: add the support for PTP hw clock driver
authorRayagond Kokatanur <rayagond@vayavyalabs.com>
Tue, 26 Mar 2013 04:43:11 +0000 (04:43 +0000)
committerDavid S. Miller <davem@davemloft.net>
Tue, 26 Mar 2013 16:53:37 +0000 (12:53 -0400)
This patch implements PHC (ptp hardware clock) driver for stmmac
driver to support 1588 PTP.

V2: added support for FINE method, reduced loop delay and review spinlock.

Signed-off-by: Rayagond Kokatanur <rayagond@vayavyalabs.com>
Hacked-by: Giuseppe Cavallaro <peppe.cavallaro@st.com>
Cc: Richard Cochran <richardcochran@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>
drivers/net/ethernet/stmicro/stmmac/Kconfig
drivers/net/ethernet/stmicro/stmmac/Makefile
drivers/net/ethernet/stmicro/stmmac/common.h
drivers/net/ethernet/stmicro/stmmac/stmmac.h
drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c [new file with mode: 0644]

index f0720d0d57714996b80489a1309264921270a1d4..f695a50bac47d99cdc141a3cf059dff340e54dc3 100644 (file)
@@ -5,6 +5,7 @@ config STMMAC_ETH
        select MII
        select PHYLIB
        select CRC32
+       select PTP_1588_CLOCK
        ---help---
          This is the driver for the Ethernet IPs are built around a
          Synopsys IP Core and only tested on the STMicroelectronics
index 1aca0e6881bda465bdb580f4e36058f00878c20c..356a9dd32be7e57a2ff1855304b4c22d17a5dc03 100644 (file)
@@ -4,4 +4,4 @@ stmmac-$(CONFIG_STMMAC_PCI) += stmmac_pci.o
 stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o \
              chain_mode.o dwmac_lib.o dwmac1000_core.o  dwmac1000_dma.o \
              dwmac100_core.o dwmac100_dma.o enh_desc.o  norm_desc.o \
-             mmc_core.o stmmac_hwtstamp.o $(stmmac-y)
+             mmc_core.o stmmac_hwtstamp.o stmmac_ptp.o $(stmmac-y)
index 6fa975c5523442d4e0fcf19c1fbdced6886b03ca..ad7e20a9875d03bee2e063acde901d792bc17342 100644 (file)
@@ -411,6 +411,9 @@ struct stmmac_hwtimestamp {
        void (*config_sub_second_increment) (void __iomem *ioaddr);
        int (*init_systime) (void __iomem *ioaddr, u32 sec, u32 nsec);
        int (*config_addend)(void __iomem *ioaddr, u32 addend);
+       int (*adjust_systime)(void __iomem *ioaddr, u32 sec, u32 nsec,
+                             int add_sub);
+       u64 (*get_systime)(void __iomem *ioaddr);
 };
 
 struct mac_link {
index a21d1b9c90943db0201806cb56f43ef364f2c88f..52002e7c59aee401d535958b0ab4214d25a59c68 100644 (file)
@@ -31,6 +31,7 @@
 #include <linux/phy.h>
 #include <linux/pci.h>
 #include "common.h"
+#include <linux/ptp_clock_kernel.h>
 
 struct stmmac_priv {
        /* Frequently used values are kept adjacent for cache effect */
@@ -103,6 +104,9 @@ struct stmmac_priv {
        int hwts_rx_en;
        unsigned int default_addend;
        u32 adv_ts;
+       struct ptp_clock *ptp_clock;
+       struct ptp_clock_info ptp_clock_ops;
+       spinlock_t ptp_lock;
 };
 
 extern int phyaddr;
@@ -113,6 +117,8 @@ extern void stmmac_set_ethtool_ops(struct net_device *netdev);
 extern const struct stmmac_desc_ops enh_desc_ops;
 extern const struct stmmac_desc_ops ndesc_ops;
 extern const struct stmmac_hwtimestamp stmmac_ptp;
+extern int stmmac_ptp_register(struct stmmac_priv *priv);
+extern void stmmac_ptp_unregister(struct stmmac_priv *priv);
 int stmmac_freeze(struct net_device *ndev);
 int stmmac_restore(struct net_device *ndev);
 int stmmac_resume(struct net_device *ndev);
index 5b340c23fd6bbbe48a8b846d79efe725f90ca079..c5f9cb85c8ef9c31c2ce05894faffa2a484a8093 100644 (file)
@@ -737,6 +737,9 @@ static int stmmac_get_ts_info(struct net_device *dev,
                                        SOF_TIMESTAMPING_RX_HARDWARE |
                                        SOF_TIMESTAMPING_RAW_HARDWARE;
 
+               if (priv->ptp_clock)
+                       info->phc_index = ptp_clock_index(priv->ptp_clock);
+
                info->tx_types = (1 << HWTSTAMP_TX_OFF) | (1 << HWTSTAMP_TX_ON);
 
                info->rx_filters = ((1 << HWTSTAMP_FILTER_NONE) |
index 380baeb016a9c2a30f1bca47814125ffbb98fddc..def7e75e1d5797979a2597eb6c8f705c7ec6f711 100644 (file)
@@ -100,9 +100,49 @@ static int stmmac_config_addend(void __iomem *ioaddr, u32 addend)
        return 0;
 }
 
+static int stmmac_adjust_systime(void __iomem *ioaddr, u32 sec, u32 nsec,
+                                int add_sub)
+{
+       u32 value;
+       int limit;
+
+       writel(sec, ioaddr + PTP_STSUR);
+       writel(((add_sub << PTP_STNSUR_ADDSUB_SHIFT) | nsec),
+               ioaddr + PTP_STNSUR);
+       /* issue command to initialize the system time value */
+       value = readl(ioaddr + PTP_TCR);
+       value |= PTP_TCR_TSUPDT;
+       writel(value, ioaddr + PTP_TCR);
+
+       /* wait for present system time adjust/update to complete */
+       limit = 10;
+       while (limit--) {
+               if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSUPDT))
+                       break;
+               mdelay(10);
+       }
+       if (limit < 0)
+               return -EBUSY;
+
+       return 0;
+}
+
+static u64 stmmac_get_systime(void __iomem *ioaddr)
+{
+       u64 ns;
+
+       ns = readl(ioaddr + PTP_STNSR);
+       /* convert sec time value to nanosecond */
+       ns += readl(ioaddr + PTP_STSR) * 1000000000ULL;
+
+       return ns;
+}
+
 const struct stmmac_hwtimestamp stmmac_ptp = {
        .config_hw_tstamping = stmmac_config_hw_tstamping,
        .init_systime = stmmac_init_systime,
        .config_sub_second_increment = stmmac_config_sub_second_increment,
        .config_addend = stmmac_config_addend,
+       .adjust_systime = stmmac_adjust_systime,
+       .get_systime = stmmac_get_systime,
 };
index 6906772069e33a7aed97b1323861a3c58dec4847..6b26d31c268fdbc9d4e5e80b6190277808a0ce02 100644 (file)
@@ -618,20 +618,32 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
                            sizeof(struct hwtstamp_config)) ? -EFAULT : 0;
 }
 
-static void stmmac_init_ptp(struct stmmac_priv *priv)
+static int stmmac_init_ptp(struct stmmac_priv *priv)
 {
-       if (priv->dma_cap.time_stamp) {
-               pr_debug("IEEE 1588-2002 Time Stamp supported\n");
-               priv->adv_ts = 0;
-       }
-       if (priv->dma_cap.atime_stamp && priv->extend_desc) {
-               pr_debug("IEEE 1588-2008 Advanced Time Stamp supported\n");
-               priv->adv_ts = 1;
+       if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp))
+               return -EOPNOTSUPP;
+
+       if (netif_msg_hw(priv)) {
+               if (priv->dma_cap.time_stamp) {
+                       pr_debug("IEEE 1588-2002 Time Stamp supported\n");
+                       priv->adv_ts = 0;
+               }
+               if (priv->dma_cap.atime_stamp && priv->extend_desc) {
+                       pr_debug("IEEE 1588-2008 Advanced Time Stamp supported\n");
+                       priv->adv_ts = 1;
+               }
        }
 
        priv->hw->ptp = &stmmac_ptp;
        priv->hwts_tx_en = 0;
        priv->hwts_rx_en = 0;
+
+       return stmmac_ptp_register(priv);
+}
+
+static void stmmac_release_ptp(struct stmmac_priv *priv)
+{
+       stmmac_ptp_unregister(priv);
 }
 
 /**
@@ -1567,7 +1579,9 @@ static int stmmac_open(struct net_device *dev)
 
        stmmac_mmc_setup(priv);
 
-       stmmac_init_ptp(priv);
+       ret = stmmac_init_ptp(priv);
+       if (ret)
+               pr_warn("%s: failed PTP initialisation\n", __func__);
 
 #ifdef CONFIG_STMMAC_DEBUG_FS
        ret = stmmac_init_fs(dev);
@@ -1677,6 +1691,8 @@ static int stmmac_release(struct net_device *dev)
 #endif
        clk_disable_unprepare(priv->stmmac_clk);
 
+       stmmac_release_ptp(priv);
+
        return 0;
 }
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
new file mode 100644 (file)
index 0000000..93d4bef
--- /dev/null
@@ -0,0 +1,215 @@
+/*******************************************************************************
+  PTP 1588 clock using the STMMAC.
+
+  Copyright (C) 2013  Vayavya Labs Pvt Ltd
+
+  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.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
+*******************************************************************************/
+#include "stmmac.h"
+#include "stmmac_ptp.h"
+
+/**
+ * stmmac_adjust_freq
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @ppb: desired period change in parts ber billion
+ *
+ * Description: this function will adjust the frequency of hardware clock.
+ */
+static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
+{
+       struct stmmac_priv *priv =
+           container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+       unsigned long flags;
+       u32 diff, addend;
+       int neg_adj = 0;
+       u64 adj;
+
+       if (ppb < 0) {
+               neg_adj = 1;
+               ppb = -ppb;
+       }
+
+       addend = priv->default_addend;
+       adj = addend;
+       adj *= ppb;
+       diff = div_u64(adj, 1000000000ULL);
+       addend = neg_adj ? (addend - diff) : (addend + diff);
+
+       spin_lock_irqsave(&priv->ptp_lock, flags);
+
+       priv->hw->ptp->config_addend(priv->ioaddr, addend);
+
+       spin_unlock_irqrestore(&priv->lock, flags);
+
+       return 0;
+}
+
+/**
+ * stmmac_adjust_time
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @delta: desired change in nanoseconds
+ *
+ * Description: this function will shift/adjust the hardware clock time.
+ */
+static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
+{
+       struct stmmac_priv *priv =
+           container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+       unsigned long flags;
+       u32 sec, nsec;
+       u32 quotient, reminder;
+       int neg_adj = 0;
+
+       if (delta < 0) {
+               neg_adj = 1;
+               delta = -delta;
+       }
+
+       quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
+       sec = quotient;
+       nsec = reminder;
+
+       spin_lock_irqsave(&priv->ptp_lock, flags);
+
+       priv->hw->ptp->adjust_systime(priv->ioaddr, sec, nsec, neg_adj);
+
+       spin_unlock_irqrestore(&priv->lock, flags);
+
+       return 0;
+}
+
+/**
+ * stmmac_get_time
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @ts: pointer to hold time/result
+ *
+ * Description: this function will read the current time from the
+ * hardware clock and store it in @ts.
+ */
+static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+       struct stmmac_priv *priv =
+           container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+       unsigned long flags;
+       u64 ns;
+       u32 reminder;
+
+       spin_lock_irqsave(&priv->ptp_lock, flags);
+
+       ns = priv->hw->ptp->get_systime(priv->ioaddr);
+
+       spin_unlock_irqrestore(&priv->ptp_lock, flags);
+
+       ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder);
+       ts->tv_nsec = reminder;
+
+       return 0;
+}
+
+/**
+ * stmmac_set_time
+ *
+ * @ptp: pointer to ptp_clock_info structure
+ * @ts: time value to set
+ *
+ * Description: this function will set the current time on the
+ * hardware clock.
+ */
+static int stmmac_set_time(struct ptp_clock_info *ptp,
+                          const struct timespec *ts)
+{
+       struct stmmac_priv *priv =
+           container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+       unsigned long flags;
+
+       spin_lock_irqsave(&priv->ptp_lock, flags);
+
+       priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec);
+
+       spin_unlock_irqrestore(&priv->ptp_lock, flags);
+
+       return 0;
+}
+
+static int stmmac_enable(struct ptp_clock_info *ptp,
+                        struct ptp_clock_request *rq, int on)
+{
+       return -EOPNOTSUPP;
+}
+
+/* structure describing a PTP hardware clock */
+static struct ptp_clock_info stmmac_ptp_clock_ops = {
+       .owner = THIS_MODULE,
+       .name = "stmmac_ptp_clock",
+       .max_adj = 62500000,
+       .n_alarm = 0,
+       .n_ext_ts = 0,
+       .n_per_out = 0,
+       .pps = 0,
+       .adjfreq = stmmac_adjust_freq,
+       .adjtime = stmmac_adjust_time,
+       .gettime = stmmac_get_time,
+       .settime = stmmac_set_time,
+       .enable = stmmac_enable,
+};
+
+/**
+ * stmmac_ptp_register
+ *
+ * @ndev: net device pointer
+ *
+ * Description: this function will register the ptp clock driver
+ * to kernel. It also does some house keeping work.
+ */
+int stmmac_ptp_register(struct stmmac_priv *priv)
+{
+       spin_lock_init(&priv->ptp_lock);
+       priv->ptp_clock_ops = stmmac_ptp_clock_ops;
+
+       priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
+                                            priv->device);
+       if (IS_ERR(priv->ptp_clock)) {
+               priv->ptp_clock = NULL;
+               pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
+       } else
+               pr_debug("Added PTP HW clock successfully on %s\n",
+                        priv->dev->name);
+
+       return 0;
+}
+
+/**
+ * stmmac_ptp_unregister
+ *
+ * @ndev: net device pointer
+ *
+ * Description: this function will remove/unregister the ptp clock driver
+ * from the kernel.
+ */
+void stmmac_ptp_unregister(struct stmmac_priv *priv)
+{
+       if (priv->ptp_clock) {
+               ptp_clock_unregister(priv->ptp_clock);
+               pr_debug("Removed PTP HW clock successfully on %s\n",
+                        priv->dev->name);
+       }
+}