packet: move hw/sw timestamp extraction into a small helper

[karo-tx-linux.git] / net / packet / af_packet.c

diff --git a/net/packet/af_packet.c b/net/packet/af_packet.c
index e566b793f07c3ac7aff12178bcda82936aa5209b..7e387ff64465c934075993f77c4def101d2160de 100644 (file)
--- a/net/packet/af_packet.c
+++ b/net/packet/af_packet.c
@@ -1657,6 +1657,26 @@ drop:
        return 0;
 }
 
+static void tpacket_get_timestamp(struct sk_buff *skb, struct timespec *ts,
+                                 unsigned int flags)
+{
+       struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
+
+       if (shhwtstamps) {
+               if ((flags & SOF_TIMESTAMPING_SYS_HARDWARE) &&
+                   ktime_to_timespec_cond(shhwtstamps->syststamp, ts))
+                       return;
+               if ((flags & SOF_TIMESTAMPING_RAW_HARDWARE) &&
+                   ktime_to_timespec_cond(shhwtstamps->hwtstamp, ts))
+                       return;
+       }
+
+       if (ktime_to_timespec_cond(skb->tstamp, ts))
+               return;
+
+       getnstimeofday(ts);
+}
+
 static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
                       struct packet_type *pt, struct net_device *orig_dev)
 {
@@ -1670,9 +1690,7 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
        unsigned long status = TP_STATUS_USER;
        unsigned short macoff, netoff, hdrlen;
        struct sk_buff *copy_skb = NULL;
-       struct timeval tv;
        struct timespec ts;
-       struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
 
        if (skb->pkt_type == PACKET_LOOPBACK)
                goto drop;
@@ -1755,6 +1773,7 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
        spin_unlock(&sk->sk_receive_queue.lock);
 
        skb_copy_bits(skb, 0, h.raw + macoff, snaplen);
+       tpacket_get_timestamp(skb, &ts, po->tp_tstamp);
 
        switch (po->tp_version) {
        case TPACKET_V1:
@@ -1762,18 +1781,8 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
                h.h1->tp_snaplen = snaplen;
                h.h1->tp_mac = macoff;
                h.h1->tp_net = netoff;
-               if ((po->tp_tstamp & SOF_TIMESTAMPING_SYS_HARDWARE)
-                               && shhwtstamps->syststamp.tv64)
-                       tv = ktime_to_timeval(shhwtstamps->syststamp);
-               else if ((po->tp_tstamp & SOF_TIMESTAMPING_RAW_HARDWARE)
-                               && shhwtstamps->hwtstamp.tv64)
-                       tv = ktime_to_timeval(shhwtstamps->hwtstamp);
-               else if (skb->tstamp.tv64)
-                       tv = ktime_to_timeval(skb->tstamp);
-               else
-                       do_gettimeofday(&tv);
-               h.h1->tp_sec = tv.tv_sec;
-               h.h1->tp_usec = tv.tv_usec;
+               h.h1->tp_sec = ts.tv_sec;
+               h.h1->tp_usec = ts.tv_nsec / NSEC_PER_USEC;
                hdrlen = sizeof(*h.h1);
                break;
        case TPACKET_V2:
@@ -1781,16 +1790,6 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
                h.h2->tp_snaplen = snaplen;
                h.h2->tp_mac = macoff;
                h.h2->tp_net = netoff;
-               if ((po->tp_tstamp & SOF_TIMESTAMPING_SYS_HARDWARE)
-                               && shhwtstamps->syststamp.tv64)
-                       ts = ktime_to_timespec(shhwtstamps->syststamp);
-               else if ((po->tp_tstamp & SOF_TIMESTAMPING_RAW_HARDWARE)
-                               && shhwtstamps->hwtstamp.tv64)
-                       ts = ktime_to_timespec(shhwtstamps->hwtstamp);
-               else if (skb->tstamp.tv64)
-                       ts = ktime_to_timespec(skb->tstamp);
-               else
-                       getnstimeofday(&ts);
                h.h2->tp_sec = ts.tv_sec;
                h.h2->tp_nsec = ts.tv_nsec;
                if (vlan_tx_tag_present(skb)) {
@@ -1811,16 +1810,6 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
                h.h3->tp_snaplen = snaplen;
                h.h3->tp_mac = macoff;
                h.h3->tp_net = netoff;
-               if ((po->tp_tstamp & SOF_TIMESTAMPING_SYS_HARDWARE)
-                               && shhwtstamps->syststamp.tv64)
-                       ts = ktime_to_timespec(shhwtstamps->syststamp);
-               else if ((po->tp_tstamp & SOF_TIMESTAMPING_RAW_HARDWARE)
-                               && shhwtstamps->hwtstamp.tv64)
-                       ts = ktime_to_timespec(shhwtstamps->hwtstamp);
-               else if (skb->tstamp.tv64)
-                       ts = ktime_to_timespec(skb->tstamp);
-               else
-                       getnstimeofday(&ts);
                h.h3->tp_sec  = ts.tv_sec;
                h.h3->tp_nsec = ts.tv_nsec;
                hdrlen = sizeof(*h.h3);