1 files changed, 64 insertions, 20 deletions

diff --git a/net/socket.c b/net/socket.c
index 0f75746ab06e..af0205ff56f2 100644
--- a/net/socket.c
+++ b/net/socket.c
@@ -545,6 +545,18 @@ void sock_release(struct socket *sock)
         sock->file = NULL;
 }
 
+int sock_tx_timestamp(struct msghdr *msg, struct sock *sk,
+                      union skb_shared_tx *shtx)
+{
+        shtx->flags = 0;
+        if (sock_flag(sk, SOCK_TIMESTAMPING_TX_HARDWARE))
+                shtx->hardware = 1;
+        if (sock_flag(sk, SOCK_TIMESTAMPING_TX_SOFTWARE))
+                shtx->software = 1;
+        return 0;
+}
+EXPORT_SYMBOL(sock_tx_timestamp);
+
 static inline int __sock_sendmsg(struct kiocb *iocb, struct socket *sock,
                                  struct msghdr *msg, size_t size)
 {
@@ -595,33 +607,65 @@ int kernel_sendmsg(struct socket *sock, struct msghdr *msg,
         return result;
 }
 
+static int ktime2ts(ktime_t kt, struct timespec *ts)
+{
+        if (kt.tv64) {
+                *ts = ktime_to_timespec(kt);
+                return 1;
+        } else {
+                return 0;
+        }
+}
+
 /*
  * called from sock_recv_timestamp() if sock_flag(sk, SOCK_RCVTSTAMP)
  */
 void __sock_recv_timestamp(struct msghdr *msg, struct sock *sk,
         struct sk_buff *skb)
 {
-        ktime_t kt = skb->tstamp;
-
-        if (!sock_flag(sk, SOCK_RCVTSTAMPNS)) {
-                struct timeval tv;
-                /* Race occurred between timestamp enabling and packet
-                   receiving.  Fill in the current time for now. */
-                if (kt.tv64 == 0)
-                        kt = ktime_get_real();
-                skb->tstamp = kt;
-                tv = ktime_to_timeval(kt);
-                put_cmsg(msg, SOL_SOCKET, SCM_TIMESTAMP, sizeof(tv), &tv);
-        } else {
-                struct timespec ts;
-                /* Race occurred between timestamp enabling and packet
-                   receiving.  Fill in the current time for now. */
-                if (kt.tv64 == 0)
-                        kt = ktime_get_real();
-                skb->tstamp = kt;
-                ts = ktime_to_timespec(kt);
-                put_cmsg(msg, SOL_SOCKET, SCM_TIMESTAMPNS, sizeof(ts), &ts);
+        int need_software_tstamp = sock_flag(sk, SOCK_RCVTSTAMP);
+        struct timespec ts[3];
+        int empty = 1;
+        struct skb_shared_hwtstamps *shhwtstamps =
+                skb_hwtstamps(skb);
+
+        /* Race occurred between timestamp enabling and packet
+           receiving.  Fill in the current time for now. */
+        if (need_software_tstamp && skb->tstamp.tv64 == 0)
+                __net_timestamp(skb);
+
+        if (need_software_tstamp) {
+                if (!sock_flag(sk, SOCK_RCVTSTAMPNS)) {
+                        struct timeval tv;
+                        skb_get_timestamp(skb, &tv);
+                        put_cmsg(msg, SOL_SOCKET, SCM_TIMESTAMP,
+                                 sizeof(tv), &tv);
+                } else {
+                        struct timespec ts;
+                        skb_get_timestampns(skb, &ts);
+                        put_cmsg(msg, SOL_SOCKET, SCM_TIMESTAMPNS,
+                                 sizeof(ts), &ts);
+                }
+        }
+
+
+        memset(ts, 0, sizeof(ts));
+        if (skb->tstamp.tv64 &&
+            sock_flag(sk, SOCK_TIMESTAMPING_SOFTWARE)) {
+                skb_get_timestampns(skb, ts + 0);
+                empty = 0;
+        }
+        if (shhwtstamps) {
+                if (sock_flag(sk, SOCK_TIMESTAMPING_SYS_HARDWARE) &&
+                    ktime2ts(shhwtstamps->syststamp, ts + 1))
+                        empty = 0;
+                if (sock_flag(sk, SOCK_TIMESTAMPING_RAW_HARDWARE) &&
+                    ktime2ts(shhwtstamps->hwtstamp, ts + 2))
+                        empty = 0;
         }
+        if (!empty)
+                put_cmsg(msg, SOL_SOCKET,
+                         SCM_TIMESTAMPING, sizeof(ts), &ts);
 }
 
 EXPORT_SYMBOL_GPL(__sock_recv_timestamp);