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>
9 #include <trace/events/sock.h>
10 #if IS_ENABLED(CONFIG_IPV6)
11 #include <net/ipv6_stubs.h>
12 #endif
13
handle_nonesp(struct espintcp_ctx * ctx,struct sk_buff * skb,struct sock * sk)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)) {
19 XFRM_INC_STATS(sock_net(sk), LINUX_MIB_XFRMINERROR);
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
handle_esp(struct sk_buff * skb,struct sock * sk)31 static void handle_esp(struct sk_buff *skb, struct sock *sk)
32 {
33 struct tcp_skb_cb *tcp_cb = (struct tcp_skb_cb *)skb->cb;
34
35 skb_reset_transport_header(skb);
36
37 /* restore IP CB, we need at least IP6CB->nhoff */
38 memmove(skb->cb, &tcp_cb->header, sizeof(tcp_cb->header));
39
40 rcu_read_lock();
41 skb->dev = dev_get_by_index_rcu(sock_net(sk), skb->skb_iif);
42 local_bh_disable();
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);
49 local_bh_enable();
50 rcu_read_unlock();
51 }
52
espintcp_rcv(struct strparser * strp,struct sk_buff * skb)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);
58 int len = rxm->full_len - 2;
59 u32 nonesp_marker;
60 int err;
61
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) {
68 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
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))) {
81 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
82 kfree_skb(skb);
83 return;
84 }
85
86 err = skb_copy_bits(skb, rxm->offset + 2, &nonesp_marker,
87 sizeof(nonesp_marker));
88 if (err < 0) {
89 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINHDRERROR);
90 kfree_skb(skb);
91 return;
92 }
93
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);
97 kfree_skb(skb);
98 return;
99 }
100
101 if (pskb_trim(skb, rxm->full_len - 2) != 0) {
102 XFRM_INC_STATS(sock_net(strp->sk), LINUX_MIB_XFRMINERROR);
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
espintcp_parse(struct strparser * strp,struct sk_buff * skb)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);
128 if (len < 2)
129 return -EINVAL;
130
131 return len;
132 }
133
espintcp_recvmsg(struct sock * sk,struct msghdr * msg,size_t len,int flags,int * addr_len)134 static int espintcp_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
135 int flags, int *addr_len)
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
143 skb = __skb_recv_datagram(sk, &ctx->ike_queue, flags, &off, &err);
144 if (!skb) {
145 if (err == -EAGAIN && sk->sk_shutdown & RCV_SHUTDOWN)
146 return 0;
147 return err;
148 }
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
espintcp_queue_out(struct sock * sk,struct sk_buff * skb)168 int espintcp_queue_out(struct sock *sk, struct sk_buff *skb)
169 {
170 struct espintcp_ctx *ctx = espintcp_getctx(sk);
171
172 if (skb_queue_len(&ctx->out_queue) >= READ_ONCE(netdev_max_backlog))
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
espintcp_sendskb_locked(struct sock * sk,struct espintcp_msg * emsg,int flags)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
espintcp_sendskmsg_locked(struct sock * sk,struct espintcp_msg * emsg,int flags)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
espintcp_push_msgs(struct sock * sk,int flags)251 static int espintcp_push_msgs(struct sock *sk, int flags)
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)
265 err = espintcp_sendskb_locked(sk, emsg, flags);
266 else
267 err = espintcp_sendskmsg_locked(sk, emsg, flags);
268 if (err == -EAGAIN) {
269 ctx->tx_running = 0;
270 return flags & MSG_DONTWAIT ? -EAGAIN : 0;
271 }
272 if (!err)
273 memset(emsg, 0, sizeof(*emsg));
274
275 ctx->tx_running = 0;
276
277 return err;
278 }
279
espintcp_push_skb(struct sock * sk,struct sk_buff * skb)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
295 espintcp_push_msgs(sk, 0);
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
308 espintcp_push_msgs(sk, 0);
309
310 return 0;
311 }
312 EXPORT_SYMBOL_GPL(espintcp_push_skb);
313
espintcp_sendmsg(struct sock * sk,struct msghdr * msg,size_t size)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
325 if (msg->msg_flags & ~MSG_DONTWAIT)
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
336 err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
337 if (err < 0) {
338 if (err != -EAGAIN || !(msg->msg_flags & MSG_DONTWAIT))
339 err = -ENOBUFS;
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);
358 iov_iter_kvec(&pfx_iter, ITER_SOURCE, &pfx_iov, 1, pfx_iov.iov_len);
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
376 err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
377 /* this message could be partially sent, keep it */
378
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;
393 static struct proto espintcp6_prot;
394 static struct proto_ops espintcp6_ops;
395 static DEFINE_MUTEX(tcpv6_prot_mutex);
396
espintcp_data_ready(struct sock * sk)397 static void espintcp_data_ready(struct sock *sk)
398 {
399 struct espintcp_ctx *ctx = espintcp_getctx(sk);
400
401 trace_sk_data_ready(sk);
402
403 strp_data_ready(&ctx->strp);
404 }
405
espintcp_tx_work(struct work_struct * work)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)
414 espintcp_push_msgs(sk, 0);
415 release_sock(sk);
416 }
417
espintcp_write_space(struct sock * sk)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
espintcp_destruct(struct sock * sk)426 static void espintcp_destruct(struct sock *sk)
427 {
428 struct espintcp_ctx *ctx = espintcp_getctx(sk);
429
430 ctx->saved_destruct(sk);
431 kfree(ctx);
432 }
433
tcp_is_ulp_esp(struct sock * sk)434 bool tcp_is_ulp_esp(struct sock *sk)
435 {
436 return sk->sk_prot == &espintcp_prot || sk->sk_prot == &espintcp6_prot;
437 }
438 EXPORT_SYMBOL_GPL(tcp_is_ulp_esp);
439
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);
espintcp_init_sk(struct sock * sk)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);
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 }
484 ctx->saved_data_ready = sk->sk_data_ready;
485 ctx->saved_write_space = sk->sk_write_space;
486 ctx->saved_destruct = sk->sk_destruct;
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;
495 sk->sk_use_task_frag = false;
496
497 return 0;
498
499 free:
500 kfree(ctx);
501 return err;
502 }
503
espintcp_release(struct sock * sk)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
espintcp_close(struct sock * sk,long timeout)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
espintcp_poll(struct file * file,struct socket * sock,poll_table * wait)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
build_protos(struct proto * espintcp_prot,struct proto_ops * espintcp_ops,const struct proto * orig_prot,const struct proto_ops * orig_ops)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
572 static struct tcp_ulp_ops espintcp_ulp __read_mostly = {
573 .name = "espintcp",
574 .owner = THIS_MODULE,
575 .init = espintcp_init_sk,
576 };
577
espintcp_init(void)578 void __init espintcp_init(void)
579 {
580 build_protos(&espintcp_prot, &espintcp_ops, &tcp_prot, &inet_stream_ops);
581
582 tcp_register_ulp(&espintcp_ulp);
583 }
584