1 // SPDX-License-Identifier: GPL-2.0
3 #include <net/strparser.h>
6 #include <net/espintcp.h>
7 #include <linux/skmsg.h>
8 #include <net/inet_common.h>
9 #include <trace/events/sock.h>
10 #if IS_ENABLED(CONFIG_IPV6)
11 #include <net/ipv6_stubs.h>
14 static void handle_nonesp(struct espintcp_ctx *ctx, struct sk_buff *skb,
17 if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf ||
18 !sk_rmem_schedule(sk, skb, skb->truesize)) {
19 XFRM_INC_STATS(sock_net(sk), LINUX_MIB_XFRMINERROR);
24 skb_set_owner_r(skb, sk);
26 memset(skb->cb, 0, sizeof(skb->cb));
27 skb_queue_tail(&ctx->ike_queue, skb);
28 ctx->saved_data_ready(sk);
31 static void handle_esp(struct sk_buff *skb, struct sock *sk)
33 struct tcp_skb_cb *tcp_cb = (struct tcp_skb_cb *)skb->cb;
35 skb_reset_transport_header(skb);
37 /* restore IP CB, we need at least IP6CB->nhoff */
38 memmove(skb->cb, &tcp_cb->header, sizeof(tcp_cb->header));
41 skb->dev = dev_get_by_index_rcu(sock_net(sk), skb->skb_iif);
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);
48 xfrm4_rcv_encap(skb, IPPROTO_ESP, 0, TCP_ENCAP_ESPINTCP);
53 static void espintcp_rcv(struct strparser *strp, struct sk_buff *skb)
55 struct espintcp_ctx *ctx = container_of(strp, struct espintcp_ctx,
57 struct strp_msg *rxm = strp_msg(skb);
58 int len = rxm->full_len - 2;
62 /* keepalive packet? */
63 if (unlikely(len == 1)) {
66 err = skb_copy_bits(skb, rxm->offset + 2, &data, 1);
68 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
79 /* drop other short messages */
80 if (unlikely(len <= sizeof(nonesp_marker))) {
81 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
86 err = skb_copy_bits(skb, rxm->offset + 2, &nonesp_marker,
87 sizeof(nonesp_marker));
89 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
94 /* remove header, leave non-ESP marker/SPI */
95 if (!pskb_pull(skb, rxm->offset + 2)) {
96 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
101 if (pskb_trim(skb, rxm->full_len - 2) != 0) {
102 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
107 if (nonesp_marker == 0)
108 handle_nonesp(ctx, skb, strp->sk);
110 handle_esp(skb, strp->sk);
113 static int espintcp_parse(struct strparser *strp, struct sk_buff *skb)
115 struct strp_msg *rxm = strp_msg(skb);
120 if (skb->len < rxm->offset + 2)
123 err = skb_copy_bits(skb, rxm->offset, &blen, sizeof(blen));
127 len = be16_to_cpu(blen);
134 static int espintcp_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
135 int flags, int *addr_len)
137 struct espintcp_ctx *ctx = espintcp_getctx(sk);
143 skb = __skb_recv_datagram(sk, &ctx->ike_queue, flags, &off, &err);
145 if (err == -EAGAIN && sk->sk_shutdown & RCV_SHUTDOWN)
151 if (copied > skb->len)
153 else if (copied < skb->len)
154 msg->msg_flags |= MSG_TRUNC;
156 err = skb_copy_datagram_msg(skb, 0, msg, copied);
162 if (flags & MSG_TRUNC)
168 int espintcp_queue_out(struct sock *sk, struct sk_buff *skb)
170 struct espintcp_ctx *ctx = espintcp_getctx(sk);
172 if (skb_queue_len(&ctx->out_queue) >= READ_ONCE(netdev_max_backlog))
175 __skb_queue_tail(&ctx->out_queue, skb);
179 EXPORT_SYMBOL_GPL(espintcp_queue_out);
181 /* espintcp length field is 2B and length includes the length field's size */
182 #define MAX_ESPINTCP_MSG (((1 << 16) - 1) - 2)
184 static int espintcp_sendskb_locked(struct sock *sk, struct espintcp_msg *emsg,
190 ret = skb_send_sock_locked(sk, emsg->skb,
191 emsg->offset, emsg->len);
197 } while (emsg->len > 0);
199 kfree_skb(emsg->skb);
200 memset(emsg, 0, sizeof(*emsg));
205 static int espintcp_sendskmsg_locked(struct sock *sk,
206 struct espintcp_msg *emsg, int flags)
208 struct msghdr msghdr = { .msg_flags = flags | MSG_SPLICE_PAGES, };
209 struct sk_msg *skmsg = &emsg->skmsg;
210 struct scatterlist *sg;
214 msghdr.msg_flags |= MSG_SENDPAGE_NOTLAST;
215 sg = &skmsg->sg.data[skmsg->sg.start];
218 size_t size = sg->length - emsg->offset;
219 int offset = sg->offset + emsg->offset;
225 msghdr.msg_flags &= ~MSG_SENDPAGE_NOTLAST;
229 bvec_set_page(&bvec, p, size, offset);
230 iov_iter_bvec(&msghdr.msg_iter, ITER_SOURCE, &bvec, 1, size);
231 ret = tcp_sendmsg_locked(sk, &msghdr, size);
233 emsg->offset = offset - sg->offset;
234 skmsg->sg.start += done;
246 sk_mem_uncharge(sk, sg->length);
250 memset(emsg, 0, sizeof(*emsg));
255 static int espintcp_push_msgs(struct sock *sk, int flags)
257 struct espintcp_ctx *ctx = espintcp_getctx(sk);
258 struct espintcp_msg *emsg = &ctx->partial;
269 err = espintcp_sendskb_locked(sk, emsg, flags);
271 err = espintcp_sendskmsg_locked(sk, emsg, flags);
272 if (err == -EAGAIN) {
274 return flags & MSG_DONTWAIT ? -EAGAIN : 0;
277 memset(emsg, 0, sizeof(*emsg));
284 int espintcp_push_skb(struct sock *sk, struct sk_buff *skb)
286 struct espintcp_ctx *ctx = espintcp_getctx(sk);
287 struct espintcp_msg *emsg = &ctx->partial;
291 if (sk->sk_state != TCP_ESTABLISHED) {
296 offset = skb_transport_offset(skb);
297 len = skb->len - offset;
299 espintcp_push_msgs(sk, 0);
306 skb_set_owner_w(skb, sk);
308 emsg->offset = offset;
312 espintcp_push_msgs(sk, 0);
316 EXPORT_SYMBOL_GPL(espintcp_push_skb);
318 static int espintcp_sendmsg(struct sock *sk, struct msghdr *msg, size_t size)
320 long timeo = sock_sndtimeo(sk, msg->msg_flags & MSG_DONTWAIT);
321 struct espintcp_ctx *ctx = espintcp_getctx(sk);
322 struct espintcp_msg *emsg = &ctx->partial;
323 struct iov_iter pfx_iter;
324 struct kvec pfx_iov = {};
325 size_t msglen = size + 2;
329 if (msg->msg_flags & ~MSG_DONTWAIT)
332 if (size > MAX_ESPINTCP_MSG)
335 if (msg->msg_controllen)
340 err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
342 if (err != -EAGAIN || !(msg->msg_flags & MSG_DONTWAIT))
347 sk_msg_init(&emsg->skmsg);
349 /* only -ENOMEM is possible since we don't coalesce */
350 err = sk_msg_alloc(sk, &emsg->skmsg, msglen, 0);
354 err = sk_stream_wait_memory(sk, &timeo);
359 *((__be16 *)buf) = cpu_to_be16(msglen);
360 pfx_iov.iov_base = buf;
361 pfx_iov.iov_len = sizeof(buf);
362 iov_iter_kvec(&pfx_iter, ITER_SOURCE, &pfx_iov, 1, pfx_iov.iov_len);
364 err = sk_msg_memcopy_from_iter(sk, &pfx_iter, &emsg->skmsg,
369 err = sk_msg_memcopy_from_iter(sk, &msg->msg_iter, &emsg->skmsg, size);
373 end = emsg->skmsg.sg.end;
375 sk_msg_iter_var_prev(end);
376 sg_mark_end(sk_msg_elem(&emsg->skmsg, end));
378 tcp_rate_check_app_limited(sk);
380 err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
381 /* this message could be partially sent, keep it */
388 sk_msg_free(sk, &emsg->skmsg);
389 memset(emsg, 0, sizeof(*emsg));
395 static struct proto espintcp_prot __ro_after_init;
396 static struct proto_ops espintcp_ops __ro_after_init;
397 static struct proto espintcp6_prot;
398 static struct proto_ops espintcp6_ops;
399 static DEFINE_MUTEX(tcpv6_prot_mutex);
401 static void espintcp_data_ready(struct sock *sk)
403 struct espintcp_ctx *ctx = espintcp_getctx(sk);
405 trace_sk_data_ready(sk);
407 strp_data_ready(&ctx->strp);
410 static void espintcp_tx_work(struct work_struct *work)
412 struct espintcp_ctx *ctx = container_of(work,
413 struct espintcp_ctx, work);
414 struct sock *sk = ctx->strp.sk;
417 if (!ctx->tx_running)
418 espintcp_push_msgs(sk, 0);
422 static void espintcp_write_space(struct sock *sk)
424 struct espintcp_ctx *ctx = espintcp_getctx(sk);
426 schedule_work(&ctx->work);
427 ctx->saved_write_space(sk);
430 static void espintcp_destruct(struct sock *sk)
432 struct espintcp_ctx *ctx = espintcp_getctx(sk);
434 ctx->saved_destruct(sk);
438 bool tcp_is_ulp_esp(struct sock *sk)
440 return sk->sk_prot == &espintcp_prot || sk->sk_prot == &espintcp6_prot;
442 EXPORT_SYMBOL_GPL(tcp_is_ulp_esp);
444 static void build_protos(struct proto *espintcp_prot,
445 struct proto_ops *espintcp_ops,
446 const struct proto *orig_prot,
447 const struct proto_ops *orig_ops);
448 static int espintcp_init_sk(struct sock *sk)
450 struct inet_connection_sock *icsk = inet_csk(sk);
451 struct strp_callbacks cb = {
452 .rcv_msg = espintcp_rcv,
453 .parse_msg = espintcp_parse,
455 struct espintcp_ctx *ctx;
458 /* sockmap is not compatible with espintcp */
459 if (sk->sk_user_data)
462 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
466 err = strp_init(&ctx->strp, sk, &cb);
472 strp_check_rcv(&ctx->strp);
473 skb_queue_head_init(&ctx->ike_queue);
474 skb_queue_head_init(&ctx->out_queue);
476 if (sk->sk_family == AF_INET) {
477 sk->sk_prot = &espintcp_prot;
478 sk->sk_socket->ops = &espintcp_ops;
480 mutex_lock(&tcpv6_prot_mutex);
481 if (!espintcp6_prot.recvmsg)
482 build_protos(&espintcp6_prot, &espintcp6_ops, sk->sk_prot, sk->sk_socket->ops);
483 mutex_unlock(&tcpv6_prot_mutex);
485 sk->sk_prot = &espintcp6_prot;
486 sk->sk_socket->ops = &espintcp6_ops;
488 ctx->saved_data_ready = sk->sk_data_ready;
489 ctx->saved_write_space = sk->sk_write_space;
490 ctx->saved_destruct = sk->sk_destruct;
491 sk->sk_data_ready = espintcp_data_ready;
492 sk->sk_write_space = espintcp_write_space;
493 sk->sk_destruct = espintcp_destruct;
494 rcu_assign_pointer(icsk->icsk_ulp_data, ctx);
495 INIT_WORK(&ctx->work, espintcp_tx_work);
497 /* avoid using task_frag */
498 sk->sk_allocation = GFP_ATOMIC;
499 sk->sk_use_task_frag = false;
508 static void espintcp_release(struct sock *sk)
510 struct espintcp_ctx *ctx = espintcp_getctx(sk);
511 struct sk_buff_head queue;
514 __skb_queue_head_init(&queue);
515 skb_queue_splice_init(&ctx->out_queue, &queue);
517 while ((skb = __skb_dequeue(&queue)))
518 espintcp_push_skb(sk, skb);
523 static void espintcp_close(struct sock *sk, long timeout)
525 struct espintcp_ctx *ctx = espintcp_getctx(sk);
526 struct espintcp_msg *emsg = &ctx->partial;
528 strp_stop(&ctx->strp);
530 sk->sk_prot = &tcp_prot;
533 cancel_work_sync(&ctx->work);
534 strp_done(&ctx->strp);
536 skb_queue_purge(&ctx->out_queue);
537 skb_queue_purge(&ctx->ike_queue);
541 kfree_skb(emsg->skb);
543 sk_msg_free(sk, &emsg->skmsg);
546 tcp_close(sk, timeout);
549 static __poll_t espintcp_poll(struct file *file, struct socket *sock,
552 __poll_t mask = datagram_poll(file, sock, wait);
553 struct sock *sk = sock->sk;
554 struct espintcp_ctx *ctx = espintcp_getctx(sk);
556 if (!skb_queue_empty(&ctx->ike_queue))
557 mask |= EPOLLIN | EPOLLRDNORM;
562 static void build_protos(struct proto *espintcp_prot,
563 struct proto_ops *espintcp_ops,
564 const struct proto *orig_prot,
565 const struct proto_ops *orig_ops)
567 memcpy(espintcp_prot, orig_prot, sizeof(struct proto));
568 memcpy(espintcp_ops, orig_ops, sizeof(struct proto_ops));
569 espintcp_prot->sendmsg = espintcp_sendmsg;
570 espintcp_prot->recvmsg = espintcp_recvmsg;
571 espintcp_prot->close = espintcp_close;
572 espintcp_prot->release_cb = espintcp_release;
573 espintcp_ops->poll = espintcp_poll;
576 static struct tcp_ulp_ops espintcp_ulp __read_mostly = {
578 .owner = THIS_MODULE,
579 .init = espintcp_init_sk,
582 void __init espintcp_init(void)
584 build_protos(&espintcp_prot, &espintcp_ops, &tcp_prot, &inet_stream_ops);
586 tcp_register_ulp(&espintcp_ulp);