[mirror_ubuntu-jammy-kernel.git] / net / xfrm / espintcp.c

// SPDX-License-Identifier: GPL-2.0
#include <net/tcp.h>
#include <net/strparser.h>
#include <net/xfrm.h>
#include <net/esp.h>
#include <net/espintcp.h>
#include <linux/skmsg.h>
#include <net/inet_common.h>
#if IS_ENABLED(CONFIG_IPV6)
#include <net/ipv6_stubs.h>
#endif

static void handle_nonesp(struct espintcp_ctx *ctx, struct sk_buff *skb,
			  struct sock *sk)
{
	if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf ||
	    !sk_rmem_schedule(sk, skb, skb->truesize)) {
		XFRM_INC_STATS(sock_net(sk), LINUX_MIB_XFRMINERROR);
		kfree_skb(skb);
		return;
	}

	skb_set_owner_r(skb, sk);

	memset(skb->cb, 0, sizeof(skb->cb));
	skb_queue_tail(&ctx->ike_queue, skb);
	ctx->saved_data_ready(sk);
}

static void handle_esp(struct sk_buff *skb, struct sock *sk)
{
	struct tcp_skb_cb *tcp_cb = (struct tcp_skb_cb *)skb->cb;

	skb_reset_transport_header(skb);

	/* restore IP CB, we need at least IP6CB->nhoff */
	memmove(skb->cb, &tcp_cb->header, sizeof(tcp_cb->header));

	rcu_read_lock();
	skb->dev = dev_get_by_index_rcu(sock_net(sk), skb->skb_iif);
	local_bh_disable();
#if IS_ENABLED(CONFIG_IPV6)
	if (sk->sk_family == AF_INET6)
		ipv6_stub->xfrm6_rcv_encap(skb, IPPROTO_ESP, 0, TCP_ENCAP_ESPINTCP);
	else
#endif
		xfrm4_rcv_encap(skb, IPPROTO_ESP, 0, TCP_ENCAP_ESPINTCP);
	local_bh_enable();
	rcu_read_unlock();
}

static void espintcp_rcv(struct strparser *strp, struct sk_buff *skb)
{
	struct espintcp_ctx *ctx = container_of(strp, struct espintcp_ctx,
						strp);
	struct strp_msg *rxm = strp_msg(skb);
	int len = rxm->full_len - 2;
	u32 nonesp_marker;
	int err;

	/* keepalive packet? */
	if (unlikely(len == 1)) {
		u8 data;

		err = skb_copy_bits(skb, rxm->offset + 2, &data, 1);
		if (err < 0) {
			XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
			kfree_skb(skb);
			return;
		}

		if (data == 0xff) {
			kfree_skb(skb);
			return;
		}
	}

	/* drop other short messages */
	if (unlikely(len <= sizeof(nonesp_marker))) {
		XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
		kfree_skb(skb);
		return;
	}

	err = skb_copy_bits(skb, rxm->offset + 2, &nonesp_marker,
			    sizeof(nonesp_marker));
	if (err < 0) {
		XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
		kfree_skb(skb);
		return;
	}

	/* remove header, leave non-ESP marker/SPI */
	if (!__pskb_pull(skb, rxm->offset + 2)) {
		XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
		kfree_skb(skb);
		return;
	}

	if (pskb_trim(skb, rxm->full_len - 2) != 0) {
		XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
		kfree_skb(skb);
		return;
	}

	if (nonesp_marker == 0)
		handle_nonesp(ctx, skb, strp->sk);
	else
		handle_esp(skb, strp->sk);
}

static int espintcp_parse(struct strparser *strp, struct sk_buff *skb)
{
	struct strp_msg *rxm = strp_msg(skb);
	__be16 blen;
	u16 len;
	int err;

	if (skb->len < rxm->offset + 2)
		return 0;

	err = skb_copy_bits(skb, rxm->offset, &blen, sizeof(blen));
	if (err < 0)
		return err;

	len = be16_to_cpu(blen);
	if (len < 2)
		return -EINVAL;

	return len;
}

static int espintcp_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
			    int nonblock, int flags, int *addr_len)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct sk_buff *skb;
	int err = 0;
	int copied;
	int off = 0;

	flags |= nonblock ? MSG_DONTWAIT : 0;

	skb = __skb_recv_datagram(sk, &ctx->ike_queue, flags, &off, &err);
	if (!skb) {
		if (err == -EAGAIN && sk->sk_shutdown & RCV_SHUTDOWN)
			return 0;
		return err;
	}

	copied = len;
	if (copied > skb->len)
		copied = skb->len;
	else if (copied < skb->len)
		msg->msg_flags |= MSG_TRUNC;

	err = skb_copy_datagram_msg(skb, 0, msg, copied);
	if (unlikely(err)) {
		kfree_skb(skb);
		return err;
	}

	if (flags & MSG_TRUNC)
		copied = skb->len;
	kfree_skb(skb);
	return copied;
}

int espintcp_queue_out(struct sock *sk, struct sk_buff *skb)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);

	if (skb_queue_len(&ctx->out_queue) >= netdev_max_backlog)
		return -ENOBUFS;

	__skb_queue_tail(&ctx->out_queue, skb);

	return 0;
}
EXPORT_SYMBOL_GPL(espintcp_queue_out);

/* espintcp length field is 2B and length includes the length field's size */
#define MAX_ESPINTCP_MSG (((1 << 16) - 1) - 2)

static int espintcp_sendskb_locked(struct sock *sk, struct espintcp_msg *emsg,
				   int flags)
{
	do {
		int ret;

		ret = skb_send_sock_locked(sk, emsg->skb,
					   emsg->offset, emsg->len);
		if (ret < 0)
			return ret;

		emsg->len -= ret;
		emsg->offset += ret;
	} while (emsg->len > 0);

	kfree_skb(emsg->skb);
	memset(emsg, 0, sizeof(*emsg));

	return 0;
}

static int espintcp_sendskmsg_locked(struct sock *sk,
				     struct espintcp_msg *emsg, int flags)
{
	struct sk_msg *skmsg = &emsg->skmsg;
	struct scatterlist *sg;
	int done = 0;
	int ret;

	flags |= MSG_SENDPAGE_NOTLAST;
	sg = &skmsg->sg.data[skmsg->sg.start];
	do {
		size_t size = sg->length - emsg->offset;
		int offset = sg->offset + emsg->offset;
		struct page *p;

		emsg->offset = 0;

		if (sg_is_last(sg))
			flags &= ~MSG_SENDPAGE_NOTLAST;

		p = sg_page(sg);
retry:
		ret = do_tcp_sendpages(sk, p, offset, size, flags);
		if (ret < 0) {
			emsg->offset = offset - sg->offset;
			skmsg->sg.start += done;
			return ret;
		}

		if (ret != size) {
			offset += ret;
			size -= ret;
			goto retry;
		}

		done++;
		put_page(p);
		sk_mem_uncharge(sk, sg->length);
		sg = sg_next(sg);
	} while (sg);

	memset(emsg, 0, sizeof(*emsg));

	return 0;
}

static int espintcp_push_msgs(struct sock *sk, int flags)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct espintcp_msg *emsg = &ctx->partial;
	int err;

	if (!emsg->len)
		return 0;

	if (ctx->tx_running)
		return -EAGAIN;
	ctx->tx_running = 1;

	if (emsg->skb)
		err = espintcp_sendskb_locked(sk, emsg, flags);
	else
		err = espintcp_sendskmsg_locked(sk, emsg, flags);
	if (err == -EAGAIN) {
		ctx->tx_running = 0;
		return flags & MSG_DONTWAIT ? -EAGAIN : 0;
	}
	if (!err)
		memset(emsg, 0, sizeof(*emsg));

	ctx->tx_running = 0;

	return err;
}

int espintcp_push_skb(struct sock *sk, struct sk_buff *skb)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct espintcp_msg *emsg = &ctx->partial;
	unsigned int len;
	int offset;

	if (sk->sk_state != TCP_ESTABLISHED) {
		kfree_skb(skb);
		return -ECONNRESET;
	}

	offset = skb_transport_offset(skb);
	len = skb->len - offset;

	espintcp_push_msgs(sk, 0);

	if (emsg->len) {
		kfree_skb(skb);
		return -ENOBUFS;
	}

	skb_set_owner_w(skb, sk);

	emsg->offset = offset;
	emsg->len = len;
	emsg->skb = skb;

	espintcp_push_msgs(sk, 0);

	return 0;
}
EXPORT_SYMBOL_GPL(espintcp_push_skb);

static int espintcp_sendmsg(struct sock *sk, struct msghdr *msg, size_t size)
{
	long timeo = sock_sndtimeo(sk, msg->msg_flags & MSG_DONTWAIT);
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct espintcp_msg *emsg = &ctx->partial;
	struct iov_iter pfx_iter;
	struct kvec pfx_iov = {};
	size_t msglen = size + 2;
	char buf[2] = {0};
	int err, end;

	if (msg->msg_flags & ~MSG_DONTWAIT)
		return -EOPNOTSUPP;

	if (size > MAX_ESPINTCP_MSG)
		return -EMSGSIZE;

	if (msg->msg_controllen)
		return -EOPNOTSUPP;

	lock_sock(sk);

	err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
	if (err < 0) {
		if (err != -EAGAIN || !(msg->msg_flags & MSG_DONTWAIT))
			err = -ENOBUFS;
		goto unlock;
	}

	sk_msg_init(&emsg->skmsg);
	while (1) {
		/* only -ENOMEM is possible since we don't coalesce */
		err = sk_msg_alloc(sk, &emsg->skmsg, msglen, 0);
		if (!err)
			break;

		err = sk_stream_wait_memory(sk, &timeo);
		if (err)
			goto fail;
	}

	*((__be16 *)buf) = cpu_to_be16(msglen);
	pfx_iov.iov_base = buf;
	pfx_iov.iov_len = sizeof(buf);
	iov_iter_kvec(&pfx_iter, WRITE, &pfx_iov, 1, pfx_iov.iov_len);

	err = sk_msg_memcopy_from_iter(sk, &pfx_iter, &emsg->skmsg,
				       pfx_iov.iov_len);
	if (err < 0)
		goto fail;

	err = sk_msg_memcopy_from_iter(sk, &msg->msg_iter, &emsg->skmsg, size);
	if (err < 0)
		goto fail;

	end = emsg->skmsg.sg.end;
	emsg->len = size;
	sk_msg_iter_var_prev(end);
	sg_mark_end(sk_msg_elem(&emsg->skmsg, end));

	tcp_rate_check_app_limited(sk);

	err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
	/* this message could be partially sent, keep it */

	release_sock(sk);

	return size;

fail:
	sk_msg_free(sk, &emsg->skmsg);
	memset(emsg, 0, sizeof(*emsg));
unlock:
	release_sock(sk);
	return err;
}

static struct proto espintcp_prot __ro_after_init;
static struct proto_ops espintcp_ops __ro_after_init;
static struct proto espintcp6_prot;
static struct proto_ops espintcp6_ops;
static DEFINE_MUTEX(tcpv6_prot_mutex);

static void espintcp_data_ready(struct sock *sk)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);

	strp_data_ready(&ctx->strp);
}

static void espintcp_tx_work(struct work_struct *work)
{
	struct espintcp_ctx *ctx = container_of(work,
						struct espintcp_ctx, work);
	struct sock *sk = ctx->strp.sk;

	lock_sock(sk);
	if (!ctx->tx_running)
		espintcp_push_msgs(sk, 0);
	release_sock(sk);
}

static void espintcp_write_space(struct sock *sk)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);

	schedule_work(&ctx->work);
	ctx->saved_write_space(sk);
}

static void espintcp_destruct(struct sock *sk)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);

	ctx->saved_destruct(sk);
	kfree(ctx);
}

bool tcp_is_ulp_esp(struct sock *sk)
{
	return sk->sk_prot == &espintcp_prot || sk->sk_prot == &espintcp6_prot;
}
EXPORT_SYMBOL_GPL(tcp_is_ulp_esp);

static void build_protos(struct proto *espintcp_prot,
			 struct proto_ops *espintcp_ops,
			 const struct proto *orig_prot,
			 const struct proto_ops *orig_ops);
static int espintcp_init_sk(struct sock *sk)
{
	struct inet_connection_sock *icsk = inet_csk(sk);
	struct strp_callbacks cb = {
		.rcv_msg = espintcp_rcv,
		.parse_msg = espintcp_parse,
	};
	struct espintcp_ctx *ctx;
	int err;

	/* sockmap is not compatible with espintcp */
	if (sk->sk_user_data)
		return -EBUSY;

	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
	if (!ctx)
		return -ENOMEM;

	err = strp_init(&ctx->strp, sk, &cb);
	if (err)
		goto free;

	__sk_dst_reset(sk);

	strp_check_rcv(&ctx->strp);
	skb_queue_head_init(&ctx->ike_queue);
	skb_queue_head_init(&ctx->out_queue);

	if (sk->sk_family == AF_INET) {
		sk->sk_prot = &espintcp_prot;
		sk->sk_socket->ops = &espintcp_ops;
	} else {
		mutex_lock(&tcpv6_prot_mutex);
		if (!espintcp6_prot.recvmsg)
			build_protos(&espintcp6_prot, &espintcp6_ops, sk->sk_prot, sk->sk_socket->ops);
		mutex_unlock(&tcpv6_prot_mutex);

		sk->sk_prot = &espintcp6_prot;
		sk->sk_socket->ops = &espintcp6_ops;
	}
	ctx->saved_data_ready = sk->sk_data_ready;
	ctx->saved_write_space = sk->sk_write_space;
	ctx->saved_destruct = sk->sk_destruct;
	sk->sk_data_ready = espintcp_data_ready;
	sk->sk_write_space = espintcp_write_space;
	sk->sk_destruct = espintcp_destruct;
	rcu_assign_pointer(icsk->icsk_ulp_data, ctx);
	INIT_WORK(&ctx->work, espintcp_tx_work);

	/* avoid using task_frag */
	sk->sk_allocation = GFP_ATOMIC;

	return 0;

free:
	kfree(ctx);
	return err;
}

static void espintcp_release(struct sock *sk)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct sk_buff_head queue;
	struct sk_buff *skb;

	__skb_queue_head_init(&queue);
	skb_queue_splice_init(&ctx->out_queue, &queue);

	while ((skb = __skb_dequeue(&queue)))
		espintcp_push_skb(sk, skb);

	tcp_release_cb(sk);
}

static void espintcp_close(struct sock *sk, long timeout)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct espintcp_msg *emsg = &ctx->partial;

	strp_stop(&ctx->strp);

	sk->sk_prot = &tcp_prot;
	barrier();

	cancel_work_sync(&ctx->work);
	strp_done(&ctx->strp);

	skb_queue_purge(&ctx->out_queue);
	skb_queue_purge(&ctx->ike_queue);

	if (emsg->len) {
		if (emsg->skb)
			kfree_skb(emsg->skb);
		else
			sk_msg_free(sk, &emsg->skmsg);
	}

	tcp_close(sk, timeout);
}

static __poll_t espintcp_poll(struct file *file, struct socket *sock,
			      poll_table *wait)
{
	__poll_t mask = datagram_poll(file, sock, wait);
	struct sock *sk = sock->sk;
	struct espintcp_ctx *ctx = espintcp_getctx(sk);

	if (!skb_queue_empty(&ctx->ike_queue))
		mask |= EPOLLIN | EPOLLRDNORM;

	return mask;
}

static void build_protos(struct proto *espintcp_prot,
			 struct proto_ops *espintcp_ops,
			 const struct proto *orig_prot,
			 const struct proto_ops *orig_ops)
{
	memcpy(espintcp_prot, orig_prot, sizeof(struct proto));
	memcpy(espintcp_ops, orig_ops, sizeof(struct proto_ops));
	espintcp_prot->sendmsg = espintcp_sendmsg;
	espintcp_prot->recvmsg = espintcp_recvmsg;
	espintcp_prot->close = espintcp_close;
	espintcp_prot->release_cb = espintcp_release;
	espintcp_ops->poll = espintcp_poll;
}

static struct tcp_ulp_ops espintcp_ulp __read_mostly = {
	.name = "espintcp",
	.owner = THIS_MODULE,
	.init = espintcp_init_sk,
};

void __init espintcp_init(void)
{
	build_protos(&espintcp_prot, &espintcp_ops, &tcp_prot, &inet_stream_ops);

	tcp_register_ulp(&espintcp_ulp);
}
Commit	Line	Data
e27cca96 SD	1	// SPDX-License-Identifier: GPL-2.0
	2	#include <net/tcp.h>
	3	#include <net/strparser.h>
	4	#include <net/xfrm.h>
	5	#include <net/esp.h>
	6	#include <net/espintcp.h>
	7	#include <linux/skmsg.h>
	8	#include <net/inet_common.h>
26333c37 SD	9	#if IS_ENABLED(CONFIG_IPV6)
	10	#include <net/ipv6_stubs.h>
	11	#endif
e27cca96 SD	12
	13	static void handle_nonesp(struct espintcp_ctx ctx, struct sk_buff skb,
	14	struct sock *sk)
	15	{
	16	if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf \|\|
	17	!sk_rmem_schedule(sk, skb, skb->truesize)) {
71b59bf4	18	XFRM_INC_STATS(sock_net(sk), LINUX_MIB_XFRMINERROR);
e27cca96 SD	19	kfree_skb(skb);
	20	return;
	21	}
	22
	23	skb_set_owner_r(skb, sk);
	24
	25	memset(skb->cb, 0, sizeof(skb->cb));
	26	skb_queue_tail(&ctx->ike_queue, skb);
	27	ctx->saved_data_ready(sk);
	28	}
	29
	30	static void handle_esp(struct sk_buff skb, struct sock sk)
	31	{
4eb2e134 SD	32	struct tcp_skb_cb tcp_cb = (struct tcp_skb_cb )skb->cb;
4eb2e134 SD	33
e27cca96	34	skb_reset_transport_header(skb);
4eb2e134 SD	35
	36	/* restore IP CB, we need at least IP6CB->nhoff */
	37	memmove(skb->cb, &tcp_cb->header, sizeof(tcp_cb->header));
e27cca96 SD	38
	39	rcu_read_lock();
	40	skb->dev = dev_get_by_index_rcu(sock_net(sk), skb->skb_iif);
	41	local_bh_disable();
26333c37 SD	42	#if IS_ENABLED(CONFIG_IPV6)
	43	if (sk->sk_family == AF_INET6)
	44	ipv6_stub->xfrm6_rcv_encap(skb, IPPROTO_ESP, 0, TCP_ENCAP_ESPINTCP);
	45	else
	46	#endif
	47	xfrm4_rcv_encap(skb, IPPROTO_ESP, 0, TCP_ENCAP_ESPINTCP);
e27cca96 SD	48	local_bh_enable();
	49	rcu_read_unlock();
	50	}
	51
	52	static void espintcp_rcv(struct strparser strp, struct sk_buff skb)
	53	{
	54	struct espintcp_ctx *ctx = container_of(strp, struct espintcp_ctx,
	55	strp);
	56	struct strp_msg *rxm = strp_msg(skb);
fadd1a63	57	int len = rxm->full_len - 2;
e27cca96 SD	58	u32 nonesp_marker;
	59	int err;
	60
fadd1a63 SD	61	/* keepalive packet? */
	62	if (unlikely(len == 1)) {
	63	u8 data;
	64
	65	err = skb_copy_bits(skb, rxm->offset + 2, &data, 1);
	66	if (err < 0) {
71b59bf4	67	XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
fadd1a63 SD	68	kfree_skb(skb);
	69	return;
	70	}
	71
	72	if (data == 0xff) {
	73	kfree_skb(skb);
	74	return;
	75	}
	76	}
	77
	78	/* drop other short messages */
	79	if (unlikely(len <= sizeof(nonesp_marker))) {
71b59bf4	80	XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
fadd1a63 SD	81	kfree_skb(skb);
	82	return;
	83	}
	84
e27cca96 SD	85	err = skb_copy_bits(skb, rxm->offset + 2, &nonesp_marker,
	86	sizeof(nonesp_marker));
	87	if (err < 0) {
71b59bf4	88	XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
e27cca96 SD	89	kfree_skb(skb);
	90	return;
	91	}
	92
	93	/* remove header, leave non-ESP marker/SPI */
	94	if (!__pskb_pull(skb, rxm->offset + 2)) {
71b59bf4	95	XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
e27cca96 SD	96	kfree_skb(skb);
	97	return;
	98	}
	99
	100	if (pskb_trim(skb, rxm->full_len - 2) != 0) {
71b59bf4	101	XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
e27cca96 SD	102	kfree_skb(skb);
	103	return;
	104	}
	105
	106	if (nonesp_marker == 0)
	107	handle_nonesp(ctx, skb, strp->sk);
	108	else
	109	handle_esp(skb, strp->sk);
	110	}
	111
	112	static int espintcp_parse(struct strparser strp, struct sk_buff skb)
	113	{
	114	struct strp_msg *rxm = strp_msg(skb);
	115	__be16 blen;
	116	u16 len;
	117	int err;
	118
	119	if (skb->len < rxm->offset + 2)
	120	return 0;
	121
	122	err = skb_copy_bits(skb, rxm->offset, &blen, sizeof(blen));
	123	if (err < 0)
	124	return err;
	125
	126	len = be16_to_cpu(blen);
fadd1a63	127	if (len < 2)
e27cca96 SD	128	return -EINVAL;
	129
	130	return len;
	131	}
	132
	133	static int espintcp_recvmsg(struct sock sk, struct msghdr msg, size_t len,
	134	int nonblock, int flags, int *addr_len)
	135	{
	136	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	137	struct sk_buff *skb;
	138	int err = 0;
	139	int copied;
	140	int off = 0;
	141
	142	flags \|= nonblock ? MSG_DONTWAIT : 0;
	143
e427cad6	144	skb = __skb_recv_datagram(sk, &ctx->ike_queue, flags, &off, &err);
e229c877 SD	145	if (!skb) {
	146	if (err == -EAGAIN && sk->sk_shutdown & RCV_SHUTDOWN)
	147	return 0;
e27cca96	148	return err;
e229c877	149	}
e27cca96 SD	150
	151	copied = len;
	152	if (copied > skb->len)
	153	copied = skb->len;
	154	else if (copied < skb->len)
	155	msg->msg_flags \|= MSG_TRUNC;
	156
	157	err = skb_copy_datagram_msg(skb, 0, msg, copied);
	158	if (unlikely(err)) {
	159	kfree_skb(skb);
	160	return err;
	161	}
	162
	163	if (flags & MSG_TRUNC)
	164	copied = skb->len;
	165	kfree_skb(skb);
	166	return copied;
	167	}
	168
	169	int espintcp_queue_out(struct sock sk, struct sk_buff skb)
	170	{
	171	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	172
	173	if (skb_queue_len(&ctx->out_queue) >= netdev_max_backlog)
	174	return -ENOBUFS;
	175
	176	__skb_queue_tail(&ctx->out_queue, skb);
	177
	178	return 0;
	179	}
	180	EXPORT_SYMBOL_GPL(espintcp_queue_out);
	181
	182	/* espintcp length field is 2B and length includes the length field's size */
	183	#define MAX_ESPINTCP_MSG (((1 << 16) - 1) - 2)
	184
	185	static int espintcp_sendskb_locked(struct sock sk, struct espintcp_msg emsg,
	186	int flags)
	187	{
	188	do {
	189	int ret;
	190
	191	ret = skb_send_sock_locked(sk, emsg->skb,
	192	emsg->offset, emsg->len);
	193	if (ret < 0)
	194	return ret;
	195
	196	emsg->len -= ret;
	197	emsg->offset += ret;
	198	} while (emsg->len > 0);
	199
	200	kfree_skb(emsg->skb);
	201	memset(emsg, 0, sizeof(*emsg));
	202
	203	return 0;
	204	}
	205
	206	static int espintcp_sendskmsg_locked(struct sock *sk,
	207	struct espintcp_msg *emsg, int flags)
	208	{
	209	struct sk_msg *skmsg = &emsg->skmsg;
	210	struct scatterlist *sg;
	211	int done = 0;
	212	int ret;
	213
214	flags \|= MSG_SENDPAGE_NOTLAST;
215	sg = &skmsg->sg.data[skmsg->sg.start];
216	do {
217	size_t size = sg->length - emsg->offset;
218	int offset = sg->offset + emsg->offset;
219	struct page *p;
220
221	emsg->offset = 0;
222
223	if (sg_is_last(sg))
224	flags &= ~MSG_SENDPAGE_NOTLAST;
225
226	p = sg_page(sg);
227	retry:
228	ret = do_tcp_sendpages(sk, p, offset, size, flags);
229	if (ret < 0) {
230	emsg->offset = offset - sg->offset;
231	skmsg->sg.start += done;
232	return ret;
233	}
234
235	if (ret != size) {
236	offset += ret;
237	size -= ret;
238	goto retry;
239	}
240
241	done++;
242	put_page(p);
243	sk_mem_uncharge(sk, sg->length);
244	sg = sg_next(sg);
245	} while (sg);
246
247	memset(emsg, 0, sizeof(*emsg));
248
249	return 0;
250	}
251
ac1321ef	252	static int espintcp_push_msgs(struct sock *sk, int flags)
e27cca96 SD	253	{
	254	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	255	struct espintcp_msg *emsg = &ctx->partial;
	256	int err;
	257
	258	if (!emsg->len)
	259	return 0;
	260
	261	if (ctx->tx_running)
	262	return -EAGAIN;
	263	ctx->tx_running = 1;
	264
	265	if (emsg->skb)
ac1321ef	266	err = espintcp_sendskb_locked(sk, emsg, flags);
e27cca96	267	else
ac1321ef	268	err = espintcp_sendskmsg_locked(sk, emsg, flags);
e27cca96 SD	269	if (err == -EAGAIN) {
e27cca96 SD	270	ctx->tx_running = 0;
ac1321ef	271	return flags & MSG_DONTWAIT ? -EAGAIN : 0;
e27cca96 SD	272	}
	273	if (!err)
	274	memset(emsg, 0, sizeof(*emsg));
	275
	276	ctx->tx_running = 0;
	277
	278	return err;
	279	}
	280
	281	int espintcp_push_skb(struct sock sk, struct sk_buff skb)
	282	{
	283	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	284	struct espintcp_msg *emsg = &ctx->partial;
	285	unsigned int len;
	286	int offset;
	287
	288	if (sk->sk_state != TCP_ESTABLISHED) {
	289	kfree_skb(skb);
	290	return -ECONNRESET;
	291	}
	292
	293	offset = skb_transport_offset(skb);
	294	len = skb->len - offset;
	295
ac1321ef	296	espintcp_push_msgs(sk, 0);
e27cca96 SD	297
	298	if (emsg->len) {
	299	kfree_skb(skb);
	300	return -ENOBUFS;
	301	}
	302
	303	skb_set_owner_w(skb, sk);
	304
	305	emsg->offset = offset;
	306	emsg->len = len;
	307	emsg->skb = skb;
	308
ac1321ef	309	espintcp_push_msgs(sk, 0);
e27cca96 SD	310
	311	return 0;
	312	}
	313	EXPORT_SYMBOL_GPL(espintcp_push_skb);
	314
	315	static int espintcp_sendmsg(struct sock sk, struct msghdr msg, size_t size)
	316	{
	317	long timeo = sock_sndtimeo(sk, msg->msg_flags & MSG_DONTWAIT);
	318	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	319	struct espintcp_msg *emsg = &ctx->partial;
	320	struct iov_iter pfx_iter;
	321	struct kvec pfx_iov = {};
	322	size_t msglen = size + 2;
	323	char buf[2] = {0};
	324	int err, end;
	325
ac1321ef	326	if (msg->msg_flags & ~MSG_DONTWAIT)
e27cca96 SD	327	return -EOPNOTSUPP;
	328
	329	if (size > MAX_ESPINTCP_MSG)
	330	return -EMSGSIZE;
	331
	332	if (msg->msg_controllen)
	333	return -EOPNOTSUPP;
	334
	335	lock_sock(sk);
	336
ac1321ef	337	err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
e27cca96	338	if (err < 0) {
ac1321ef SD	339	if (err != -EAGAIN \|\| !(msg->msg_flags & MSG_DONTWAIT))
ac1321ef SD	340	err = -ENOBUFS;
e27cca96 SD	341	goto unlock;
	342	}
	343
	344	sk_msg_init(&emsg->skmsg);
	345	while (1) {
	346	/* only -ENOMEM is possible since we don't coalesce */
	347	err = sk_msg_alloc(sk, &emsg->skmsg, msglen, 0);
	348	if (!err)
	349	break;
	350
	351	err = sk_stream_wait_memory(sk, &timeo);
	352	if (err)
	353	goto fail;
	354	}
	355
	356	((__be16 )buf) = cpu_to_be16(msglen);
	357	pfx_iov.iov_base = buf;
	358	pfx_iov.iov_len = sizeof(buf);
	359	iov_iter_kvec(&pfx_iter, WRITE, &pfx_iov, 1, pfx_iov.iov_len);
	360
	361	err = sk_msg_memcopy_from_iter(sk, &pfx_iter, &emsg->skmsg,
	362	pfx_iov.iov_len);
	363	if (err < 0)
	364	goto fail;
	365
	366	err = sk_msg_memcopy_from_iter(sk, &msg->msg_iter, &emsg->skmsg, size);
	367	if (err < 0)
	368	goto fail;
	369
	370	end = emsg->skmsg.sg.end;
	371	emsg->len = size;
	372	sk_msg_iter_var_prev(end);
	373	sg_mark_end(sk_msg_elem(&emsg->skmsg, end));
	374
	375	tcp_rate_check_app_limited(sk);
	376
ac1321ef	377	err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
e27cca96	378	/* this message could be partially sent, keep it */
ac1321ef	379
e27cca96 SD	380	release_sock(sk);
	381
	382	return size;
	383
	384	fail:
	385	sk_msg_free(sk, &emsg->skmsg);
	386	memset(emsg, 0, sizeof(*emsg));
	387	unlock:
	388	release_sock(sk);
	389	return err;
	390	}
	391
	392	static struct proto espintcp_prot __ro_after_init;
	393	static struct proto_ops espintcp_ops __ro_after_init;
26333c37 SD	394	static struct proto espintcp6_prot;
	395	static struct proto_ops espintcp6_ops;
	396	static DEFINE_MUTEX(tcpv6_prot_mutex);
e27cca96 SD	397
	398	static void espintcp_data_ready(struct sock *sk)
	399	{
	400	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	401
	402	strp_data_ready(&ctx->strp);
	403	}
	404
	405	static void espintcp_tx_work(struct work_struct *work)
	406	{
	407	struct espintcp_ctx *ctx = container_of(work,
	408	struct espintcp_ctx, work);
	409	struct sock *sk = ctx->strp.sk;
	410
	411	lock_sock(sk);
	412	if (!ctx->tx_running)
ac1321ef	413	espintcp_push_msgs(sk, 0);
e27cca96 SD	414	release_sock(sk);
	415	}
	416
	417	static void espintcp_write_space(struct sock *sk)
	418	{
	419	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	420
	421	schedule_work(&ctx->work);
	422	ctx->saved_write_space(sk);
	423	}
	424
	425	static void espintcp_destruct(struct sock *sk)
	426	{
	427	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	428
9f0cadc3	429	ctx->saved_destruct(sk);
e27cca96 SD	430	kfree(ctx);
	431	}
	432
	433	bool tcp_is_ulp_esp(struct sock *sk)
	434	{
26333c37	435	return sk->sk_prot == &espintcp_prot \|\| sk->sk_prot == &espintcp6_prot;
e27cca96 SD	436	}
	437	EXPORT_SYMBOL_GPL(tcp_is_ulp_esp);
	438
26333c37 SD	439	static void build_protos(struct proto *espintcp_prot,
	440	struct proto_ops *espintcp_ops,
	441	const struct proto *orig_prot,
	442	const struct proto_ops *orig_ops);
e27cca96 SD	443	static int espintcp_init_sk(struct sock *sk)
	444	{
	445	struct inet_connection_sock *icsk = inet_csk(sk);
	446	struct strp_callbacks cb = {
	447	.rcv_msg = espintcp_rcv,
	448	.parse_msg = espintcp_parse,
	449	};
	450	struct espintcp_ctx *ctx;
	451	int err;
	452
	453	/* sockmap is not compatible with espintcp */
	454	if (sk->sk_user_data)
	455	return -EBUSY;
	456
	457	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
	458	if (!ctx)
	459	return -ENOMEM;
	460
	461	err = strp_init(&ctx->strp, sk, &cb);
	462	if (err)
	463	goto free;
	464
	465	__sk_dst_reset(sk);
	466
	467	strp_check_rcv(&ctx->strp);
	468	skb_queue_head_init(&ctx->ike_queue);
	469	skb_queue_head_init(&ctx->out_queue);
26333c37 SD	470
	471	if (sk->sk_family == AF_INET) {
	472	sk->sk_prot = &espintcp_prot;
	473	sk->sk_socket->ops = &espintcp_ops;
	474	} else {
	475	mutex_lock(&tcpv6_prot_mutex);
	476	if (!espintcp6_prot.recvmsg)
	477	build_protos(&espintcp6_prot, &espintcp6_ops, sk->sk_prot, sk->sk_socket->ops);
	478	mutex_unlock(&tcpv6_prot_mutex);
	479
	480	sk->sk_prot = &espintcp6_prot;
	481	sk->sk_socket->ops = &espintcp6_ops;
	482	}
e27cca96 SD	483	ctx->saved_data_ready = sk->sk_data_ready;
e27cca96 SD	484	ctx->saved_write_space = sk->sk_write_space;
9f0cadc3	485	ctx->saved_destruct = sk->sk_destruct;
e27cca96 SD	486	sk->sk_data_ready = espintcp_data_ready;
	487	sk->sk_write_space = espintcp_write_space;
	488	sk->sk_destruct = espintcp_destruct;
	489	rcu_assign_pointer(icsk->icsk_ulp_data, ctx);
	490	INIT_WORK(&ctx->work, espintcp_tx_work);
	491
	492	/* avoid using task_frag */
	493	sk->sk_allocation = GFP_ATOMIC;
	494
	495	return 0;
	496
	497	free:
	498	kfree(ctx);
	499	return err;
	500	}
	501
	502	static void espintcp_release(struct sock *sk)
	503	{
	504	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	505	struct sk_buff_head queue;
	506	struct sk_buff *skb;
	507
	508	__skb_queue_head_init(&queue);
	509	skb_queue_splice_init(&ctx->out_queue, &queue);
	510
	511	while ((skb = __skb_dequeue(&queue)))
	512	espintcp_push_skb(sk, skb);
	513
	514	tcp_release_cb(sk);
	515	}
	516
	517	static void espintcp_close(struct sock *sk, long timeout)
	518	{
	519	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	520	struct espintcp_msg *emsg = &ctx->partial;
	521
	522	strp_stop(&ctx->strp);
	523
	524	sk->sk_prot = &tcp_prot;
	525	barrier();
	526
	527	cancel_work_sync(&ctx->work);
	528	strp_done(&ctx->strp);
	529
	530	skb_queue_purge(&ctx->out_queue);
	531	skb_queue_purge(&ctx->ike_queue);
	532
	533	if (emsg->len) {
	534	if (emsg->skb)
	535	kfree_skb(emsg->skb);
	536	else
	537	sk_msg_free(sk, &emsg->skmsg);
	538	}
	539
	540	tcp_close(sk, timeout);
	541	}
	542
	543	static __poll_t espintcp_poll(struct file file, struct socket sock,
	544	poll_table *wait)
	545	{
	546	__poll_t mask = datagram_poll(file, sock, wait);
	547	struct sock *sk = sock->sk;
	548	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	549
550	if (!skb_queue_empty(&ctx->ike_queue))
551	mask \|= EPOLLIN \| EPOLLRDNORM;
552
553	return mask;
554	}
555
26333c37 SD	556	static void build_protos(struct proto *espintcp_prot,
	557	struct proto_ops *espintcp_ops,
	558	const struct proto *orig_prot,
	559	const struct proto_ops *orig_ops)
	560	{
	561	memcpy(espintcp_prot, orig_prot, sizeof(struct proto));
	562	memcpy(espintcp_ops, orig_ops, sizeof(struct proto_ops));
	563	espintcp_prot->sendmsg = espintcp_sendmsg;
	564	espintcp_prot->recvmsg = espintcp_recvmsg;
	565	espintcp_prot->close = espintcp_close;
	566	espintcp_prot->release_cb = espintcp_release;
	567	espintcp_ops->poll = espintcp_poll;
	568	}
	569
e27cca96 SD	570	static struct tcp_ulp_ops espintcp_ulp __read_mostly = {
	571	.name = "espintcp",
	572	.owner = THIS_MODULE,
	573	.init = espintcp_init_sk,
	574	};
	575
	576	void __init espintcp_init(void)
	577	{
26333c37	578	build_protos(&espintcp_prot, &espintcp_ops, &tcp_prot, &inet_stream_ops);
e27cca96 SD	579
	580	tcp_register_ulp(&espintcp_ulp);
	581	}