ti_cpsw: Implement the SIOCGHWTSTAMP ioctl

This is untested.

Signed-off-by: Ben Hutchings <bhutchings@solarflare.com>

1 files changed, 22 insertions, 2 deletions

diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 7536a4c01293..bc0cb154fc53 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1322,7 +1322,7 @@ static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
         __raw_writel(ETH_P_1588, &priv->regs->ts_ltype);
 }
 
-static int cpsw_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
+static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 {
         struct cpsw_priv *priv = netdev_priv(dev);
         struct cpts *cpts = priv->cpts;
@@ -1383,6 +1383,24 @@ static int cpsw_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 }
 
+static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
+{
+        struct cpsw_priv *priv = netdev_priv(dev);
+        struct cpts *cpts = priv->cpts;
+        struct hwtstamp_config cfg;
+
+        if (priv->version != CPSW_VERSION_1 &&
+            priv->version != CPSW_VERSION_2)
+                return -EOPNOTSUPP;
+
+        cfg.flags = 0;
+        cfg.tx_type = cpts->tx_enable ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
+        cfg.rx_filter = (cpts->rx_enable ?
+                         HWTSTAMP_FILTER_PTP_V2_EVENT : HWTSTAMP_FILTER_NONE);
+
+        return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
+}
+
 #endif /*CONFIG_TI_CPTS*/
 
 static int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
@@ -1397,7 +1415,9 @@ static int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
         switch (cmd) {
 #ifdef CONFIG_TI_CPTS
         case SIOCSHWTSTAMP:
-                return cpsw_hwtstamp_ioctl(dev, req);
+                return cpsw_hwtstamp_set(dev, req);
+        case SIOCGHWTSTAMP:
+                return cpsw_hwtstamp_get(dev, req);
 #endif
         case SIOCGMIIPHY:
                 data->phy_id = priv->slaves[slave_no].phy->addr;