Bluetooth: Add common code for stream-oriented recvmsg()

[pandora-kernel.git] / net / bluetooth / af_bluetooth.c

diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index ed0f22f..c4cf3f5 100644 (file)
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -265,6 +265,115 @@ int bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 }
 EXPORT_SYMBOL(bt_sock_recvmsg);
 
+static long bt_sock_data_wait(struct sock *sk, long timeo)
+{
+       DECLARE_WAITQUEUE(wait, current);
+
+       add_wait_queue(sk_sleep(sk), &wait);
+       for (;;) {
+               set_current_state(TASK_INTERRUPTIBLE);
+
+               if (!skb_queue_empty(&sk->sk_receive_queue))
+                       break;
+
+               if (sk->sk_err || (sk->sk_shutdown & RCV_SHUTDOWN))
+                       break;
+
+               if (signal_pending(current) || !timeo)
+                       break;
+
+               set_bit(SOCK_ASYNC_WAITDATA, &sk->sk_socket->flags);
+               release_sock(sk);
+               timeo = schedule_timeout(timeo);
+               lock_sock(sk);
+               clear_bit(SOCK_ASYNC_WAITDATA, &sk->sk_socket->flags);
+       }
+
+       __set_current_state(TASK_RUNNING);
+       remove_wait_queue(sk_sleep(sk), &wait);
+       return timeo;
+}
+
+int bt_sock_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
+                              struct msghdr *msg, size_t size, int flags)
+{
+       struct sock *sk = sock->sk;
+       int err = 0;
+       size_t target, copied = 0;
+       long timeo;
+
+       if (flags & MSG_OOB)
+               return -EOPNOTSUPP;
+
+       msg->msg_namelen = 0;
+
+       BT_DBG("sk %p size %zu", sk, size);
+
+       lock_sock(sk);
+
+       target = sock_rcvlowat(sk, flags & MSG_WAITALL, size);
+       timeo  = sock_rcvtimeo(sk, flags & MSG_DONTWAIT);
+
+       do {
+               struct sk_buff *skb;
+               int chunk;
+
+               skb = skb_dequeue(&sk->sk_receive_queue);
+               if (!skb) {
+                       if (copied >= target)
+                               break;
+
+                       if ((err = sock_error(sk)) != 0)
+                               break;
+                       if (sk->sk_shutdown & RCV_SHUTDOWN)
+                               break;
+
+                       err = -EAGAIN;
+                       if (!timeo)
+                               break;
+
+                       timeo = bt_sock_data_wait(sk, timeo);
+
+                       if (signal_pending(current)) {
+                               err = sock_intr_errno(timeo);
+                               goto out;
+                       }
+                       continue;
+               }
+
+               chunk = min_t(unsigned int, skb->len, size);
+               if (memcpy_toiovec(msg->msg_iov, skb->data, chunk)) {
+                       skb_queue_head(&sk->sk_receive_queue, skb);
+                       if (!copied)
+                               copied = -EFAULT;
+                       break;
+               }
+               copied += chunk;
+               size   -= chunk;
+
+               sock_recv_ts_and_drops(msg, sk, skb);
+
+               if (!(flags & MSG_PEEK)) {
+                       skb_pull(skb, chunk);
+                       if (skb->len) {
+                               skb_queue_head(&sk->sk_receive_queue, skb);
+                               break;
+                       }
+                       kfree_skb(skb);
+
+               } else {
+                       /* put message back and return */
+                       skb_queue_head(&sk->sk_receive_queue, skb);
+                       break;
+               }
+       } while (size);
+
+out:
+       release_sock(sk);
+       return copied ? : err;
+}
+EXPORT_SYMBOL(bt_sock_stream_recvmsg);
+
 static inline unsigned int bt_accept_poll(struct sock *parent)
 {
        struct list_head *p, *n;