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