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