10 files changed, 208 insertions, 1 deletions

diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index bd88d818f082..5130db8f5c4e 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -361,6 +361,11 @@ int phy_mii_ioctl(struct phy_device *phydev,
                 }
                 break;
 
+        case SIOCSHWTSTAMP:
+                if (phydev->drv->hwtstamp)
+                        return phydev->drv->hwtstamp(phydev, ifr);
+                /* fall through */
+
         default:
                 return -EOPNOTSUPP;
         }
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 1a99bb244106..c0761197c07e 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -460,6 +460,7 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
         }
 
         phydev->attached_dev = dev;
+        dev->phydev = phydev;
 
         phydev->dev_flags = flags;
 
@@ -513,6 +514,7 @@ EXPORT_SYMBOL(phy_attach);
  */
 void phy_detach(struct phy_device *phydev)
 {
+        phydev->attached_dev->phydev = NULL;
         phydev->attached_dev = NULL;
 
         /* If the device had no specific driver before (i.e. - it
diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index c4fedf000541..fdc3f2992230 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -54,6 +54,7 @@
 
 struct vlan_group;
 struct netpoll_info;
+struct phy_device;
 /* 802.11 specific */
 struct wireless_dev;
                                         /* source back-compat hooks */
@@ -1065,6 +1066,9 @@ struct net_device {
 #endif
         /* n-tuple filter list attached to this device */
         struct ethtool_rx_ntuple_list ethtool_ntuple_list;
+
+        /* phy device may attach itself for hardware timestamping */
+        struct phy_device *phydev;
 };
 #define to_net_dev(d) container_of(d, struct net_device, dev)
 
diff --git a/include/linux/phy.h b/include/linux/phy.h
index d63736a84002..6b0a782c6224 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -234,6 +234,8 @@ enum phy_state {
         PHY_RESUMING
 };
 
+struct sk_buff;
+
 /* phy_device: An instance of a PHY
  *
  * drv: Pointer to the driver for this PHY instance
@@ -402,6 +404,26 @@ struct phy_driver {
         /* Clears up any memory if needed */
         void (*remove)(struct phy_device *phydev);
 
+        /* Handles SIOCSHWTSTAMP ioctl for hardware time stamping. */
+        int  (*hwtstamp)(struct phy_device *phydev, struct ifreq *ifr);
+
+        /*
+         * Requests a Rx timestamp for 'skb'. If the skb is accepted,
+         * the phy driver promises to deliver it using netif_rx() as
+         * soon as a timestamp becomes available. One of the
+         * PTP_CLASS_ values is passed in 'type'. The function must
+         * return true if the skb is accepted for delivery.
+         */
+        bool (*rxtstamp)(struct phy_device *dev, struct sk_buff *skb, int type);
+
+        /*
+         * Requests a Tx timestamp for 'skb'. The phy driver promises
+         * to deliver it to the socket's error queue as soon as a
+         * timestamp becomes available. One of the PTP_CLASS_ values
+         * is passed in 'type'.
+         */
+        void (*txtstamp)(struct phy_device *dev, struct sk_buff *skb, int type);
+
         struct device_driver driver;
 };
 #define to_phy_driver(d) container_of(d, struct phy_driver, driver)
diff --git a/include/linux/skbuff.h b/include/linux/skbuff.h
index a1b0400c8d86..f5aa87e1e0c8 100644
--- a/include/linux/skbuff.h
+++ b/include/linux/skbuff.h
@@ -1933,6 +1933,36 @@ static inline ktime_t net_invalid_timestamp(void)
         return ktime_set(0, 0);
 }
 
+extern void skb_timestamping_init(void);
+
+#ifdef CONFIG_NETWORK_PHY_TIMESTAMPING
+
+extern void skb_clone_tx_timestamp(struct sk_buff *skb);
+extern bool skb_defer_rx_timestamp(struct sk_buff *skb);
+
+#else /* CONFIG_NETWORK_PHY_TIMESTAMPING */
+
+static inline void skb_clone_tx_timestamp(struct sk_buff *skb)
+{
+}
+
+static inline bool skb_defer_rx_timestamp(struct sk_buff *skb)
+{
+        return false;
+}
+
+#endif /* !CONFIG_NETWORK_PHY_TIMESTAMPING */
+
+/**
+ * skb_complete_tx_timestamp() - deliver cloned skb with tx timestamps
+ *
+ * @skb: clone of the the original outgoing packet
+ * @hwtstamps: hardware time stamps
+ *
+ */
+void skb_complete_tx_timestamp(struct sk_buff *skb,
+                               struct skb_shared_hwtstamps *hwtstamps);
+
 /**
  * skb_tstamp_tx - queue clone of skb with send time stamps
  * @orig_skb:   the original outgoing packet
@@ -1965,6 +1995,7 @@ static inline void sw_tx_timestamp(struct sk_buff *skb)
  */
 static inline void skb_tx_timestamp(struct sk_buff *skb)
 {
+        skb_clone_tx_timestamp(skb);
         sw_tx_timestamp(skb);
 }
 
diff --git a/net/Kconfig b/net/Kconfig
index 0d68b40fc0e6..b3250944cde9 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -86,6 +86,16 @@ config NETWORK_SECMARK
           to nfmark, but designated for security purposes.
           If you are unsure how to answer this question, answer N.
 
+config NETWORK_PHY_TIMESTAMPING
+        bool "Timestamping in PHY devices"
+        depends on EXPERIMENTAL
+        help
+          This allows timestamping of network packets by PHYs with
+          hardware timestamping capabilities. This option adds some
+          overhead in the transmit and receive paths.
+
+          If you are unsure how to answer this question, answer N.
+
 menuconfig NETFILTER
         bool "Network packet filtering framework (Netfilter)"
         ---help---
diff --git a/net/core/Makefile b/net/core/Makefile
index 51c3eec850ef..8a04dd22cf77 100644
--- a/net/core/Makefile
+++ b/net/core/Makefile
@@ -18,4 +18,4 @@ obj-$(CONFIG_NET_DMA) += user_dma.o
 obj-$(CONFIG_FIB_RULES) += fib_rules.o
 obj-$(CONFIG_TRACEPOINTS) += net-traces.o
 obj-$(CONFIG_NET_DROP_MONITOR) += drop_monitor.o
-
+obj-$(CONFIG_NETWORK_PHY_TIMESTAMPING) += timestamping.o
diff --git a/net/core/dev.c b/net/core/dev.c
index e2b9fa2c917e..1c002c7ef5d5 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -2957,6 +2957,9 @@ int netif_receive_skb(struct sk_buff *skb)
         if (netdev_tstamp_prequeue)
                 net_timestamp_check(skb);
 
+        if (skb_defer_rx_timestamp(skb))
+                return NET_RX_SUCCESS;
+
 #ifdef CONFIG_RPS
         {
                 struct rps_dev_flow voidflow, *rflow = &voidflow;
diff --git a/net/core/timestamping.c b/net/core/timestamping.c
new file mode 100644
index 000000000000..0ae6c22da85b
--- /dev/null
+++ b/net/core/timestamping.c
@@ -0,0 +1,126 @@
+/*
+ * PTP 1588 clock support - support for timestamping in PHY devices
+ *
+ * Copyright (C) 2010 OMICRON electronics GmbH
+ *
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/errqueue.h>
+#include <linux/phy.h>
+#include <linux/ptp_classify.h>
+#include <linux/skbuff.h>
+
+static struct sock_filter ptp_filter[] = {
+        PTP_FILTER
+};
+
+static unsigned int classify(struct sk_buff *skb)
+{
+        if (likely(skb->dev &&
+                   skb->dev->phydev &&
+                   skb->dev->phydev->drv))
+                return sk_run_filter(skb, ptp_filter, ARRAY_SIZE(ptp_filter));
+        else
+                return PTP_CLASS_NONE;
+}
+
+void skb_clone_tx_timestamp(struct sk_buff *skb)
+{
+        struct phy_device *phydev;
+        struct sk_buff *clone;
+        struct sock *sk = skb->sk;
+        unsigned int type;
+
+        if (!sk)
+                return;
+
+        type = classify(skb);
+
+        switch (type) {
+        case PTP_CLASS_V1_IPV4:
+        case PTP_CLASS_V1_IPV6:
+        case PTP_CLASS_V2_IPV4:
+        case PTP_CLASS_V2_IPV6:
+        case PTP_CLASS_V2_L2:
+        case PTP_CLASS_V2_VLAN:
+                phydev = skb->dev->phydev;
+                if (likely(phydev->drv->txtstamp)) {
+                        clone = skb_clone(skb, GFP_ATOMIC);
+                        if (!clone)
+                                return;
+                        clone->sk = sk;
+                        phydev->drv->txtstamp(phydev, clone, type);
+                }
+                break;
+        default:
+                break;
+        }
+}
+
+void skb_complete_tx_timestamp(struct sk_buff *skb,
+                               struct skb_shared_hwtstamps *hwtstamps)
+{
+        struct sock *sk = skb->sk;
+        struct sock_exterr_skb *serr;
+        int err;
+
+        if (!hwtstamps)
+                return;
+
+        *skb_hwtstamps(skb) = *hwtstamps;
+        serr = SKB_EXT_ERR(skb);
+        memset(serr, 0, sizeof(*serr));
+        serr->ee.ee_errno = ENOMSG;
+        serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
+        skb->sk = NULL;
+        err = sock_queue_err_skb(sk, skb);
+        if (err)
+                kfree_skb(skb);
+}
+EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp);
+
+bool skb_defer_rx_timestamp(struct sk_buff *skb)
+{
+        struct phy_device *phydev;
+        unsigned int type;
+
+        skb_push(skb, ETH_HLEN);
+
+        type = classify(skb);
+
+        skb_pull(skb, ETH_HLEN);
+
+        switch (type) {
+        case PTP_CLASS_V1_IPV4:
+        case PTP_CLASS_V1_IPV6:
+        case PTP_CLASS_V2_IPV4:
+        case PTP_CLASS_V2_IPV6:
+        case PTP_CLASS_V2_L2:
+        case PTP_CLASS_V2_VLAN:
+                phydev = skb->dev->phydev;
+                if (likely(phydev->drv->rxtstamp))
+                        return phydev->drv->rxtstamp(phydev, skb, type);
+                break;
+        default:
+                break;
+        }
+
+        return false;
+}
+
+void __init skb_timestamping_init(void)
+{
+        BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter)));
+}
diff --git a/net/socket.c b/net/socket.c
index 6fe484122a44..2270b941bcc7 100644
--- a/net/socket.c
+++ b/net/socket.c
@@ -2394,6 +2394,10 @@ static int __init sock_init(void)
         netfilter_init();
 #endif
 
+#ifdef CONFIG_NETWORK_PHY_TIMESTAMPING
+        skb_timestamping_init();
+#endif
+
         return 0;
 }