espintcp.c 13 KB

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