5 files changed, 76 insertions, 21 deletions

diff --git a/net/core/datagram.c b/net/core/datagram.c
index 68bbf9f65cb0..d3cf12f62c8f 100644
--- a/net/core/datagram.c
+++ b/net/core/datagram.c
@@ -132,6 +132,8 @@ out_noerr:
  *      __skb_recv_datagram - Receive a datagram skbuff
  *      @sk: socket
  *      @flags: MSG_ flags
+ *      @off: an offset in bytes to peek skb from. Returns an offset
+ *            within an skb where data actually starts
  *      @peeked: returns non-zero if this packet has been seen before
  *      @err: error code returned
  *
@@ -158,7 +160,7 @@ out_noerr:
  *      the standard around please.
  */
 struct sk_buff *__skb_recv_datagram(struct sock *sk, unsigned flags,
-                                    int *peeked, int *err)
+                                    int *peeked, int *off, int *err)
 {
         struct sk_buff *skb;
         long timeo;
@@ -180,21 +182,25 @@ struct sk_buff *__skb_recv_datagram(struct sock *sk, unsigned flags,
                  * However, this function was correct in any case. 8)
                  */
                 unsigned long cpu_flags;
+                struct sk_buff_head *queue = &sk->sk_receive_queue;
 
-                spin_lock_irqsave(&sk->sk_receive_queue.lock, cpu_flags);
-                skb = skb_peek(&sk->sk_receive_queue);
-                if (skb) {
+                spin_lock_irqsave(&queue->lock, cpu_flags);
+                skb_queue_walk(queue, skb) {
                         *peeked = skb->peeked;
                         if (flags & MSG_PEEK) {
+                                if (*off >= skb->len) {
+                                        *off -= skb->len;
+                                        continue;
+                                }
                                 skb->peeked = 1;
                                 atomic_inc(&skb->users);
                         } else
-                                __skb_unlink(skb, &sk->sk_receive_queue);
-                }
-                spin_unlock_irqrestore(&sk->sk_receive_queue.lock, cpu_flags);
+                                __skb_unlink(skb, queue);
 
-                if (skb)
+                        spin_unlock_irqrestore(&queue->lock, cpu_flags);
                         return skb;
+                }
+                spin_unlock_irqrestore(&queue->lock, cpu_flags);
 
                 /* User doesn't want to wait */
                 error = -EAGAIN;
@@ -214,10 +220,10 @@ EXPORT_SYMBOL(__skb_recv_datagram);
 struct sk_buff *skb_recv_datagram(struct sock *sk, unsigned flags,
                                   int noblock, int *err)
 {
-        int peeked;
+        int peeked, off = 0;
 
         return __skb_recv_datagram(sk, flags | (noblock ? MSG_DONTWAIT : 0),
-                                   &peeked, err);
+                                   &peeked, &off, err);
 }
 EXPORT_SYMBOL(skb_recv_datagram);
 
diff --git a/net/core/sock.c b/net/core/sock.c
index 02f8dfe320b7..19942d4bb6e6 100644
--- a/net/core/sock.c
+++ b/net/core/sock.c
@@ -793,6 +793,12 @@ set_rcvbuf:
                 sock_valbool_flag(sk, SOCK_WIFI_STATUS, valbool);
                 break;
 
+        case SO_PEEK_OFF:
+                if (sock->ops->set_peek_off)
+                        sock->ops->set_peek_off(sk, val);
+                else
+                        ret = -EOPNOTSUPP;
+                break;
         default:
                 ret = -ENOPROTOOPT;
                 break;
@@ -1018,6 +1024,12 @@ int sock_getsockopt(struct socket *sock, int level, int optname,
                 v.val = !!sock_flag(sk, SOCK_WIFI_STATUS);
                 break;
 
+        case SO_PEEK_OFF:
+                if (!sock->ops->set_peek_off)
+                        return -EOPNOTSUPP;
+
+                v.val = sk->sk_peek_off;
+                break;
         default:
                 return -ENOPROTOOPT;
         }
@@ -2092,6 +2104,7 @@ void sock_init_data(struct socket *sock, struct sock *sk)
 
         sk->sk_sndmsg_page      =       NULL;
         sk->sk_sndmsg_off       =       0;
+        sk->sk_peek_off         =       -1;
 
         sk->sk_peer_pid         =       NULL;
         sk->sk_peer_cred        =       NULL;
diff --git a/net/ipv4/udp.c b/net/ipv4/udp.c
index cd99f1a0f59f..7c41ab84e72e 100644
--- a/net/ipv4/udp.c
+++ b/net/ipv4/udp.c
@@ -1167,7 +1167,7 @@ int udp_recvmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
         struct sockaddr_in *sin = (struct sockaddr_in *)msg->msg_name;
         struct sk_buff *skb;
         unsigned int ulen, copied;
-        int peeked;
+        int peeked, off = 0;
         int err;
         int is_udplite = IS_UDPLITE(sk);
         bool slow;
@@ -1183,7 +1183,7 @@ int udp_recvmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
 
 try_again:
         skb = __skb_recv_datagram(sk, flags | (noblock ? MSG_DONTWAIT : 0),
-                                  &peeked, &err);
+                                  &peeked, &off, &err);
         if (!skb)
                 goto out;
 
diff --git a/net/ipv6/udp.c b/net/ipv6/udp.c
index 8aebf8f90436..37b0699e95e5 100644
--- a/net/ipv6/udp.c
+++ b/net/ipv6/udp.c
@@ -342,7 +342,7 @@ int udpv6_recvmsg(struct kiocb *iocb, struct sock *sk,
         struct inet_sock *inet = inet_sk(sk);
         struct sk_buff *skb;
         unsigned int ulen, copied;
-        int peeked;
+        int peeked, off = 0;
         int err;
         int is_udplite = IS_UDPLITE(sk);
         int is_udp4;
@@ -359,7 +359,7 @@ int udpv6_recvmsg(struct kiocb *iocb, struct sock *sk,
 
 try_again:
         skb = __skb_recv_datagram(sk, flags | (noblock ? MSG_DONTWAIT : 0),
-                                  &peeked, &err);
+                                  &peeked, &off, &err);
         if (!skb)
                 goto out;
 
diff --git a/net/unix/af_unix.c b/net/unix/af_unix.c
index 85d3bb7490aa..0be4d24f6ae8 100644
--- a/net/unix/af_unix.c
+++ b/net/unix/af_unix.c
@@ -530,6 +530,16 @@ static int unix_seqpacket_sendmsg(struct kiocb *, struct socket *,
 static int unix_seqpacket_recvmsg(struct kiocb *, struct socket *,
                                   struct msghdr *, size_t, int);
 
+static void unix_set_peek_off(struct sock *sk, int val)
+{
+        struct unix_sock *u = unix_sk(sk);
+
+        mutex_lock(&u->readlock);
+        sk->sk_peek_off = val;
+        mutex_unlock(&u->readlock);
+}
+
+
 static const struct proto_ops unix_stream_ops = {
         .family =       PF_UNIX,
         .owner =        THIS_MODULE,
@@ -549,6 +559,7 @@ static const struct proto_ops unix_stream_ops = {
         .recvmsg =      unix_stream_recvmsg,
         .mmap =         sock_no_mmap,
         .sendpage =     sock_no_sendpage,
+        .set_peek_off = unix_set_peek_off,
 };
 
 static const struct proto_ops unix_dgram_ops = {
@@ -570,6 +581,7 @@ static const struct proto_ops unix_dgram_ops = {
         .recvmsg =      unix_dgram_recvmsg,
         .mmap =         sock_no_mmap,
         .sendpage =     sock_no_sendpage,
+        .set_peek_off = unix_set_peek_off,
 };
 
 static const struct proto_ops unix_seqpacket_ops = {
@@ -591,6 +603,7 @@ static const struct proto_ops unix_seqpacket_ops = {
         .recvmsg =      unix_seqpacket_recvmsg,
         .mmap =         sock_no_mmap,
         .sendpage =     sock_no_sendpage,
+        .set_peek_off = unix_set_peek_off,
 };
 
 static struct proto unix_proto = {
@@ -1756,6 +1769,7 @@ static int unix_dgram_recvmsg(struct kiocb *iocb, struct socket *sock,
         int noblock = flags & MSG_DONTWAIT;
         struct sk_buff *skb;
         int err;
+        int peeked, skip;
 
         err = -EOPNOTSUPP;
         if (flags&MSG_OOB)
@@ -1769,7 +1783,9 @@ static int unix_dgram_recvmsg(struct kiocb *iocb, struct socket *sock,
                 goto out;
         }
 
-        skb = skb_recv_datagram(sk, flags, noblock, &err);
+        skip = sk_peek_offset(sk, flags);
+
+        skb = __skb_recv_datagram(sk, flags, &peeked, &skip, &err);
         if (!skb) {
                 unix_state_lock(sk);
                 /* Signal EOF on disconnected non-blocking SEQPACKET socket. */
@@ -1786,12 +1802,12 @@ static int unix_dgram_recvmsg(struct kiocb *iocb, struct socket *sock,
         if (msg->msg_name)
                 unix_copy_addr(msg, skb->sk);
 
-        if (size > skb->len)
-                size = skb->len;
-        else if (size < skb->len)
+        if (size > skb->len - skip)
+                size = skb->len - skip;
+        else if (size < skb->len - skip)
                 msg->msg_flags |= MSG_TRUNC;
 
-        err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, size);
+        err = skb_copy_datagram_iovec(skb, skip, msg->msg_iov, size);
         if (err)
                 goto out_free;
 
@@ -1808,6 +1824,8 @@ static int unix_dgram_recvmsg(struct kiocb *iocb, struct socket *sock,
         if (!(flags & MSG_PEEK)) {
                 if (UNIXCB(skb).fp)
                         unix_detach_fds(siocb->scm, skb);
+
+                sk_peek_offset_bwd(sk, skb->len);
         } else {
                 /* It is questionable: on PEEK we could:
                    - do not return fds - good, but too simple 8)
@@ -1821,6 +1839,9 @@ static int unix_dgram_recvmsg(struct kiocb *iocb, struct socket *sock,
                    clearly however!
 
                 */
+
+                sk_peek_offset_fwd(sk, size);
+
                 if (UNIXCB(skb).fp)
                         siocb->scm->fp = scm_fp_dup(UNIXCB(skb).fp);
         }
@@ -1884,6 +1905,7 @@ static int unix_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
         int target;
         int err = 0;
         long timeo;
+        int skip;
 
         err = -EINVAL;
         if (sk->sk_state != TCP_ESTABLISHED)
@@ -1913,12 +1935,15 @@ static int unix_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
                 goto out;
         }
 
+        skip = sk_peek_offset(sk, flags);
+
         do {
                 int chunk;
                 struct sk_buff *skb;
 
                 unix_state_lock(sk);
                 skb = skb_peek(&sk->sk_receive_queue);
+again:
                 if (skb == NULL) {
                         unix_sk(sk)->recursion_level = 0;
                         if (copied >= target)
@@ -1953,6 +1978,13 @@ static int unix_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
                         unix_state_unlock(sk);
                         break;
                 }
+
+                if (skip >= skb->len) {
+                        skip -= skb->len;
+                        skb = skb_peek_next(skb, &sk->sk_receive_queue);
+                        goto again;
+                }
+
                 unix_state_unlock(sk);
 
                 if (check_creds) {
@@ -1972,8 +2004,8 @@ static int unix_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
                         sunaddr = NULL;
                 }
 
-                chunk = min_t(unsigned int, skb->len, size);
-                if (memcpy_toiovec(msg->msg_iov, skb->data, chunk)) {
+                chunk = min_t(unsigned int, skb->len - skip, size);
+                if (memcpy_toiovec(msg->msg_iov, skb->data + skip, chunk)) {
                         if (copied == 0)
                                 copied = -EFAULT;
                         break;
@@ -1985,6 +2017,8 @@ static int unix_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
                 if (!(flags & MSG_PEEK)) {
                         skb_pull(skb, chunk);
 
+                        sk_peek_offset_bwd(sk, chunk);
+
                         if (UNIXCB(skb).fp)
                                 unix_detach_fds(siocb->scm, skb);
 
@@ -2002,6 +2036,8 @@ static int unix_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
                         if (UNIXCB(skb).fp)
                                 siocb->scm->fp = scm_fp_dup(UNIXCB(skb).fp);
 
+                        sk_peek_offset_fwd(sk, chunk);
+
                         break;
                 }
         } while (size);