caif: Rewritten socket implementation

Changes:
 This is a complete re-write of the socket layer. Making the socket
 implementation more aligned with the other socket layers and using more
 of the support functions available in sock.c. Lots of code is copied
 from af_unix (and some from af_irda).
 Non-blocking mode should be working as well.

Signed-off-by: Sjur Braendeland <sjur.brandeland@stericsson.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 828 insertions, 967 deletions

diff --git a/net/caif/caif_socket.c b/net/caif/caif_socket.c
index d455375789f..c3a70c5c893 100644
--- a/net/caif/caif_socket.c
+++ b/net/caif/caif_socket.c
@@ -1,7 +1,6 @@
 /*
  * Copyright (C) ST-Ericsson AB 2010
  * Author:	Sjur Brendeland sjur.brandeland@stericsson.com
- *		Per Sigmond per.sigmond@stericsson.com
  * License terms: GNU General Public License (GPL) version 2
  */
 
@@ -16,91 +15,52 @@
 #include <linux/poll.h>
 #include <linux/tcp.h>
 #include <linux/uaccess.h>
-#include <asm/atomic.h>
-
+#include <linux/mutex.h>
+#include <linux/debugfs.h>
 #include <linux/caif/caif_socket.h>
+#include <asm/atomic.h>
+#include <net/sock.h>
+#include <net/tcp_states.h>
 #include <net/caif/caif_layer.h>
 #include <net/caif/caif_dev.h>
 #include <net/caif/cfpkt.h>
 
 MODULE_LICENSE("GPL");
+MODULE_ALIAS_NETPROTO(AF_CAIF);
+
+#define CAIF_DEF_SNDBUF (CAIF_MAX_PAYLOAD_SIZE*10)
+#define CAIF_DEF_RCVBUF (CAIF_MAX_PAYLOAD_SIZE*100)
+
+/*
+ * CAIF state is re-using the TCP socket states.
+ * caif_states stored in sk_state reflect the state as reported by
+ * the CAIF stack, while sk_socket->state is the state of the socket.
+ */
+enum caif_states {
+	CAIF_CONNECTED		= TCP_ESTABLISHED,
+	CAIF_CONNECTING	= TCP_SYN_SENT,
+	CAIF_DISCONNECTED	= TCP_CLOSE
+};
+
+#define TX_FLOW_ON_BIT	1
+#define RX_FLOW_ON_BIT	2
 
-#define CHNL_SKT_READ_QUEUE_HIGH 200
-#define CHNL_SKT_READ_QUEUE_LOW 100
-
-static int caif_sockbuf_size = 40000;
-static atomic_t caif_nr_socks = ATOMIC_INIT(0);
-
-#define CONN_STATE_OPEN_BIT	      1
-#define CONN_STATE_PENDING_BIT	      2
-#define CONN_STATE_PEND_DESTROY_BIT   3
-#define CONN_REMOTE_SHUTDOWN_BIT      4
-
-#define TX_FLOW_ON_BIT		      1
-#define RX_FLOW_ON_BIT		      2
-
-#define STATE_IS_OPEN(cf_sk) test_bit(CONN_STATE_OPEN_BIT,\
-				    (void *) &(cf_sk)->conn_state)
-#define STATE_IS_REMOTE_SHUTDOWN(cf_sk) test_bit(CONN_REMOTE_SHUTDOWN_BIT,\
-				    (void *) &(cf_sk)->conn_state)
-#define STATE_IS_PENDING(cf_sk) test_bit(CONN_STATE_PENDING_BIT,\
-				       (void *) &(cf_sk)->conn_state)
-#define STATE_IS_PENDING_DESTROY(cf_sk) test_bit(CONN_STATE_PEND_DESTROY_BIT,\
-				       (void *) &(cf_sk)->conn_state)
-
-#define SET_STATE_PENDING_DESTROY(cf_sk) set_bit(CONN_STATE_PEND_DESTROY_BIT,\
-				    (void *) &(cf_sk)->conn_state)
-#define SET_STATE_OPEN(cf_sk) set_bit(CONN_STATE_OPEN_BIT,\
-				    (void *) &(cf_sk)->conn_state)
-#define SET_STATE_CLOSED(cf_sk) clear_bit(CONN_STATE_OPEN_BIT,\
-					(void *) &(cf_sk)->conn_state)
-#define SET_PENDING_ON(cf_sk) set_bit(CONN_STATE_PENDING_BIT,\
-				    (void *) &(cf_sk)->conn_state)
-#define SET_PENDING_OFF(cf_sk) clear_bit(CONN_STATE_PENDING_BIT,\
-				       (void *) &(cf_sk)->conn_state)
-#define SET_REMOTE_SHUTDOWN(cf_sk) set_bit(CONN_REMOTE_SHUTDOWN_BIT,\
-				    (void *) &(cf_sk)->conn_state)
-
-#define SET_REMOTE_SHUTDOWN_OFF(dev) clear_bit(CONN_REMOTE_SHUTDOWN_BIT,\
-				    (void *) &(dev)->conn_state)
-#define RX_FLOW_IS_ON(cf_sk) test_bit(RX_FLOW_ON_BIT,\
-				    (void *) &(cf_sk)->flow_state)
-#define TX_FLOW_IS_ON(cf_sk) test_bit(TX_FLOW_ON_BIT,\
-				    (void *) &(cf_sk)->flow_state)
-
-#define SET_RX_FLOW_OFF(cf_sk) clear_bit(RX_FLOW_ON_BIT,\
-				       (void *) &(cf_sk)->flow_state)
-#define SET_RX_FLOW_ON(cf_sk) set_bit(RX_FLOW_ON_BIT,\
-				    (void *) &(cf_sk)->flow_state)
-#define SET_TX_FLOW_OFF(cf_sk) clear_bit(TX_FLOW_ON_BIT,\
-				       (void *) &(cf_sk)->flow_state)
-#define SET_TX_FLOW_ON(cf_sk) set_bit(TX_FLOW_ON_BIT,\
-				    (void *) &(cf_sk)->flow_state)
-
-#define SKT_READ_FLAG 0x01
-#define SKT_WRITE_FLAG 0x02
 static struct dentry *debugfsdir;
-#include <linux/debugfs.h>
 
 #ifdef CONFIG_DEBUG_FS
 struct debug_fs_counter {
-	atomic_t num_open;
-	atomic_t num_close;
-	atomic_t num_init;
-	atomic_t num_init_resp;
-	atomic_t num_init_fail_resp;
-	atomic_t num_deinit;
-	atomic_t num_deinit_resp;
+	atomic_t caif_nr_socks;
+	atomic_t num_connect_req;
+	atomic_t num_connect_resp;
+	atomic_t num_connect_fail_resp;
+	atomic_t num_disconnect;
 	atomic_t num_remote_shutdown_ind;
 	atomic_t num_tx_flow_off_ind;
 	atomic_t num_tx_flow_on_ind;
 	atomic_t num_rx_flow_off;
 	atomic_t num_rx_flow_on;
-	atomic_t skb_in_use;
-	atomic_t skb_alloc;
-	atomic_t skb_free;
 };
-static struct debug_fs_counter cnt;
+struct debug_fs_counter cnt;
 #define	dbfs_atomic_inc(v) atomic_inc(v)
 #define	dbfs_atomic_dec(v) atomic_dec(v)
 #else
@@ -108,624 +68,666 @@ static struct debug_fs_counter cnt;
 #define	dbfs_atomic_dec(v)
 #endif
 
-/* The AF_CAIF socket */
 struct caifsock {
-	/* NOTE: sk has to be the first member */
-	struct sock sk;
+	struct sock sk; /* must be first member */
 	struct cflayer layer;
-	char name[CAIF_LAYER_NAME_SZ];
-	u32 conn_state;
+	char name[CAIF_LAYER_NAME_SZ]; /* Used for debugging */
 	u32 flow_state;
-	struct cfpktq *pktq;
-	int file_mode;
 	struct caif_connect_request conn_req;
-	int read_queue_len;
-	/* protect updates of read_queue_len */
-	spinlock_t read_queue_len_lock;
+	struct mutex readlock;
 	struct dentry *debugfs_socket_dir;
 };
 
-static void drain_queue(struct caifsock *cf_sk);
+static int rx_flow_is_on(struct caifsock *cf_sk)
+{
+	return test_bit(RX_FLOW_ON_BIT,
+			(void *) &cf_sk->flow_state);
+}
+
+static int tx_flow_is_on(struct caifsock *cf_sk)
+{
+	return test_bit(TX_FLOW_ON_BIT,
+			(void *) &cf_sk->flow_state);
+}
 
-/* Packet Receive Callback function called from CAIF Stack */
-static int caif_sktrecv_cb(struct cflayer *layr, struct cfpkt *pkt)
+static void set_rx_flow_off(struct caifsock *cf_sk)
 {
-	struct caifsock *cf_sk;
-	int read_queue_high;
-	cf_sk = container_of(layr, struct caifsock, layer);
+	 clear_bit(RX_FLOW_ON_BIT,
+		 (void *) &cf_sk->flow_state);
+}
 
-	if (!STATE_IS_OPEN(cf_sk)) {
-		/*FIXME: This should be allowed finally!*/
-		pr_debug("CAIF: %s(): called after close request\n", __func__);
-		cfpkt_destroy(pkt);
-		return 0;
-	}
-	/* NOTE: This function may be called in Tasklet context! */
+static void set_rx_flow_on(struct caifsock *cf_sk)
+{
+	 set_bit(RX_FLOW_ON_BIT,
+			(void *) &cf_sk->flow_state);
+}
 
-	/* The queue has its own lock */
-	cfpkt_queue(cf_sk->pktq, pkt, 0);
+static void set_tx_flow_off(struct caifsock *cf_sk)
+{
+	 clear_bit(TX_FLOW_ON_BIT,
+		(void *) &cf_sk->flow_state);
+}
 
-	spin_lock(&cf_sk->read_queue_len_lock);
-	cf_sk->read_queue_len++;
+static void set_tx_flow_on(struct caifsock *cf_sk)
+{
+	 set_bit(TX_FLOW_ON_BIT,
+		(void *) &cf_sk->flow_state);
+}
 
-	read_queue_high = (cf_sk->read_queue_len > CHNL_SKT_READ_QUEUE_HIGH);
-	spin_unlock(&cf_sk->read_queue_len_lock);
+static void caif_read_lock(struct sock *sk)
+{
+	struct caifsock *cf_sk;
+	cf_sk = container_of(sk, struct caifsock, sk);
+	mutex_lock(&cf_sk->readlock);
+}
 
-	if (RX_FLOW_IS_ON(cf_sk) && read_queue_high) {
-		dbfs_atomic_inc(&cnt.num_rx_flow_off);
-		SET_RX_FLOW_OFF(cf_sk);
+static void caif_read_unlock(struct sock *sk)
+{
+	struct caifsock *cf_sk;
+	cf_sk = container_of(sk, struct caifsock, sk);
+	mutex_unlock(&cf_sk->readlock);
+}
 
-		/* Send flow off (NOTE: must not sleep) */
-		pr_debug("CAIF: %s():"
-			" sending flow OFF (queue len = %d)\n",
-			__func__,
-		     cf_sk->read_queue_len);
-		caif_assert(cf_sk->layer.dn);
-		caif_assert(cf_sk->layer.dn->ctrlcmd);
+int sk_rcvbuf_lowwater(struct caifsock *cf_sk)
+{
+	/* A quarter of full buffer is used a low water mark */
+	return cf_sk->sk.sk_rcvbuf / 4;
+}
 
-		(void) cf_sk->layer.dn->modemcmd(cf_sk->layer.dn,
-					       CAIF_MODEMCMD_FLOW_OFF_REQ);
-	}
+void caif_flow_ctrl(struct sock *sk, int mode)
+{
+	struct caifsock *cf_sk;
+	cf_sk = container_of(sk, struct caifsock, sk);
+	if (cf_sk->layer.dn)
+		cf_sk->layer.dn->modemcmd(cf_sk->layer.dn, mode);
+}
 
-	/* Signal reader that data is available. */
+/*
+ * Copied from sock.c:sock_queue_rcv_skb(), but changed so packets are
+ * not dropped, but CAIF is sending flow off instead.
+ */
+int caif_queue_rcv_skb(struct sock *sk, struct sk_buff *skb)
+{
+	int err;
+	int skb_len;
+	unsigned long flags;
+	struct sk_buff_head *list = &sk->sk_receive_queue;
+	struct caifsock *cf_sk = container_of(sk, struct caifsock, sk);
 
-	wake_up_interruptible(sk_sleep(&cf_sk->sk));
+	if (atomic_read(&sk->sk_rmem_alloc) + skb->truesize >=
+		(unsigned)sk->sk_rcvbuf && rx_flow_is_on(cf_sk)) {
+		trace_printk("CAIF: %s():"
+			" sending flow OFF (queue len = %d %d)\n",
+			__func__,
+			atomic_read(&cf_sk->sk.sk_rmem_alloc),
+			sk_rcvbuf_lowwater(cf_sk));
+		set_rx_flow_off(cf_sk);
+		if (cf_sk->layer.dn)
+			cf_sk->layer.dn->modemcmd(cf_sk->layer.dn,
+						CAIF_MODEMCMD_FLOW_OFF_REQ);
+	}
 
+	err = sk_filter(sk, skb);
+	if (err)
+		return err;
+	if (!sk_rmem_schedule(sk, skb->truesize) && rx_flow_is_on(cf_sk)) {
+		set_rx_flow_off(cf_sk);
+		trace_printk("CAIF: %s():"
+			" sending flow OFF due to rmem_schedule\n",
+			__func__);
+		if (cf_sk->layer.dn)
+			cf_sk->layer.dn->modemcmd(cf_sk->layer.dn,
+						CAIF_MODEMCMD_FLOW_OFF_REQ);
+	}
+	skb->dev = NULL;
+	skb_set_owner_r(skb, sk);
+	/* Cache the SKB length before we tack it onto the receive
+	 * queue. Once it is added it no longer belongs to us and
+	 * may be freed by other threads of control pulling packets
+	 * from the queue.
+	 */
+	skb_len = skb->len;
+	spin_lock_irqsave(&list->lock, flags);
+	if (!sock_flag(sk, SOCK_DEAD))
+		__skb_queue_tail(list, skb);
+	spin_unlock_irqrestore(&list->lock, flags);
+
+	if (!sock_flag(sk, SOCK_DEAD))
+		sk->sk_data_ready(sk, skb_len);
+	else
+		kfree_skb(skb);
 	return 0;
 }
 
-/* Packet Flow Control Callback function called from CAIF */
-static void caif_sktflowctrl_cb(struct cflayer *layr,
-				enum caif_ctrlcmd flow,
-				int phyid)
+/* Packet Receive Callback function called from CAIF Stack */
+static int caif_sktrecv_cb(struct cflayer *layr, struct cfpkt *pkt)
 {
 	struct caifsock *cf_sk;
-
-	/* NOTE: This function may be called in Tasklet context! */
-	pr_debug("CAIF: %s(): flowctrl func called: %s.\n",
-		      __func__,
-		      flow == CAIF_CTRLCMD_FLOW_ON_IND ? "ON" :
-		      flow == CAIF_CTRLCMD_FLOW_OFF_IND ? "OFF" :
-		      flow == CAIF_CTRLCMD_INIT_RSP ? "INIT_RSP" :
-		      flow == CAIF_CTRLCMD_DEINIT_RSP ? "DEINIT_RSP" :
-		      flow == CAIF_CTRLCMD_INIT_FAIL_RSP ? "INIT_FAIL_RSP" :
-		      flow ==
-		      CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND ? "REMOTE_SHUTDOWN" :
-		      "UKNOWN CTRL COMMAND");
-
-	if (layr == NULL)
-		return;
+	struct sk_buff *skb;
 
 	cf_sk = container_of(layr, struct caifsock, layer);
+	skb = cfpkt_tonative(pkt);
+
+	if (unlikely(cf_sk->sk.sk_state != CAIF_CONNECTED)) {
+		cfpkt_destroy(pkt);
+		return 0;
+	}
+	caif_queue_rcv_skb(&cf_sk->sk, skb);
+	return 0;
+}
 
+/* Packet Control Callback function called from CAIF */
+static void caif_ctrl_cb(struct cflayer *layr,
+				enum caif_ctrlcmd flow,
+				int phyid)
+{
+	struct caifsock *cf_sk = container_of(layr, struct caifsock, layer);
 	switch (flow) {
 	case CAIF_CTRLCMD_FLOW_ON_IND:
+		/* OK from modem to start sending again */
 		dbfs_atomic_inc(&cnt.num_tx_flow_on_ind);
-		/* Signal reader that data is available. */
-		SET_TX_FLOW_ON(cf_sk);
-		wake_up_interruptible(sk_sleep(&cf_sk->sk));
+		set_tx_flow_on(cf_sk);
+		cf_sk->sk.sk_state_change(&cf_sk->sk);
 		break;
 
 	case CAIF_CTRLCMD_FLOW_OFF_IND:
+		/* Modem asks us to shut up */
 		dbfs_atomic_inc(&cnt.num_tx_flow_off_ind);
-		SET_TX_FLOW_OFF(cf_sk);
+		set_tx_flow_off(cf_sk);
+		cf_sk->sk.sk_state_change(&cf_sk->sk);
 		break;
 
 	case CAIF_CTRLCMD_INIT_RSP:
-		dbfs_atomic_inc(&cnt.num_init_resp);
-		/* Signal reader that data is available. */
-		caif_assert(STATE_IS_OPEN(cf_sk));
-		SET_PENDING_OFF(cf_sk);
-		SET_TX_FLOW_ON(cf_sk);
-		wake_up_interruptible(sk_sleep(&cf_sk->sk));
+		/* We're now connected */
+		dbfs_atomic_inc(&cnt.num_connect_resp);
+		cf_sk->sk.sk_state = CAIF_CONNECTED;
+		set_tx_flow_on(cf_sk);
+		cf_sk->sk.sk_state_change(&cf_sk->sk);
 		break;
 
 	case CAIF_CTRLCMD_DEINIT_RSP:
-		dbfs_atomic_inc(&cnt.num_deinit_resp);
-		caif_assert(!STATE_IS_OPEN(cf_sk));
-		SET_PENDING_OFF(cf_sk);
-		if (!STATE_IS_PENDING_DESTROY(cf_sk)) {
-			if (sk_sleep(&cf_sk->sk) != NULL)
-				wake_up_interruptible(sk_sleep(&cf_sk->sk));
-		}
-		dbfs_atomic_inc(&cnt.num_deinit);
-		sock_put(&cf_sk->sk);
+		/* We're now disconnected */
+		cf_sk->sk.sk_state = CAIF_DISCONNECTED;
+		cf_sk->sk.sk_state_change(&cf_sk->sk);
+		cfcnfg_release_adap_layer(&cf_sk->layer);
 		break;
 
 	case CAIF_CTRLCMD_INIT_FAIL_RSP:
-		dbfs_atomic_inc(&cnt.num_init_fail_resp);
-		caif_assert(STATE_IS_OPEN(cf_sk));
-		SET_STATE_CLOSED(cf_sk);
-		SET_PENDING_OFF(cf_sk);
-		SET_TX_FLOW_OFF(cf_sk);
-		wake_up_interruptible(sk_sleep(&cf_sk->sk));
+		/* Connect request failed */
+		dbfs_atomic_inc(&cnt.num_connect_fail_resp);
+		cf_sk->sk.sk_err = ECONNREFUSED;
+		cf_sk->sk.sk_state = CAIF_DISCONNECTED;
+		cf_sk->sk.sk_shutdown = SHUTDOWN_MASK;
+		/*
+		 * Socket "standards" seems to require POLLOUT to
+		 * be set at connect failure.
+		 */
+		set_tx_flow_on(cf_sk);
+		cf_sk->sk.sk_state_change(&cf_sk->sk);
 		break;
 
 	case CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND:
+		/* Modem has closed this connection, or device is down. */
 		dbfs_atomic_inc(&cnt.num_remote_shutdown_ind);
-		SET_REMOTE_SHUTDOWN(cf_sk);
-		/* Use sk_shutdown to indicate remote shutdown indication */
-		cf_sk->sk.sk_shutdown |= RCV_SHUTDOWN;
-		cf_sk->file_mode = 0;
-		wake_up_interruptible(sk_sleep(&cf_sk->sk));
+		cf_sk->sk.sk_shutdown = SHUTDOWN_MASK;
+		cf_sk->sk.sk_err = ECONNRESET;
+		set_rx_flow_on(cf_sk);
+		cf_sk->sk.sk_error_report(&cf_sk->sk);
 		break;
 
 	default:
 		pr_debug("CAIF: %s(): Unexpected flow command %d\n",
-			      __func__, flow);
+				__func__, flow);
 	}
 }
 
-static void skb_destructor(struct sk_buff *skb)
+static void caif_check_flow_release(struct sock *sk)
 {
-	dbfs_atomic_inc(&cnt.skb_free);
-	dbfs_atomic_dec(&cnt.skb_in_use);
-}
+	struct caifsock *cf_sk = container_of(sk, struct caifsock, sk);
 
+	if (cf_sk->layer.dn == NULL || cf_sk->layer.dn->modemcmd == NULL)
+		return;
+	if (rx_flow_is_on(cf_sk))
+		return;
 
-static int caif_recvmsg(struct kiocb *iocb, struct socket *sock,
+	if (atomic_read(&sk->sk_rmem_alloc) <= sk_rcvbuf_lowwater(cf_sk)) {
+			dbfs_atomic_inc(&cnt.num_rx_flow_on);
+			set_rx_flow_on(cf_sk);
+			cf_sk->layer.dn->modemcmd(cf_sk->layer.dn,
+						CAIF_MODEMCMD_FLOW_ON_REQ);
+	}
+}
+/*
+ * Copied from sock.c:sock_queue_rcv_skb(), and added check that user buffer
+ * has sufficient size.
+ */
+
+static int caif_seqpkt_recvmsg(struct kiocb *iocb, struct socket *sock,
 				struct msghdr *m, size_t buf_len, int flags)
 
 {
 	struct sock *sk = sock->sk;
-	struct caifsock *cf_sk = container_of(sk, struct caifsock, sk);
-	struct cfpkt *pkt = NULL;
-	size_t len;
-	int result;
 	struct sk_buff *skb;
-	ssize_t ret = -EIO;
-	int read_queue_low;
-
-	if (cf_sk == NULL) {
-		pr_debug("CAIF: %s(): private_data not set!\n",
-			      __func__);
-		ret = -EBADFD;
-		goto read_error;
-	}
-
-	/* Don't do multiple iovec entries yet */
-	if (m->msg_iovlen != 1)
-		return -EOPNOTSUPP;
+	int ret = 0;
+	int len;
 
 	if (unlikely(!buf_len))
 		return -EINVAL;
 
-	lock_sock(&(cf_sk->sk));
-
-	caif_assert(cf_sk->pktq);
-
-	if (!STATE_IS_OPEN(cf_sk)) {
-		/* Socket is closed or closing. */
-		if (!STATE_IS_PENDING(cf_sk)) {
-			pr_debug("CAIF: %s(): socket is closed (by remote)\n",
-				 __func__);
-			ret = -EPIPE;
-		} else {
-			pr_debug("CAIF: %s(): socket is closing..\n", __func__);
-			ret = -EBADF;
-		}
+	skb = skb_recv_datagram(sk, flags, 0 , &ret);
+	if (!skb)
 		goto read_error;
-	}
-	/* Socket is open or opening. */
-	if (STATE_IS_PENDING(cf_sk)) {
-		pr_debug("CAIF: %s(): socket is opening...\n", __func__);
-
-		if (flags & MSG_DONTWAIT) {
-			/* We can't block. */
-			pr_debug("CAIF: %s():state pending and MSG_DONTWAIT\n",
-				 __func__);
-			ret = -EAGAIN;
-			goto read_error;
-		}
 
+	len = skb->len;
+
+	if (skb && skb->len > buf_len && !(flags & MSG_PEEK)) {
+		len = buf_len;
 		/*
-		 * Blocking mode; state is pending and we need to wait
-		 * for its conclusion.
+		 * Push skb back on receive queue if buffer too small.
+		 * This has a built-in race where multi-threaded receive
+		 * may get packet in wrong order, but multiple read does
+		 * not really guarantee ordered delivery anyway.
+		 * Let's optimize for speed without taking locks.
 		 */
-		release_sock(&cf_sk->sk);
-
-		result =
-		    wait_event_interruptible(*sk_sleep(&cf_sk->sk),
-					     !STATE_IS_PENDING(cf_sk));
 
-		lock_sock(&(cf_sk->sk));
-
-		if (result == -ERESTARTSYS) {
-			pr_debug("CAIF: %s(): wait_event_interruptible"
-				 " woken by a signal (1)", __func__);
-			ret = -ERESTARTSYS;
-			goto read_error;
-		}
+		skb_queue_head(&sk->sk_receive_queue, skb);
+		ret = -EMSGSIZE;
+		goto read_error;
 	}
 
-	if (STATE_IS_REMOTE_SHUTDOWN(cf_sk) ||
-		!STATE_IS_OPEN(cf_sk) ||
-		STATE_IS_PENDING(cf_sk)) {
-
-		pr_debug("CAIF: %s(): socket closed\n",
-			__func__);
-		ret = -ESHUTDOWN;
+	ret = skb_copy_datagram_iovec(skb, 0, m->msg_iov, len);
+	if (ret)
 		goto read_error;
-	}
 
-	/*
-	 * Block if we don't have any received buffers.
-	 * The queue has its own lock.
-	 */
-	while ((pkt = cfpkt_qpeek(cf_sk->pktq)) == NULL) {
+	skb_free_datagram(sk, skb);
 
-		if (flags & MSG_DONTWAIT) {
-			pr_debug("CAIF: %s(): MSG_DONTWAIT\n", __func__);
-			ret = -EAGAIN;
-			goto read_error;
-		}
-		trace_printk("CAIF: %s() wait_event\n", __func__);
+	caif_check_flow_release(sk);
 
-		/* Let writers in. */
-		release_sock(&cf_sk->sk);
+	return len;
 
-		/* Block reader until data arrives or socket is closed. */
-		if (wait_event_interruptible(*sk_sleep(&cf_sk->sk),
-					cfpkt_qpeek(cf_sk->pktq)
-					|| STATE_IS_REMOTE_SHUTDOWN(cf_sk)
-					|| !STATE_IS_OPEN(cf_sk)) ==
-		    -ERESTARTSYS) {
-			pr_debug("CAIF: %s():"
-				" wait_event_interruptible woken by "
-				"a signal, signal_pending(current) = %d\n",
-				__func__,
-				signal_pending(current));
-			return -ERESTARTSYS;
-		}
+read_error:
+	return ret;
+}
 
-		trace_printk("CAIF: %s() awake\n", __func__);
-		if (STATE_IS_REMOTE_SHUTDOWN(cf_sk)) {
-			pr_debug("CAIF: %s(): "
-				 "received remote_shutdown indication\n",
-				 __func__);
-			ret = -ESHUTDOWN;
-			goto read_error_no_unlock;
-		}
 
-		/* I want to be alone on cf_sk (except status and queue). */
-		lock_sock(&(cf_sk->sk));
+/* Copied from unix_stream_wait_data, identical except for lock call. */
+static long caif_stream_data_wait(struct sock *sk, long timeo)
+{
+	DEFINE_WAIT(wait);
+	lock_sock(sk);
+
+	for (;;) {
+		prepare_to_wait(sk_sleep(sk), &wait, TASK_INTERRUPTIBLE);
+
+		if (!skb_queue_empty(&sk->sk_receive_queue) ||
+			sk->sk_err ||
+			sk->sk_state != CAIF_CONNECTED ||
+			sock_flag(sk, SOCK_DEAD) ||
+			(sk->sk_shutdown & RCV_SHUTDOWN) ||
+			signal_pending(current) ||
+			!timeo)
+			break;
 
-		if (!STATE_IS_OPEN(cf_sk)) {
-			/* Someone closed the link, report error. */
-			pr_debug("CAIF: %s(): remote end shutdown!\n",
-				      __func__);
-			ret = -EPIPE;
-			goto read_error;
-		}
+		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);
 	}
 
-	/* The queue has its own lock. */
-	len = cfpkt_getlen(pkt);
-
-	/* Check max length that can be copied. */
-	if (len <= buf_len)
-		pkt = cfpkt_dequeue(cf_sk->pktq);
-	else {
-		pr_debug("CAIF: %s(): user buffer too small (%ld,%ld)\n",
-			 __func__, (long) len, (long) buf_len);
-		if (sock->type == SOCK_SEQPACKET) {
-			ret = -EMSGSIZE;
-			goto read_error;
-		}
-		len = buf_len;
-	}
+	finish_wait(sk_sleep(sk), &wait);
+	release_sock(sk);
+	return timeo;
+}
 
 
-	spin_lock(&cf_sk->read_queue_len_lock);
-	cf_sk->read_queue_len--;
-	read_queue_low = (cf_sk->read_queue_len < CHNL_SKT_READ_QUEUE_LOW);
-	spin_unlock(&cf_sk->read_queue_len_lock);
+/*
+ * Copied from unix_stream_recvmsg, but removed credit checks,
+ * changed locking calls, changed address handling.
+ */
+static int caif_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
+				struct msghdr *msg, size_t size,
+				int flags)
+{
+	struct sock *sk = sock->sk;
+	int copied = 0;
+	int target;
+	int err = 0;
+	long timeo;
 
-	if (!RX_FLOW_IS_ON(cf_sk) && read_queue_low) {
-		dbfs_atomic_inc(&cnt.num_rx_flow_on);
-		SET_RX_FLOW_ON(cf_sk);
+	err = -EOPNOTSUPP;
+	if (flags&MSG_OOB)
+		goto out;
 
-		/* Send flow on. */
-		pr_debug("CAIF: %s(): sending flow ON (queue len = %d)\n",
-			 __func__, cf_sk->read_queue_len);
-		caif_assert(cf_sk->layer.dn);
-		caif_assert(cf_sk->layer.dn->ctrlcmd);
-		(void) cf_sk->layer.dn->modemcmd(cf_sk->layer.dn,
-					       CAIF_MODEMCMD_FLOW_ON_REQ);
+	msg->msg_namelen = 0;
 
-		caif_assert(cf_sk->read_queue_len >= 0);
-	}
+	/*
+	 * Lock the socket to prevent queue disordering
+	 * while sleeps in memcpy_tomsg
+	 */
+	err = -EAGAIN;
+	if (sk->sk_state == CAIF_CONNECTING)
+		goto out;
 
-	skb = cfpkt_tonative(pkt);
-	result = skb_copy_datagram_iovec(skb, 0, m->msg_iov, len);
-	skb_pull(skb, len);
+	caif_read_lock(sk);
+	target = sock_rcvlowat(sk, flags&MSG_WAITALL, size);
+	timeo = sock_rcvtimeo(sk, flags&MSG_DONTWAIT);
 
-	if (result) {
-		pr_debug("CAIF: %s(): copy to_iovec failed\n", __func__);
-		cfpkt_destroy(pkt);
-		ret = -EFAULT;
-		goto read_error;
-	}
+	do {
+		int chunk;
+		struct sk_buff *skb;
 
-	/* Free packet and remove from queue */
-	if (skb->len == 0)
-		skb_free_datagram(sk, skb);
+		lock_sock(sk);
+		skb = skb_dequeue(&sk->sk_receive_queue);
+		caif_check_flow_release(sk);
 
-	/* Let the others in. */
-	release_sock(&cf_sk->sk);
-	return len;
+		if (skb == NULL) {
+			if (copied >= target)
+				goto unlock;
+			/*
+			 *	POSIX 1003.1g mandates this order.
+			 */
+			err = sock_error(sk);
+			if (err)
+				goto unlock;
+			err = -ECONNRESET;
+			if (sk->sk_shutdown & RCV_SHUTDOWN)
+				goto unlock;
 
-read_error:
-	release_sock(&cf_sk->sk);
-read_error_no_unlock:
-	return ret;
-}
+			err = -EPIPE;
+			if (sk->sk_state != CAIF_CONNECTED)
+				goto unlock;
+			if (sock_flag(sk, SOCK_DEAD))
+				goto unlock;
 
-/* Send a signal as a consequence of sendmsg, sendto or caif_sendmsg. */
-static int caif_sendmsg(struct kiocb *kiocb, struct socket *sock,
-			struct msghdr *msg, size_t len)
-{
+			release_sock(sk);
 
-	struct sock *sk = sock->sk;
-	struct caifsock *cf_sk = container_of(sk, struct caifsock, sk);
-	size_t payload_size = msg->msg_iov->iov_len;
-	struct cfpkt *pkt = NULL;
-	struct caif_payload_info info;
-	unsigned char *txbuf;
-	ssize_t ret = -EIO;
-	int result;
-	struct sk_buff *skb;
-	caif_assert(msg->msg_iovlen == 1);
+			err = -EAGAIN;
+			if (!timeo)
+				break;
 
-	if (cf_sk == NULL) {
-		pr_debug("CAIF: %s(): private_data not set!\n",
-			      __func__);
-		ret = -EBADFD;
-		goto write_error_no_unlock;
-	}
+			caif_read_unlock(sk);
 
-	if (unlikely(msg->msg_iov->iov_base == NULL)) {
-		pr_warning("CAIF: %s(): Buffer is NULL.\n", __func__);
-		ret = -EINVAL;
-		goto write_error_no_unlock;
-	}
+			timeo = caif_stream_data_wait(sk, timeo);
 
-	if (payload_size > CAIF_MAX_PAYLOAD_SIZE) {
-		pr_debug("CAIF: %s(): buffer too long\n", __func__);
-		if (sock->type == SOCK_SEQPACKET) {
-			ret = -EINVAL;
-			goto write_error_no_unlock;
+			if (signal_pending(current)) {
+				err = sock_intr_errno(timeo);
+				goto out;
+			}
+			caif_read_lock(sk);
+			continue;
+unlock:
+			release_sock(sk);
+			break;
 		}
-		payload_size = CAIF_MAX_PAYLOAD_SIZE;
-	}
+		release_sock(sk);
+		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 == 0)
+				copied = -EFAULT;
+			break;
+		}
+		copied += chunk;
+		size -= chunk;
 
-	/* I want to be alone on cf_sk (except status and queue) */
-	lock_sock(&(cf_sk->sk));
+		/* Mark read part of skb as used */
+		if (!(flags & MSG_PEEK)) {
+			skb_pull(skb, chunk);
 
-	caif_assert(cf_sk->pktq);
+			/* put the skb back if we didn't use it up. */
+			if (skb->len) {
+				skb_queue_head(&sk->sk_receive_queue, skb);
+				break;
+			}
+			kfree_skb(skb);
 
-	if (!STATE_IS_OPEN(cf_sk)) {
-		/* Socket is closed or closing */
-		if (!STATE_IS_PENDING(cf_sk)) {
-			pr_debug("CAIF: %s(): socket is closed (by remote)\n",
-				 __func__);
-			ret = -EPIPE;
 		} else {
-			pr_debug("CAIF: %s(): socket is closing...\n",
-				 __func__);
-			ret = -EBADF;
-		}
-		goto write_error;
-	}
-
-	/* Socket is open or opening */
-	if (STATE_IS_PENDING(cf_sk)) {
-		pr_debug("CAIF: %s(): socket is opening...\n", __func__);
-
-		if (msg->msg_flags & MSG_DONTWAIT) {
-			/* We can't block */
-			trace_printk("CAIF: %s():state pending:"
-				     "state=MSG_DONTWAIT\n", __func__);
-			ret = -EAGAIN;
-			goto write_error;
+			/*
+			 * It is questionable, see note in unix_dgram_recvmsg.
+			 */
+			/* put message back and return */
+			skb_queue_head(&sk->sk_receive_queue, skb);
+			break;
 		}
-		/* Let readers in */
-		release_sock(&cf_sk->sk);
-
-		/*
-		 * Blocking mode; state is pending and we need to wait
-		 * for its conclusion.
-		 */
-		result =
-		    wait_event_interruptible(*sk_sleep(&cf_sk->sk),
-					     !STATE_IS_PENDING(cf_sk));
-		/* I want to be alone on cf_sk (except status and queue) */
-		lock_sock(&(cf_sk->sk));
+	} while (size);
+	caif_read_unlock(sk);
 
-		if (result == -ERESTARTSYS) {
-			pr_debug("CAIF: %s(): wait_event_interruptible"
-				 " woken by a signal (1)", __func__);
-			ret = -ERESTARTSYS;
-			goto write_error;
-		}
-	}
-	if (STATE_IS_REMOTE_SHUTDOWN(cf_sk) ||
-		!STATE_IS_OPEN(cf_sk) ||
-		STATE_IS_PENDING(cf_sk)) {
+out:
+	return copied ? : err;
+}
 
-		pr_debug("CAIF: %s(): socket closed\n",
-			__func__);
-		ret = -ESHUTDOWN;
-		goto write_error;
+/*
+ * Copied from sock.c:sock_wait_for_wmem, but change to wait for
+ * CAIF flow-on and sock_writable.
+ */
+static long caif_wait_for_flow_on(struct caifsock *cf_sk,
+				int wait_writeable, long timeo, int *err)
+{
+	struct sock *sk = &cf_sk->sk;
+	DEFINE_WAIT(wait);
+	for (;;) {
+		*err = 0;
+		if (tx_flow_is_on(cf_sk) &&
+			(!wait_writeable || sock_writeable(&cf_sk->sk)))
+			break;
+		*err = -ETIMEDOUT;
+		if (!timeo)
+			break;
+		*err = -ERESTARTSYS;
+		if (signal_pending(current))
+			break;
+		prepare_to_wait(sk_sleep(sk), &wait, TASK_INTERRUPTIBLE);
+		*err = -ECONNRESET;
+		if (sk->sk_shutdown & SHUTDOWN_MASK)
+			break;
+		*err = -sk->sk_err;
+		if (sk->sk_err)
+			break;
+		*err = -EPIPE;
+		if (cf_sk->sk.sk_state != CAIF_CONNECTED)
+			break;
+		timeo = schedule_timeout(timeo);
 	}
+	finish_wait(sk_sleep(sk), &wait);
+	return timeo;
+}
 
-	if (!TX_FLOW_IS_ON(cf_sk)) {
+/*
+ * Transmit a SKB. The device may temporarily request re-transmission
+ * by returning EAGAIN.
+ */
+static int transmit_skb(struct sk_buff *skb, struct caifsock *cf_sk,
+			int noblock, long timeo)
+{
+	struct cfpkt *pkt;
+	int ret, loopcnt = 0;
 
-		/* Flow is off. Check non-block flag */
-		if (msg->msg_flags & MSG_DONTWAIT) {
-			trace_printk("CAIF: %s(): MSG_DONTWAIT and tx flow off",
-				 __func__);
-			ret = -EAGAIN;
-			goto write_error;
-		}
+	pkt = cfpkt_fromnative(CAIF_DIR_OUT, skb);
+	memset(cfpkt_info(pkt), 0, sizeof(struct caif_payload_info));
+	do {
 
-		/* release lock before waiting */
-		release_sock(&cf_sk->sk);
+		ret = -ETIMEDOUT;
 
-		/* Wait until flow is on or socket is closed */
-		if (wait_event_interruptible(*sk_sleep(&cf_sk->sk),
-					TX_FLOW_IS_ON(cf_sk)
-					|| !STATE_IS_OPEN(cf_sk)
-					|| STATE_IS_REMOTE_SHUTDOWN(cf_sk)
-					) == -ERESTARTSYS) {
-			pr_debug("CAIF: %s():"
-				 " wait_event_interruptible woken by a signal",
-				 __func__);
-			ret = -ERESTARTSYS;
-			goto write_error_no_unlock;
+		/* Slight paranoia, probably not needed. */
+		if (unlikely(loopcnt++ > 1000)) {
+			pr_warning("CAIF: %s(): transmit retries failed,"
+				" error = %d\n", __func__, ret);
+			break;
 		}
 
-		/* I want to be alone on cf_sk (except status and queue) */
-		lock_sock(&(cf_sk->sk));
-
-		if (!STATE_IS_OPEN(cf_sk)) {
-			/* someone closed the link, report error */
-			pr_debug("CAIF: %s(): remote end shutdown!\n",
-				      __func__);
-			ret = -EPIPE;
-			goto write_error;
+		if (cf_sk->layer.dn != NULL)
+			ret = cf_sk->layer.dn->transmit(cf_sk->layer.dn, pkt);
+		if (likely(ret >= 0))
+			break;
+		/* if transmit return -EAGAIN, then retry */
+		if (noblock && ret == -EAGAIN)
+			break;
+		timeo = caif_wait_for_flow_on(cf_sk, 0, timeo, &ret);
+		if (signal_pending(current)) {
+			ret = sock_intr_errno(timeo);
+			break;
 		}
-
-		if (STATE_IS_REMOTE_SHUTDOWN(cf_sk)) {
-			pr_debug("CAIF: %s(): "
-				 "received remote_shutdown indication\n",
-				 __func__);
-			ret = -ESHUTDOWN;
-			goto write_error;
+		if (ret)
+			break;
+		if (cf_sk->sk.sk_state != CAIF_CONNECTED ||
+			sock_flag(&cf_sk->sk, SOCK_DEAD) ||
+			(cf_sk->sk.sk_shutdown & RCV_SHUTDOWN)) {
+			ret = -EPIPE;
+			cf_sk->sk.sk_err = EPIPE;
+			break;
 		}
-	}
+	} while (ret == -EAGAIN);
+	return ret;
+}
 
-	pkt = cfpkt_create(payload_size);
-	skb = (struct sk_buff *)pkt;
-	skb->destructor = skb_destructor;
-	skb->sk = sk;
-	dbfs_atomic_inc(&cnt.skb_alloc);
-	dbfs_atomic_inc(&cnt.skb_in_use);
-	if (cfpkt_raw_append(pkt, (void **) &txbuf, payload_size) < 0) {
-		pr_debug("CAIF: %s(): cfpkt_raw_append failed\n", __func__);
-		cfpkt_destroy(pkt);
-		ret = -EINVAL;
-		goto write_error;
-	}
+/* Copied from af_unix:unix_dgram_sendmsg, and adapted to CAIF */
+static int caif_seqpkt_sendmsg(struct kiocb *kiocb, struct socket *sock,
+			struct msghdr *msg, size_t len)
+{
+	struct sock *sk = sock->sk;
+	struct caifsock *cf_sk = container_of(sk, struct caifsock, sk);
+	int buffer_size;
+	int ret = 0;
+	struct sk_buff *skb = NULL;
+	int noblock;
+	long timeo;
+	caif_assert(cf_sk);
+	ret = sock_error(sk);
+	if (ret)
+		goto err;
+
+	ret = -EOPNOTSUPP;
+	if (msg->msg_flags&MSG_OOB)
+		goto err;
+
+	ret = -EOPNOTSUPP;
+	if (msg->msg_namelen)
+		goto err;
+
+	ret = -EINVAL;
+	if (unlikely(msg->msg_iov->iov_base == NULL))
+		goto err;
+	noblock = msg->msg_flags & MSG_DONTWAIT;
+
+	buffer_size = len + CAIF_NEEDED_HEADROOM + CAIF_NEEDED_TAILROOM;
+
+	ret = -EMSGSIZE;
+	if (buffer_size > CAIF_MAX_PAYLOAD_SIZE)
+		goto err;
+
+	timeo = sock_sndtimeo(sk, noblock);
+	timeo = caif_wait_for_flow_on(container_of(sk, struct caifsock, sk),
+				1, timeo, &ret);
+
+	ret = -EPIPE;
+	if (cf_sk->sk.sk_state != CAIF_CONNECTED ||
+		sock_flag(sk, SOCK_DEAD) ||
+		(sk->sk_shutdown & RCV_SHUTDOWN))
+		goto err;
+
+	ret = -ENOMEM;
+	skb = sock_alloc_send_skb(sk, buffer_size, noblock, &ret);
+	if (!skb)
+		goto err;
+	skb_reserve(skb, CAIF_NEEDED_HEADROOM);
+
+	ret = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
+
+	if (ret)
+		goto err;
+	ret = transmit_skb(skb, cf_sk, noblock, timeo);
+	if (ret < 0)
+		goto err;
+	return len;
+err:
+	kfree_skb(skb);
+	return ret;
+}
 
-	/* Copy data into buffer. */
-	if (copy_from_user(txbuf, msg->msg_iov->iov_base, payload_size)) {
-		pr_debug("CAIF: %s(): copy_from_user returned non zero.\n",
-			 __func__);
-		cfpkt_destroy(pkt);
-		ret = -EINVAL;
-		goto write_error;
-	}
-	memset(&info, 0, sizeof(info));
+/*
+ * Copied from unix_stream_sendmsg and adapted to CAIF:
+ * Changed removed permission handling and added waiting for flow on
+ * and other minor adaptations.
+ */
+static int caif_stream_sendmsg(struct kiocb *kiocb, struct socket *sock,
+				struct msghdr *msg, size_t len)
+{
+	struct sock *sk = sock->sk;
+	struct caifsock *cf_sk = container_of(sk, struct caifsock, sk);
+	int err, size;
+	struct sk_buff *skb;
+	int sent = 0;
+	long timeo;
 
-	/* Send the packet down the stack. */
-	caif_assert(cf_sk->layer.dn);
-	caif_assert(cf_sk->layer.dn->transmit);
+	err = -EOPNOTSUPP;
 
-	do {
-		ret = cf_sk->layer.dn->transmit(cf_sk->layer.dn, pkt);
+	if (unlikely(msg->msg_flags&MSG_OOB))
+		goto out_err;
 
-		if (likely((ret >= 0) || (ret != -EAGAIN)))
-			break;
+	if (unlikely(msg->msg_namelen))
+		goto out_err;
 
-		/* EAGAIN - retry */
-		if (msg->msg_flags & MSG_DONTWAIT) {
-			pr_debug("CAIF: %s(): NONBLOCK and transmit failed,"
-