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 
handle_nonesp(struct espintcp_ctx * ctx,struct sk_buff * skb,struct sock * sk)13 static 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 
handle_esp(struct sk_buff * skb,struct sock * sk)30 static 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 
espintcp_rcv(struct strparser * strp,struct sk_buff * skb)52 static 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 
espintcp_parse(struct strparser * strp,struct sk_buff * skb)112 static 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 
espintcp_recvmsg(struct sock * sk,struct msghdr * msg,size_t len,int flags,int * addr_len)133 static int espintcp_recvmsg(struct sock *sk, struct msghdr *msg, size_t len,
134 			    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 	skb = __skb_recv_datagram(sk, &ctx->ike_queue, flags, &off, &err);
143 	if (!skb) {
144 		if (err == -EAGAIN && sk->sk_shutdown & RCV_SHUTDOWN)
145 			return 0;
146 		return err;
147 	}
148 
149 	copied = len;
150 	if (copied > skb->len)
151 		copied = skb->len;
152 	else if (copied < skb->len)
153 		msg->msg_flags |= MSG_TRUNC;
154 
155 	err = skb_copy_datagram_msg(skb, 0, msg, copied);
156 	if (unlikely(err)) {
157 		kfree_skb(skb);
158 		return err;
159 	}
160 
161 	if (flags & MSG_TRUNC)
162 		copied = skb->len;
163 	kfree_skb(skb);
164 	return copied;
165 }
166 
espintcp_queue_out(struct sock * sk,struct sk_buff * skb)167 int espintcp_queue_out(struct sock *sk, struct sk_buff *skb)
168 {
169 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
170 
171 	if (skb_queue_len(&ctx->out_queue) >= READ_ONCE(netdev_max_backlog))
172 		return -ENOBUFS;
173 
174 	__skb_queue_tail(&ctx->out_queue, skb);
175 
176 	return 0;
177 }
178 EXPORT_SYMBOL_GPL(espintcp_queue_out);
179 
180 /* espintcp length field is 2B and length includes the length field's size */
181 #define MAX_ESPINTCP_MSG (((1 << 16) - 1) - 2)
182 
espintcp_sendskb_locked(struct sock * sk,struct espintcp_msg * emsg,int flags)183 static int espintcp_sendskb_locked(struct sock *sk, struct espintcp_msg *emsg,
184 				   int flags)
185 {
186 	do {
187 		int ret;
188 
189 		ret = skb_send_sock_locked(sk, emsg->skb,
190 					   emsg->offset, emsg->len);
191 		if (ret < 0)
192 			return ret;
193 
194 		emsg->len -= ret;
195 		emsg->offset += ret;
196 	} while (emsg->len > 0);
197 
198 	kfree_skb(emsg->skb);
199 	memset(emsg, 0, sizeof(*emsg));
200 
201 	return 0;
202 }
203 
espintcp_sendskmsg_locked(struct sock * sk,struct espintcp_msg * emsg,int flags)204 static int espintcp_sendskmsg_locked(struct sock *sk,
205 				     struct espintcp_msg *emsg, int flags)
206 {
207 	struct sk_msg *skmsg = &emsg->skmsg;
208 	struct scatterlist *sg;
209 	int done = 0;
210 	int ret;
211 
212 	flags |= MSG_SENDPAGE_NOTLAST;
213 	sg = &skmsg->sg.data[skmsg->sg.start];
214 	do {
215 		size_t size = sg->length - emsg->offset;
216 		int offset = sg->offset + emsg->offset;
217 		struct page *p;
218 
219 		emsg->offset = 0;
220 
221 		if (sg_is_last(sg))
222 			flags &= ~MSG_SENDPAGE_NOTLAST;
223 
224 		p = sg_page(sg);
225 retry:
226 		ret = do_tcp_sendpages(sk, p, offset, size, flags);
227 		if (ret < 0) {
228 			emsg->offset = offset - sg->offset;
229 			skmsg->sg.start += done;
230 			return ret;
231 		}
232 
233 		if (ret != size) {
234 			offset += ret;
235 			size -= ret;
236 			goto retry;
237 		}
238 
239 		done++;
240 		put_page(p);
241 		sk_mem_uncharge(sk, sg->length);
242 		sg = sg_next(sg);
243 	} while (sg);
244 
245 	memset(emsg, 0, sizeof(*emsg));
246 
247 	return 0;
248 }
249 
espintcp_push_msgs(struct sock * sk,int flags)250 static int espintcp_push_msgs(struct sock *sk, int flags)
251 {
252 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
253 	struct espintcp_msg *emsg = &ctx->partial;
254 	int err;
255 
256 	if (!emsg->len)
257 		return 0;
258 
259 	if (ctx->tx_running)
260 		return -EAGAIN;
261 	ctx->tx_running = 1;
262 
263 	if (emsg->skb)
264 		err = espintcp_sendskb_locked(sk, emsg, flags);
265 	else
266 		err = espintcp_sendskmsg_locked(sk, emsg, flags);
267 	if (err == -EAGAIN) {
268 		ctx->tx_running = 0;
269 		return flags & MSG_DONTWAIT ? -EAGAIN : 0;
270 	}
271 	if (!err)
272 		memset(emsg, 0, sizeof(*emsg));
273 
274 	ctx->tx_running = 0;
275 
276 	return err;
277 }
278 
espintcp_push_skb(struct sock * sk,struct sk_buff * skb)279 int espintcp_push_skb(struct sock *sk, struct sk_buff *skb)
280 {
281 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
282 	struct espintcp_msg *emsg = &ctx->partial;
283 	unsigned int len;
284 	int offset;
285 
286 	if (sk->sk_state != TCP_ESTABLISHED) {
287 		kfree_skb(skb);
288 		return -ECONNRESET;
289 	}
290 
291 	offset = skb_transport_offset(skb);
292 	len = skb->len - offset;
293 
294 	espintcp_push_msgs(sk, 0);
295 
296 	if (emsg->len) {
297 		kfree_skb(skb);
298 		return -ENOBUFS;
299 	}
300 
301 	skb_set_owner_w(skb, sk);
302 
303 	emsg->offset = offset;
304 	emsg->len = len;
305 	emsg->skb = skb;
306 
307 	espintcp_push_msgs(sk, 0);
308 
309 	return 0;
310 }
311 EXPORT_SYMBOL_GPL(espintcp_push_skb);
312 
espintcp_sendmsg(struct sock * sk,struct msghdr * msg,size_t size)313 static int espintcp_sendmsg(struct sock *sk, struct msghdr *msg, size_t size)
314 {
315 	long timeo = sock_sndtimeo(sk, msg->msg_flags & MSG_DONTWAIT);
316 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
317 	struct espintcp_msg *emsg = &ctx->partial;
318 	struct iov_iter pfx_iter;
319 	struct kvec pfx_iov = {};
320 	size_t msglen = size + 2;
321 	char buf[2] = {0};
322 	int err, end;
323 
324 	if (msg->msg_flags & ~MSG_DONTWAIT)
325 		return -EOPNOTSUPP;
326 
327 	if (size > MAX_ESPINTCP_MSG)
328 		return -EMSGSIZE;
329 
330 	if (msg->msg_controllen)
331 		return -EOPNOTSUPP;
332 
333 	lock_sock(sk);
334 
335 	err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
336 	if (err < 0) {
337 		if (err != -EAGAIN || !(msg->msg_flags & MSG_DONTWAIT))
338 			err = -ENOBUFS;
339 		goto unlock;
340 	}
341 
342 	sk_msg_init(&emsg->skmsg);
343 	while (1) {
344 		/* only -ENOMEM is possible since we don't coalesce */
345 		err = sk_msg_alloc(sk, &emsg->skmsg, msglen, 0);
346 		if (!err)
347 			break;
348 
349 		err = sk_stream_wait_memory(sk, &timeo);
350 		if (err)
351 			goto fail;
352 	}
353 
354 	*((__be16 *)buf) = cpu_to_be16(msglen);
355 	pfx_iov.iov_base = buf;
356 	pfx_iov.iov_len = sizeof(buf);
357 	iov_iter_kvec(&pfx_iter, WRITE, &pfx_iov, 1, pfx_iov.iov_len);
358 
359 	err = sk_msg_memcopy_from_iter(sk, &pfx_iter, &emsg->skmsg,
360 				       pfx_iov.iov_len);
361 	if (err < 0)
362 		goto fail;
363 
364 	err = sk_msg_memcopy_from_iter(sk, &msg->msg_iter, &emsg->skmsg, size);
365 	if (err < 0)
366 		goto fail;
367 
368 	end = emsg->skmsg.sg.end;
369 	emsg->len = size;
370 	sk_msg_iter_var_prev(end);
371 	sg_mark_end(sk_msg_elem(&emsg->skmsg, end));
372 
373 	tcp_rate_check_app_limited(sk);
374 
375 	err = espintcp_push_msgs(sk, msg->msg_flags & MSG_DONTWAIT);
376 	/* this message could be partially sent, keep it */
377 
378 	release_sock(sk);
379 
380 	return size;
381 
382 fail:
383 	sk_msg_free(sk, &emsg->skmsg);
384 	memset(emsg, 0, sizeof(*emsg));
385 unlock:
386 	release_sock(sk);
387 	return err;
388 }
389 
390 static struct proto espintcp_prot __ro_after_init;
391 static struct proto_ops espintcp_ops __ro_after_init;
392 static struct proto espintcp6_prot;
393 static struct proto_ops espintcp6_ops;
394 static DEFINE_MUTEX(tcpv6_prot_mutex);
395 
espintcp_data_ready(struct sock * sk)396 static void espintcp_data_ready(struct sock *sk)
397 {
398 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
399 
400 	strp_data_ready(&ctx->strp);
401 }
402 
espintcp_tx_work(struct work_struct * work)403 static void espintcp_tx_work(struct work_struct *work)
404 {
405 	struct espintcp_ctx *ctx = container_of(work,
406 						struct espintcp_ctx, work);
407 	struct sock *sk = ctx->strp.sk;
408 
409 	lock_sock(sk);
410 	if (!ctx->tx_running)
411 		espintcp_push_msgs(sk, 0);
412 	release_sock(sk);
413 }
414 
espintcp_write_space(struct sock * sk)415 static void espintcp_write_space(struct sock *sk)
416 {
417 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
418 
419 	schedule_work(&ctx->work);
420 	ctx->saved_write_space(sk);
421 }
422 
espintcp_destruct(struct sock * sk)423 static void espintcp_destruct(struct sock *sk)
424 {
425 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
426 
427 	ctx->saved_destruct(sk);
428 	kfree(ctx);
429 }
430 
tcp_is_ulp_esp(struct sock * sk)431 bool tcp_is_ulp_esp(struct sock *sk)
432 {
433 	return sk->sk_prot == &espintcp_prot || sk->sk_prot == &espintcp6_prot;
434 }
435 EXPORT_SYMBOL_GPL(tcp_is_ulp_esp);
436 
437 static void build_protos(struct proto *espintcp_prot,
438 			 struct proto_ops *espintcp_ops,
439 			 const struct proto *orig_prot,
440 			 const struct proto_ops *orig_ops);
espintcp_init_sk(struct sock * sk)441 static int espintcp_init_sk(struct sock *sk)
442 {
443 	struct inet_connection_sock *icsk = inet_csk(sk);
444 	struct strp_callbacks cb = {
445 		.rcv_msg = espintcp_rcv,
446 		.parse_msg = espintcp_parse,
447 	};
448 	struct espintcp_ctx *ctx;
449 	int err;
450 
451 	/* sockmap is not compatible with espintcp */
452 	if (sk->sk_user_data)
453 		return -EBUSY;
454 
455 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
456 	if (!ctx)
457 		return -ENOMEM;
458 
459 	err = strp_init(&ctx->strp, sk, &cb);
460 	if (err)
461 		goto free;
462 
463 	__sk_dst_reset(sk);
464 
465 	strp_check_rcv(&ctx->strp);
466 	skb_queue_head_init(&ctx->ike_queue);
467 	skb_queue_head_init(&ctx->out_queue);
468 
469 	if (sk->sk_family == AF_INET) {
470 		sk->sk_prot = &espintcp_prot;
471 		sk->sk_socket->ops = &espintcp_ops;
472 	} else {
473 		mutex_lock(&tcpv6_prot_mutex);
474 		if (!espintcp6_prot.recvmsg)
475 			build_protos(&espintcp6_prot, &espintcp6_ops, sk->sk_prot, sk->sk_socket->ops);
476 		mutex_unlock(&tcpv6_prot_mutex);
477 
478 		sk->sk_prot = &espintcp6_prot;
479 		sk->sk_socket->ops = &espintcp6_ops;
480 	}
481 	ctx->saved_data_ready = sk->sk_data_ready;
482 	ctx->saved_write_space = sk->sk_write_space;
483 	ctx->saved_destruct = sk->sk_destruct;
484 	sk->sk_data_ready = espintcp_data_ready;
485 	sk->sk_write_space = espintcp_write_space;
486 	sk->sk_destruct = espintcp_destruct;
487 	rcu_assign_pointer(icsk->icsk_ulp_data, ctx);
488 	INIT_WORK(&ctx->work, espintcp_tx_work);
489 
490 	/* avoid using task_frag */
491 	sk->sk_allocation = GFP_ATOMIC;
492 
493 	return 0;
494 
495 free:
496 	kfree(ctx);
497 	return err;
498 }
499 
espintcp_release(struct sock * sk)500 static void espintcp_release(struct sock *sk)
501 {
502 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
503 	struct sk_buff_head queue;
504 	struct sk_buff *skb;
505 
506 	__skb_queue_head_init(&queue);
507 	skb_queue_splice_init(&ctx->out_queue, &queue);
508 
509 	while ((skb = __skb_dequeue(&queue)))
510 		espintcp_push_skb(sk, skb);
511 
512 	tcp_release_cb(sk);
513 }
514 
espintcp_close(struct sock * sk,long timeout)515 static void espintcp_close(struct sock *sk, long timeout)
516 {
517 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
518 	struct espintcp_msg *emsg = &ctx->partial;
519 
520 	strp_stop(&ctx->strp);
521 
522 	sk->sk_prot = &tcp_prot;
523 	barrier();
524 
525 	cancel_work_sync(&ctx->work);
526 	strp_done(&ctx->strp);
527 
528 	skb_queue_purge(&ctx->out_queue);
529 	skb_queue_purge(&ctx->ike_queue);
530 
531 	if (emsg->len) {
532 		if (emsg->skb)
533 			kfree_skb(emsg->skb);
534 		else
535 			sk_msg_free(sk, &emsg->skmsg);
536 	}
537 
538 	tcp_close(sk, timeout);
539 }
540 
espintcp_poll(struct file * file,struct socket * sock,poll_table * wait)541 static __poll_t espintcp_poll(struct file *file, struct socket *sock,
542 			      poll_table *wait)
543 {
544 	__poll_t mask = datagram_poll(file, sock, wait);
545 	struct sock *sk = sock->sk;
546 	struct espintcp_ctx *ctx = espintcp_getctx(sk);
547 
548 	if (!skb_queue_empty(&ctx->ike_queue))
549 		mask |= EPOLLIN | EPOLLRDNORM;
550 
551 	return mask;
552 }
553 
build_protos(struct proto * espintcp_prot,struct proto_ops * espintcp_ops,const struct proto * orig_prot,const struct proto_ops * orig_ops)554 static void build_protos(struct proto *espintcp_prot,
555 			 struct proto_ops *espintcp_ops,
556 			 const struct proto *orig_prot,
557 			 const struct proto_ops *orig_ops)
558 {
559 	memcpy(espintcp_prot, orig_prot, sizeof(struct proto));
560 	memcpy(espintcp_ops, orig_ops, sizeof(struct proto_ops));
561 	espintcp_prot->sendmsg = espintcp_sendmsg;
562 	espintcp_prot->recvmsg = espintcp_recvmsg;
563 	espintcp_prot->close = espintcp_close;
564 	espintcp_prot->release_cb = espintcp_release;
565 	espintcp_ops->poll = espintcp_poll;
566 }
567 
568 static struct tcp_ulp_ops espintcp_ulp __read_mostly = {
569 	.name = "espintcp",
570 	.owner = THIS_MODULE,
571 	.init = espintcp_init_sk,
572 };
573 
espintcp_init(void)574 void __init espintcp_init(void)
575 {
576 	build_protos(&espintcp_prot, &espintcp_ops, &tcp_prot, &inet_stream_ops);
577 
578 	tcp_register_ulp(&espintcp_ulp);
579 }
580