packet: support for TX time stamps on RAW sockets

Enable the SO_TIMESTAMPING socket infrastructure for raw packet sockets.
We introduce PACKET_TX_TIMESTAMP for the control message cmsg_type.

Similar support for UDP and CAN sockets was added in commit
51f31cabe3ce5345b51e4a4f82138b38c4d5dc91

Signed-off-by: Richard Cochran <richard.cochran@omicron.at>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/include/linux/if_packet.h b/include/linux/if_packet.h
index aa57a5f..6ac23ef 100644 (file)
--- a/include/linux/if_packet.h
+++ b/include/linux/if_packet.h
@@ -47,6 +47,7 @@ struct sockaddr_ll {
 #define PACKET_TX_RING                 13
 #define PACKET_LOSS                    14
 #define PACKET_VNET_HDR                        15
+#define PACKET_TX_TIMESTAMP            16
 
 struct tpacket_stats {
        unsigned int    tp_packets;

diff --git a/net/packet/af_packet.c b/net/packet/af_packet.c
index d7d0310..f162d59 100644 (file)
--- a/net/packet/af_packet.c
+++ b/net/packet/af_packet.c
@@ -82,6 +82,7 @@
 #include <linux/mutex.h>
 #include <linux/if_vlan.h>
 #include <linux/virtio_net.h>
+#include <linux/errqueue.h>
 
 #ifdef CONFIG_INET
 #include <net/inet_common.h>
@@ -315,6 +316,8 @@ static inline struct packet_sock *pkt_sk(struct sock *sk)
 
 static void packet_sock_destruct(struct sock *sk)
 {
+       skb_queue_purge(&sk->sk_error_queue);
+
        WARN_ON(atomic_read(&sk->sk_rmem_alloc));
        WARN_ON(atomic_read(&sk->sk_wmem_alloc));
 
@@ -483,6 +486,9 @@ retry:
        skb->dev = dev;
        skb->priority = sk->sk_priority;
        skb->mark = sk->sk_mark;
+       err = sock_tx_timestamp(msg, sk, skb_tx(skb));
+       if (err < 0)
+               goto out_unlock;
 
        dev_queue_xmit(skb);
        rcu_read_unlock();
@@ -1188,6 +1194,9 @@ static int packet_snd(struct socket *sock,
        err = skb_copy_datagram_from_iovec(skb, offset, msg->msg_iov, 0, len);
        if (err)
                goto out_free;
+       err = sock_tx_timestamp(msg, sk, skb_tx(skb));
+       if (err < 0)
+               goto out_free;
 
        skb->protocol = proto;
        skb->dev = dev;
@@ -1487,6 +1496,51 @@ out:
        return err;
 }
 
+static int packet_recv_error(struct sock *sk, struct msghdr *msg, int len)
+{
+       struct sock_exterr_skb *serr;
+       struct sk_buff *skb, *skb2;
+       int copied, err;
+
+       err = -EAGAIN;
+       skb = skb_dequeue(&sk->sk_error_queue);
+       if (skb == NULL)
+               goto out;
+
+       copied = skb->len;
+       if (copied > len) {
+               msg->msg_flags |= MSG_TRUNC;
+               copied = len;
+       }
+       err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
+       if (err)
+               goto out_free_skb;
+
+       sock_recv_timestamp(msg, sk, skb);
+
+       serr = SKB_EXT_ERR(skb);
+       put_cmsg(msg, SOL_PACKET, PACKET_TX_TIMESTAMP,
+                sizeof(serr->ee), &serr->ee);
+
+       msg->msg_flags |= MSG_ERRQUEUE;
+       err = copied;
+
+       /* Reset and regenerate socket error */
+       spin_lock_bh(&sk->sk_error_queue.lock);
+       sk->sk_err = 0;
+       if ((skb2 = skb_peek(&sk->sk_error_queue)) != NULL) {
+               sk->sk_err = SKB_EXT_ERR(skb2)->ee.ee_errno;
+               spin_unlock_bh(&sk->sk_error_queue.lock);
+               sk->sk_error_report(sk);
+       } else
+               spin_unlock_bh(&sk->sk_error_queue.lock);
+
+out_free_skb:
+       kfree_skb(skb);
+out:
+       return err;
+}
+
 /*
  *     Pull a packet from our receive queue and hand it to the user.
  *     If necessary we block.
@@ -1502,7 +1556,7 @@ static int packet_recvmsg(struct kiocb *iocb, struct socket *sock,
        int vnet_hdr_len = 0;
 
        err = -EINVAL;
-       if (flags & ~(MSG_PEEK|MSG_DONTWAIT|MSG_TRUNC|MSG_CMSG_COMPAT))
+       if (flags & ~(MSG_PEEK|MSG_DONTWAIT|MSG_TRUNC|MSG_CMSG_COMPAT|MSG_ERRQUEUE))
                goto out;
 
 #if 0
@@ -1511,6 +1565,11 @@ static int packet_recvmsg(struct kiocb *iocb, struct socket *sock,
                return -ENODEV;
 #endif
 
+       if (flags & MSG_ERRQUEUE) {
+               err = packet_recv_error(sk, msg, len);
+               goto out;
+       }
+
        /*
         *      Call the generic datagram receiver. This handles all sorts
         *      of horrible races and re-entrancy so we can forget about it