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 10 static void handle_nonesp(struct espintcp_ctx *ctx, struct sk_buff *skb, 11 struct sock *sk) 12 { 13 if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf || 14 !sk_rmem_schedule(sk, skb, skb->truesize)) { 15 kfree_skb(skb); 16 return; 17 } 18 19 skb_set_owner_r(skb, sk); 20 21 memset(skb->cb, 0, sizeof(skb->cb)); 22 skb_queue_tail(&ctx->ike_queue, skb); 23 ctx->saved_data_ready(sk); 24 } 25 26 static void handle_esp(struct sk_buff *skb, struct sock *sk) 27 { 28 skb_reset_transport_header(skb); 29 memset(skb->cb, 0, sizeof(skb->cb)); 30 31 rcu_read_lock(); 32 skb->dev = dev_get_by_index_rcu(sock_net(sk), skb->skb_iif); 33 local_bh_disable(); 34 xfrm4_rcv_encap(skb, IPPROTO_ESP, 0, TCP_ENCAP_ESPINTCP); 35 local_bh_enable(); 36 rcu_read_unlock(); 37 } 38 39 static void espintcp_rcv(struct strparser *strp, struct sk_buff *skb) 40 { 41 struct espintcp_ctx *ctx = container_of(strp, struct espintcp_ctx, 42 strp); 43 struct strp_msg *rxm = strp_msg(skb); 44 u32 nonesp_marker; 45 int err; 46 47 err = skb_copy_bits(skb, rxm->offset + 2, &nonesp_marker, 48 sizeof(nonesp_marker)); 49 if (err < 0) { 50 kfree_skb(skb); 51 return; 52 } 53 54 /* remove header, leave non-ESP marker/SPI */ 55 if (!__pskb_pull(skb, rxm->offset + 2)) { 56 kfree_skb(skb); 57 return; 58 } 59 60 if (pskb_trim(skb, rxm->full_len - 2) != 0) { 61 kfree_skb(skb); 62 return; 63 } 64 65 if (nonesp_marker == 0) 66 handle_nonesp(ctx, skb, strp->sk); 67 else 68 handle_esp(skb, strp->sk); 69 } 70 71 static int espintcp_parse(struct strparser *strp, struct sk_buff *skb) 72 { 73 struct strp_msg *rxm = strp_msg(skb); 74 __be16 blen; 75 u16 len; 76 int err; 77 78 if (skb->len < rxm->offset + 2) 79 return 0; 80 81 err = skb_copy_bits(skb, rxm->offset, &blen, sizeof(blen)); 82 if (err < 0) 83 return err; 84 85 len = be16_to_cpu(blen); 86 if (len < 6) 87 return -EINVAL; 88 89 return len; 90 } 91 92 static int espintcp_recvmsg(struct sock *sk, struct msghdr *msg, size_t len, 93 int nonblock, int flags, int *addr_len) 94 { 95 struct espintcp_ctx *ctx = espintcp_getctx(sk); 96 struct sk_buff *skb; 97 int err = 0; 98 int copied; 99 int off = 0; 100 101 flags |= nonblock ? MSG_DONTWAIT : 0; 102 103 skb = __skb_recv_datagram(sk, &ctx->ike_queue, flags, NULL, &off, &err); 104 if (!skb) 105 return err; 106 107 copied = len; 108 if (copied > skb->len) 109 copied = skb->len; 110 else if (copied < skb->len) 111 msg->msg_flags |= MSG_TRUNC; 112 113 err = skb_copy_datagram_msg(skb, 0, msg, copied); 114 if (unlikely(err)) { 115 kfree_skb(skb); 116 return err; 117 } 118 119 if (flags & MSG_TRUNC) 120 copied = skb->len; 121 kfree_skb(skb); 122 return copied; 123 } 124 125 int espintcp_queue_out(struct sock *sk, struct sk_buff *skb) 126 { 127 struct espintcp_ctx *ctx = espintcp_getctx(sk); 128 129 if (skb_queue_len(&ctx->out_queue) >= netdev_max_backlog) 130 return -ENOBUFS; 131 132 __skb_queue_tail(&ctx->out_queue, skb); 133 134 return 0; 135 } 136 EXPORT_SYMBOL_GPL(espintcp_queue_out); 137 138 /* espintcp length field is 2B and length includes the length field's size */ 139 #define MAX_ESPINTCP_MSG (((1 << 16) - 1) - 2) 140 141 static int espintcp_sendskb_locked(struct sock *sk, struct espintcp_msg *emsg, 142 int flags) 143 { 144 do { 145 int ret; 146 147 ret = skb_send_sock_locked(sk, emsg->skb, 148 emsg->offset, emsg->len); 149 if (ret < 0) 150 return ret; 151 152 emsg->len -= ret; 153 emsg->offset += ret; 154 } while (emsg->len > 0); 155 156 kfree_skb(emsg->skb); 157 memset(emsg, 0, sizeof(*emsg)); 158 159 return 0; 160 } 161 162 static int espintcp_sendskmsg_locked(struct sock *sk, 163 struct espintcp_msg *emsg, int flags) 164 { 165 struct sk_msg *skmsg = &emsg->skmsg; 166 struct scatterlist *sg; 167 int done = 0; 168 int ret; 169 170 flags |= MSG_SENDPAGE_NOTLAST; 171 sg = &skmsg->sg.data[skmsg->sg.start]; 172 do { 173 size_t size = sg->length - emsg->offset; 174 int offset = sg->offset + emsg->offset; 175 struct page *p; 176 177 emsg->offset = 0; 178 179 if (sg_is_last(sg)) 180 flags &= ~MSG_SENDPAGE_NOTLAST; 181 182 p = sg_page(sg); 183 retry: 184 ret = do_tcp_sendpages(sk, p, offset, size, flags); 185 if (ret < 0) { 186 emsg->offset = offset - sg->offset; 187 skmsg->sg.start += done; 188 return ret; 189 } 190 191 if (ret != size) { 192 offset += ret; 193 size -= ret; 194 goto retry; 195 } 196 197 done++; 198 put_page(p); 199 sk_mem_uncharge(sk, sg->length); 200 sg = sg_next(sg); 201 } while (sg); 202 203 memset(emsg, 0, sizeof(*emsg)); 204 205 return 0; 206 } 207 208 static int espintcp_push_msgs(struct sock *sk) 209 { 210 struct espintcp_ctx *ctx = espintcp_getctx(sk); 211 struct espintcp_msg *emsg = &ctx->partial; 212 int err; 213 214 if (!emsg->len) 215 return 0; 216 217 if (ctx->tx_running) 218 return -EAGAIN; 219 ctx->tx_running = 1; 220 221 if (emsg->skb) 222 err = espintcp_sendskb_locked(sk, emsg, 0); 223 else 224 err = espintcp_sendskmsg_locked(sk, emsg, 0); 225 if (err == -EAGAIN) { 226 ctx->tx_running = 0; 227 return 0; 228 } 229 if (!err) 230 memset(emsg, 0, sizeof(*emsg)); 231 232 ctx->tx_running = 0; 233 234 return err; 235 } 236 237 int espintcp_push_skb(struct sock *sk, struct sk_buff *skb) 238 { 239 struct espintcp_ctx *ctx = espintcp_getctx(sk); 240 struct espintcp_msg *emsg = &ctx->partial; 241 unsigned int len; 242 int offset; 243 244 if (sk->sk_state != TCP_ESTABLISHED) { 245 kfree_skb(skb); 246 return -ECONNRESET; 247 } 248 249 offset = skb_transport_offset(skb); 250 len = skb->len - offset; 251 252 espintcp_push_msgs(sk); 253 254 if (emsg->len) { 255 kfree_skb(skb); 256 return -ENOBUFS; 257 } 258 259 skb_set_owner_w(skb, sk); 260 261 emsg->offset = offset; 262 emsg->len = len; 263 emsg->skb = skb; 264 265 espintcp_push_msgs(sk); 266 267 return 0; 268 } 269 EXPORT_SYMBOL_GPL(espintcp_push_skb); 270 271 static int espintcp_sendmsg(struct sock *sk, struct msghdr *msg, size_t size) 272 { 273 long timeo = sock_sndtimeo(sk, msg->msg_flags & MSG_DONTWAIT); 274 struct espintcp_ctx *ctx = espintcp_getctx(sk); 275 struct espintcp_msg *emsg = &ctx->partial; 276 struct iov_iter pfx_iter; 277 struct kvec pfx_iov = {}; 278 size_t msglen = size + 2; 279 char buf[2] = {0}; 280 int err, end; 281 282 if (msg->msg_flags) 283 return -EOPNOTSUPP; 284 285 if (size > MAX_ESPINTCP_MSG) 286 return -EMSGSIZE; 287 288 if (msg->msg_controllen) 289 return -EOPNOTSUPP; 290 291 lock_sock(sk); 292 293 err = espintcp_push_msgs(sk); 294 if (err < 0) { 295 err = -ENOBUFS; 296 goto unlock; 297 } 298 299 sk_msg_init(&emsg->skmsg); 300 while (1) { 301 /* only -ENOMEM is possible since we don't coalesce */ 302 err = sk_msg_alloc(sk, &emsg->skmsg, msglen, 0); 303 if (!err) 304 break; 305 306 err = sk_stream_wait_memory(sk, &timeo); 307 if (err) 308 goto fail; 309 } 310 311 *((__be16 *)buf) = cpu_to_be16(msglen); 312 pfx_iov.iov_base = buf; 313 pfx_iov.iov_len = sizeof(buf); 314 iov_iter_kvec(&pfx_iter, WRITE, &pfx_iov, 1, pfx_iov.iov_len); 315 316 err = sk_msg_memcopy_from_iter(sk, &pfx_iter, &emsg->skmsg, 317 pfx_iov.iov_len); 318 if (err < 0) 319 goto fail; 320 321 err = sk_msg_memcopy_from_iter(sk, &msg->msg_iter, &emsg->skmsg, size); 322 if (err < 0) 323 goto fail; 324 325 end = emsg->skmsg.sg.end; 326 emsg->len = size; 327 sk_msg_iter_var_prev(end); 328 sg_mark_end(sk_msg_elem(&emsg->skmsg, end)); 329 330 tcp_rate_check_app_limited(sk); 331 332 err = espintcp_push_msgs(sk); 333 /* this message could be partially sent, keep it */ 334 if (err < 0) 335 goto unlock; 336 release_sock(sk); 337 338 return size; 339 340 fail: 341 sk_msg_free(sk, &emsg->skmsg); 342 memset(emsg, 0, sizeof(*emsg)); 343 unlock: 344 release_sock(sk); 345 return err; 346 } 347 348 static struct proto espintcp_prot __ro_after_init; 349 static struct proto_ops espintcp_ops __ro_after_init; 350 351 static void espintcp_data_ready(struct sock *sk) 352 { 353 struct espintcp_ctx *ctx = espintcp_getctx(sk); 354 355 strp_data_ready(&ctx->strp); 356 } 357 358 static void espintcp_tx_work(struct work_struct *work) 359 { 360 struct espintcp_ctx *ctx = container_of(work, 361 struct espintcp_ctx, work); 362 struct sock *sk = ctx->strp.sk; 363 364 lock_sock(sk); 365 if (!ctx->tx_running) 366 espintcp_push_msgs(sk); 367 release_sock(sk); 368 } 369 370 static void espintcp_write_space(struct sock *sk) 371 { 372 struct espintcp_ctx *ctx = espintcp_getctx(sk); 373 374 schedule_work(&ctx->work); 375 ctx->saved_write_space(sk); 376 } 377 378 static void espintcp_destruct(struct sock *sk) 379 { 380 struct espintcp_ctx *ctx = espintcp_getctx(sk); 381 382 kfree(ctx); 383 } 384 385 bool tcp_is_ulp_esp(struct sock *sk) 386 { 387 return sk->sk_prot == &espintcp_prot; 388 } 389 EXPORT_SYMBOL_GPL(tcp_is_ulp_esp); 390 391 static int espintcp_init_sk(struct sock *sk) 392 { 393 struct inet_connection_sock *icsk = inet_csk(sk); 394 struct strp_callbacks cb = { 395 .rcv_msg = espintcp_rcv, 396 .parse_msg = espintcp_parse, 397 }; 398 struct espintcp_ctx *ctx; 399 int err; 400 401 /* sockmap is not compatible with espintcp */ 402 if (sk->sk_user_data) 403 return -EBUSY; 404 405 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL); 406 if (!ctx) 407 return -ENOMEM; 408 409 err = strp_init(&ctx->strp, sk, &cb); 410 if (err) 411 goto free; 412 413 __sk_dst_reset(sk); 414 415 strp_check_rcv(&ctx->strp); 416 skb_queue_head_init(&ctx->ike_queue); 417 skb_queue_head_init(&ctx->out_queue); 418 sk->sk_prot = &espintcp_prot; 419 sk->sk_socket->ops = &espintcp_ops; 420 ctx->saved_data_ready = sk->sk_data_ready; 421 ctx->saved_write_space = sk->sk_write_space; 422 sk->sk_data_ready = espintcp_data_ready; 423 sk->sk_write_space = espintcp_write_space; 424 sk->sk_destruct = espintcp_destruct; 425 rcu_assign_pointer(icsk->icsk_ulp_data, ctx); 426 INIT_WORK(&ctx->work, espintcp_tx_work); 427 428 /* avoid using task_frag */ 429 sk->sk_allocation = GFP_ATOMIC; 430 431 return 0; 432 433 free: 434 kfree(ctx); 435 return err; 436 } 437 438 static void espintcp_release(struct sock *sk) 439 { 440 struct espintcp_ctx *ctx = espintcp_getctx(sk); 441 struct sk_buff_head queue; 442 struct sk_buff *skb; 443 444 __skb_queue_head_init(&queue); 445 skb_queue_splice_init(&ctx->out_queue, &queue); 446 447 while ((skb = __skb_dequeue(&queue))) 448 espintcp_push_skb(sk, skb); 449 450 tcp_release_cb(sk); 451 } 452 453 static void espintcp_close(struct sock *sk, long timeout) 454 { 455 struct espintcp_ctx *ctx = espintcp_getctx(sk); 456 struct espintcp_msg *emsg = &ctx->partial; 457 458 strp_stop(&ctx->strp); 459 460 sk->sk_prot = &tcp_prot; 461 barrier(); 462 463 cancel_work_sync(&ctx->work); 464 strp_done(&ctx->strp); 465 466 skb_queue_purge(&ctx->out_queue); 467 skb_queue_purge(&ctx->ike_queue); 468 469 if (emsg->len) { 470 if (emsg->skb) 471 kfree_skb(emsg->skb); 472 else 473 sk_msg_free(sk, &emsg->skmsg); 474 } 475 476 tcp_close(sk, timeout); 477 } 478 479 static __poll_t espintcp_poll(struct file *file, struct socket *sock, 480 poll_table *wait) 481 { 482 __poll_t mask = datagram_poll(file, sock, wait); 483 struct sock *sk = sock->sk; 484 struct espintcp_ctx *ctx = espintcp_getctx(sk); 485 486 if (!skb_queue_empty(&ctx->ike_queue)) 487 mask |= EPOLLIN | EPOLLRDNORM; 488 489 return mask; 490 } 491 492 static struct tcp_ulp_ops espintcp_ulp __read_mostly = { 493 .name = "espintcp", 494 .owner = THIS_MODULE, 495 .init = espintcp_init_sk, 496 }; 497 498 void __init espintcp_init(void) 499 { 500 memcpy(&espintcp_prot, &tcp_prot, sizeof(tcp_prot)); 501 memcpy(&espintcp_ops, &inet_stream_ops, sizeof(inet_stream_ops)); 502 espintcp_prot.sendmsg = espintcp_sendmsg; 503 espintcp_prot.recvmsg = espintcp_recvmsg; 504 espintcp_prot.close = espintcp_close; 505 espintcp_prot.release_cb = espintcp_release; 506 espintcp_ops.poll = espintcp_poll; 507 508 tcp_register_ulp(&espintcp_ulp); 509 } 510