[linux-block.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>
#include <trace/events/sock.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 flags, int *addr_len)
{
	struct espintcp_ctx *ctx = espintcp_getctx(sk);
	struct sk_buff *skb;
	int err = 0;
	int copied;
	int off = 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) >= READ_ONCE(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, ITER_SOURCE, &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);

	trace_sk_data_ready(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;
	sk->sk_use_task_frag = false;

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