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