1 #include <linux/module.h>
2 #include <linux/errno.h>
3 #include <linux/socket.h>
4 #include <linux/skbuff.h>
5 #include <linux/ip.h>
6 #include <linux/udp.h>
7 #include <linux/types.h>
8 #include <linux/kernel.h>
9 #include <net/genetlink.h>
10 #include <net/gue.h>
11 #include <net/ip.h>
12 #include <net/protocol.h>
13 #include <net/udp.h>
14 #include <net/udp_tunnel.h>
15 #include <net/xfrm.h>
16 #include <uapi/linux/fou.h>
17 #include <uapi/linux/genetlink.h>
18
19 struct fou {
20 struct socket *sock;
21 u8 protocol;
22 u8 flags;
23 __be16 port;
24 u16 type;
25 struct udp_offload udp_offloads;
26 struct list_head list;
27 struct rcu_head rcu;
28 };
29
30 #define FOU_F_REMCSUM_NOPARTIAL BIT(0)
31
32 struct fou_cfg {
33 u16 type;
34 u8 protocol;
35 u8 flags;
36 struct udp_port_cfg udp_config;
37 };
38
39 static unsigned int fou_net_id;
40
41 struct fou_net {
42 struct list_head fou_list;
43 struct mutex fou_lock;
44 };
45
fou_from_sock(struct sock * sk)46 static inline struct fou *fou_from_sock(struct sock *sk)
47 {
48 return sk->sk_user_data;
49 }
50
fou_recv_pull(struct sk_buff * skb,size_t len)51 static void fou_recv_pull(struct sk_buff *skb, size_t len)
52 {
53 struct iphdr *iph = ip_hdr(skb);
54
55 /* Remove 'len' bytes from the packet (UDP header and
56 * FOU header if present).
57 */
58 iph->tot_len = htons(ntohs(iph->tot_len) - len);
59 __skb_pull(skb, len);
60 skb_postpull_rcsum(skb, udp_hdr(skb), len);
61 skb_reset_transport_header(skb);
62 }
63
fou_udp_recv(struct sock * sk,struct sk_buff * skb)64 static int fou_udp_recv(struct sock *sk, struct sk_buff *skb)
65 {
66 struct fou *fou = fou_from_sock(sk);
67
68 if (!fou)
69 return 1;
70
71 fou_recv_pull(skb, sizeof(struct udphdr));
72
73 return -fou->protocol;
74 }
75
gue_remcsum(struct sk_buff * skb,struct guehdr * guehdr,void * data,size_t hdrlen,u8 ipproto,bool nopartial)76 static struct guehdr *gue_remcsum(struct sk_buff *skb, struct guehdr *guehdr,
77 void *data, size_t hdrlen, u8 ipproto,
78 bool nopartial)
79 {
80 __be16 *pd = data;
81 size_t start = ntohs(pd[0]);
82 size_t offset = ntohs(pd[1]);
83 size_t plen = sizeof(struct udphdr) + hdrlen +
84 max_t(size_t, offset + sizeof(u16), start);
85
86 if (skb->remcsum_offload)
87 return guehdr;
88
89 if (!pskb_may_pull(skb, plen))
90 return NULL;
91 guehdr = (struct guehdr *)&udp_hdr(skb)[1];
92
93 skb_remcsum_process(skb, (void *)guehdr + hdrlen,
94 start, offset, nopartial);
95
96 return guehdr;
97 }
98
gue_control_message(struct sk_buff * skb,struct guehdr * guehdr)99 static int gue_control_message(struct sk_buff *skb, struct guehdr *guehdr)
100 {
101 /* No support yet */
102 kfree_skb(skb);
103 return 0;
104 }
105
gue_udp_recv(struct sock * sk,struct sk_buff * skb)106 static int gue_udp_recv(struct sock *sk, struct sk_buff *skb)
107 {
108 struct fou *fou = fou_from_sock(sk);
109 size_t len, optlen, hdrlen;
110 struct guehdr *guehdr;
111 void *data;
112 u16 doffset = 0;
113
114 if (!fou)
115 return 1;
116
117 len = sizeof(struct udphdr) + sizeof(struct guehdr);
118 if (!pskb_may_pull(skb, len))
119 goto drop;
120
121 guehdr = (struct guehdr *)&udp_hdr(skb)[1];
122
123 optlen = guehdr->hlen << 2;
124 len += optlen;
125
126 if (!pskb_may_pull(skb, len))
127 goto drop;
128
129 /* guehdr may change after pull */
130 guehdr = (struct guehdr *)&udp_hdr(skb)[1];
131
132 hdrlen = sizeof(struct guehdr) + optlen;
133
134 if (guehdr->version != 0 || validate_gue_flags(guehdr, optlen))
135 goto drop;
136
137 hdrlen = sizeof(struct guehdr) + optlen;
138
139 ip_hdr(skb)->tot_len = htons(ntohs(ip_hdr(skb)->tot_len) - len);
140
141 /* Pull csum through the guehdr now . This can be used if
142 * there is a remote checksum offload.
143 */
144 skb_postpull_rcsum(skb, udp_hdr(skb), len);
145
146 data = &guehdr[1];
147
148 if (guehdr->flags & GUE_FLAG_PRIV) {
149 __be32 flags = *(__be32 *)(data + doffset);
150
151 doffset += GUE_LEN_PRIV;
152
153 if (flags & GUE_PFLAG_REMCSUM) {
154 guehdr = gue_remcsum(skb, guehdr, data + doffset,
155 hdrlen, guehdr->proto_ctype,
156 !!(fou->flags &
157 FOU_F_REMCSUM_NOPARTIAL));
158 if (!guehdr)
159 goto drop;
160
161 data = &guehdr[1];
162
163 doffset += GUE_PLEN_REMCSUM;
164 }
165 }
166
167 if (unlikely(guehdr->control))
168 return gue_control_message(skb, guehdr);
169
170 __skb_pull(skb, sizeof(struct udphdr) + hdrlen);
171 skb_reset_transport_header(skb);
172
173 return -guehdr->proto_ctype;
174
175 drop:
176 kfree_skb(skb);
177 return 0;
178 }
179
fou_gro_receive(struct sk_buff ** head,struct sk_buff * skb,struct udp_offload * uoff)180 static struct sk_buff **fou_gro_receive(struct sk_buff **head,
181 struct sk_buff *skb,
182 struct udp_offload *uoff)
183 {
184 const struct net_offload *ops;
185 struct sk_buff **pp = NULL;
186 u8 proto = NAPI_GRO_CB(skb)->proto;
187 const struct net_offload **offloads;
188
189 rcu_read_lock();
190 offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
191 ops = rcu_dereference(offloads[proto]);
192 if (!ops || !ops->callbacks.gro_receive)
193 goto out_unlock;
194
195 pp = ops->callbacks.gro_receive(head, skb);
196
197 out_unlock:
198 rcu_read_unlock();
199
200 return pp;
201 }
202
fou_gro_complete(struct sk_buff * skb,int nhoff,struct udp_offload * uoff)203 static int fou_gro_complete(struct sk_buff *skb, int nhoff,
204 struct udp_offload *uoff)
205 {
206 const struct net_offload *ops;
207 u8 proto = NAPI_GRO_CB(skb)->proto;
208 int err = -ENOSYS;
209 const struct net_offload **offloads;
210
211 udp_tunnel_gro_complete(skb, nhoff);
212
213 rcu_read_lock();
214 offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
215 ops = rcu_dereference(offloads[proto]);
216 if (WARN_ON(!ops || !ops->callbacks.gro_complete))
217 goto out_unlock;
218
219 err = ops->callbacks.gro_complete(skb, nhoff);
220
221 out_unlock:
222 rcu_read_unlock();
223
224 return err;
225 }
226
gue_gro_remcsum(struct sk_buff * skb,unsigned int off,struct guehdr * guehdr,void * data,size_t hdrlen,struct gro_remcsum * grc,bool nopartial)227 static struct guehdr *gue_gro_remcsum(struct sk_buff *skb, unsigned int off,
228 struct guehdr *guehdr, void *data,
229 size_t hdrlen, struct gro_remcsum *grc,
230 bool nopartial)
231 {
232 __be16 *pd = data;
233 size_t start = ntohs(pd[0]);
234 size_t offset = ntohs(pd[1]);
235
236 if (skb->remcsum_offload)
237 return guehdr;
238
239 if (!NAPI_GRO_CB(skb)->csum_valid)
240 return NULL;
241
242 guehdr = skb_gro_remcsum_process(skb, (void *)guehdr, off, hdrlen,
243 start, offset, grc, nopartial);
244
245 skb->remcsum_offload = 1;
246
247 return guehdr;
248 }
249
gue_gro_receive(struct sk_buff ** head,struct sk_buff * skb,struct udp_offload * uoff)250 static struct sk_buff **gue_gro_receive(struct sk_buff **head,
251 struct sk_buff *skb,
252 struct udp_offload *uoff)
253 {
254 const struct net_offload **offloads;
255 const struct net_offload *ops;
256 struct sk_buff **pp = NULL;
257 struct sk_buff *p;
258 struct guehdr *guehdr;
259 size_t len, optlen, hdrlen, off;
260 void *data;
261 u16 doffset = 0;
262 int flush = 1;
263 struct fou *fou = container_of(uoff, struct fou, udp_offloads);
264 struct gro_remcsum grc;
265
266 skb_gro_remcsum_init(&grc);
267
268 off = skb_gro_offset(skb);
269 len = off + sizeof(*guehdr);
270
271 guehdr = skb_gro_header_fast(skb, off);
272 if (skb_gro_header_hard(skb, len)) {
273 guehdr = skb_gro_header_slow(skb, len, off);
274 if (unlikely(!guehdr))
275 goto out;
276 }
277
278 optlen = guehdr->hlen << 2;
279 len += optlen;
280
281 if (skb_gro_header_hard(skb, len)) {
282 guehdr = skb_gro_header_slow(skb, len, off);
283 if (unlikely(!guehdr))
284 goto out;
285 }
286
287 if (unlikely(guehdr->control) || guehdr->version != 0 ||
288 validate_gue_flags(guehdr, optlen))
289 goto out;
290
291 hdrlen = sizeof(*guehdr) + optlen;
292
293 /* Adjust NAPI_GRO_CB(skb)->csum to account for guehdr,
294 * this is needed if there is a remote checkcsum offload.
295 */
296 skb_gro_postpull_rcsum(skb, guehdr, hdrlen);
297
298 data = &guehdr[1];
299
300 if (guehdr->flags & GUE_FLAG_PRIV) {
301 __be32 flags = *(__be32 *)(data + doffset);
302
303 doffset += GUE_LEN_PRIV;
304
305 if (flags & GUE_PFLAG_REMCSUM) {
306 guehdr = gue_gro_remcsum(skb, off, guehdr,
307 data + doffset, hdrlen, &grc,
308 !!(fou->flags &
309 FOU_F_REMCSUM_NOPARTIAL));
310
311 if (!guehdr)
312 goto out;
313
314 data = &guehdr[1];
315
316 doffset += GUE_PLEN_REMCSUM;
317 }
318 }
319
320 skb_gro_pull(skb, hdrlen);
321
322 flush = 0;
323
324 for (p = *head; p; p = p->next) {
325 const struct guehdr *guehdr2;
326
327 if (!NAPI_GRO_CB(p)->same_flow)
328 continue;
329
330 guehdr2 = (struct guehdr *)(p->data + off);
331
332 /* Compare base GUE header to be equal (covers
333 * hlen, version, proto_ctype, and flags.
334 */
335 if (guehdr->word != guehdr2->word) {
336 NAPI_GRO_CB(p)->same_flow = 0;
337 continue;
338 }
339
340 /* Compare optional fields are the same. */
341 if (guehdr->hlen && memcmp(&guehdr[1], &guehdr2[1],
342 guehdr->hlen << 2)) {
343 NAPI_GRO_CB(p)->same_flow = 0;
344 continue;
345 }
346 }
347
348 rcu_read_lock();
349 offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
350 ops = rcu_dereference(offloads[guehdr->proto_ctype]);
351 if (WARN_ON_ONCE(!ops || !ops->callbacks.gro_receive))
352 goto out_unlock;
353
354 pp = ops->callbacks.gro_receive(head, skb);
355
356 out_unlock:
357 rcu_read_unlock();
358 out:
359 NAPI_GRO_CB(skb)->flush |= flush;
360 skb_gro_remcsum_cleanup(skb, &grc);
361
362 return pp;
363 }
364
gue_gro_complete(struct sk_buff * skb,int nhoff,struct udp_offload * uoff)365 static int gue_gro_complete(struct sk_buff *skb, int nhoff,
366 struct udp_offload *uoff)
367 {
368 const struct net_offload **offloads;
369 struct guehdr *guehdr = (struct guehdr *)(skb->data + nhoff);
370 const struct net_offload *ops;
371 unsigned int guehlen;
372 u8 proto;
373 int err = -ENOENT;
374
375 proto = guehdr->proto_ctype;
376
377 guehlen = sizeof(*guehdr) + (guehdr->hlen << 2);
378
379 rcu_read_lock();
380 offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
381 ops = rcu_dereference(offloads[proto]);
382 if (WARN_ON(!ops || !ops->callbacks.gro_complete))
383 goto out_unlock;
384
385 err = ops->callbacks.gro_complete(skb, nhoff + guehlen);
386
387 out_unlock:
388 rcu_read_unlock();
389 return err;
390 }
391
fou_add_to_port_list(struct net * net,struct fou * fou)392 static int fou_add_to_port_list(struct net *net, struct fou *fou)
393 {
394 struct fou_net *fn = net_generic(net, fou_net_id);
395 struct fou *fout;
396
397 mutex_lock(&fn->fou_lock);
398 list_for_each_entry(fout, &fn->fou_list, list) {
399 if (fou->port == fout->port) {
400 mutex_unlock(&fn->fou_lock);
401 return -EALREADY;
402 }
403 }
404
405 list_add(&fou->list, &fn->fou_list);
406 mutex_unlock(&fn->fou_lock);
407
408 return 0;
409 }
410
fou_release(struct fou * fou)411 static void fou_release(struct fou *fou)
412 {
413 struct socket *sock = fou->sock;
414 struct sock *sk = sock->sk;
415
416 if (sk->sk_family == AF_INET)
417 udp_del_offload(&fou->udp_offloads);
418 list_del(&fou->list);
419 udp_tunnel_sock_release(sock);
420
421 kfree_rcu(fou, rcu);
422 }
423
fou_encap_init(struct sock * sk,struct fou * fou,struct fou_cfg * cfg)424 static int fou_encap_init(struct sock *sk, struct fou *fou, struct fou_cfg *cfg)
425 {
426 udp_sk(sk)->encap_rcv = fou_udp_recv;
427 fou->protocol = cfg->protocol;
428 fou->udp_offloads.callbacks.gro_receive = fou_gro_receive;
429 fou->udp_offloads.callbacks.gro_complete = fou_gro_complete;
430 fou->udp_offloads.port = cfg->udp_config.local_udp_port;
431 fou->udp_offloads.ipproto = cfg->protocol;
432
433 return 0;
434 }
435
gue_encap_init(struct sock * sk,struct fou * fou,struct fou_cfg * cfg)436 static int gue_encap_init(struct sock *sk, struct fou *fou, struct fou_cfg *cfg)
437 {
438 udp_sk(sk)->encap_rcv = gue_udp_recv;
439 fou->udp_offloads.callbacks.gro_receive = gue_gro_receive;
440 fou->udp_offloads.callbacks.gro_complete = gue_gro_complete;
441 fou->udp_offloads.port = cfg->udp_config.local_udp_port;
442
443 return 0;
444 }
445
fou_create(struct net * net,struct fou_cfg * cfg,struct socket ** sockp)446 static int fou_create(struct net *net, struct fou_cfg *cfg,
447 struct socket **sockp)
448 {
449 struct socket *sock = NULL;
450 struct fou *fou = NULL;
451 struct sock *sk;
452 int err;
453
454 /* Open UDP socket */
455 err = udp_sock_create(net, &cfg->udp_config, &sock);
456 if (err < 0)
457 goto error;
458
459 /* Allocate FOU port structure */
460 fou = kzalloc(sizeof(*fou), GFP_KERNEL);
461 if (!fou) {
462 err = -ENOMEM;
463 goto error;
464 }
465
466 sk = sock->sk;
467
468 fou->flags = cfg->flags;
469 fou->port = cfg->udp_config.local_udp_port;
470
471 /* Initial for fou type */
472 switch (cfg->type) {
473 case FOU_ENCAP_DIRECT:
474 err = fou_encap_init(sk, fou, cfg);
475 if (err)
476 goto error;
477 break;
478 case FOU_ENCAP_GUE:
479 err = gue_encap_init(sk, fou, cfg);
480 if (err)
481 goto error;
482 break;
483 default:
484 err = -EINVAL;
485 goto error;
486 }
487
488 fou->type = cfg->type;
489
490 udp_sk(sk)->encap_type = 1;
491 udp_encap_enable();
492
493 sk->sk_user_data = fou;
494 fou->sock = sock;
495
496 inet_inc_convert_csum(sk);
497
498 sk->sk_allocation = GFP_ATOMIC;
499
500 if (cfg->udp_config.family == AF_INET) {
501 err = udp_add_offload(&fou->udp_offloads);
502 if (err)
503 goto error;
504 }
505
506 err = fou_add_to_port_list(net, fou);
507 if (err)
508 goto error;
509
510 if (sockp)
511 *sockp = sock;
512
513 return 0;
514
515 error:
516 kfree(fou);
517 if (sock)
518 udp_tunnel_sock_release(sock);
519
520 return err;
521 }
522
fou_destroy(struct net * net,struct fou_cfg * cfg)523 static int fou_destroy(struct net *net, struct fou_cfg *cfg)
524 {
525 struct fou_net *fn = net_generic(net, fou_net_id);
526 __be16 port = cfg->udp_config.local_udp_port;
527 int err = -EINVAL;
528 struct fou *fou;
529
530 mutex_lock(&fn->fou_lock);
531 list_for_each_entry(fou, &fn->fou_list, list) {
532 if (fou->port == port) {
533 fou_release(fou);
534 err = 0;
535 break;
536 }
537 }
538 mutex_unlock(&fn->fou_lock);
539
540 return err;
541 }
542
543 static struct genl_family fou_nl_family = {
544 .id = GENL_ID_GENERATE,
545 .hdrsize = 0,
546 .name = FOU_GENL_NAME,
547 .version = FOU_GENL_VERSION,
548 .maxattr = FOU_ATTR_MAX,
549 .netnsok = true,
550 };
551
552 static struct nla_policy fou_nl_policy[FOU_ATTR_MAX + 1] = {
553 [FOU_ATTR_PORT] = { .type = NLA_U16, },
554 [FOU_ATTR_AF] = { .type = NLA_U8, },
555 [FOU_ATTR_IPPROTO] = { .type = NLA_U8, },
556 [FOU_ATTR_TYPE] = { .type = NLA_U8, },
557 [FOU_ATTR_REMCSUM_NOPARTIAL] = { .type = NLA_FLAG, },
558 };
559
parse_nl_config(struct genl_info * info,struct fou_cfg * cfg)560 static int parse_nl_config(struct genl_info *info,
561 struct fou_cfg *cfg)
562 {
563 memset(cfg, 0, sizeof(*cfg));
564
565 cfg->udp_config.family = AF_INET;
566
567 if (info->attrs[FOU_ATTR_AF]) {
568 u8 family = nla_get_u8(info->attrs[FOU_ATTR_AF]);
569
570 if (family != AF_INET)
571 return -EINVAL;
572
573 cfg->udp_config.family = family;
574 }
575
576 if (info->attrs[FOU_ATTR_PORT]) {
577 __be16 port = nla_get_be16(info->attrs[FOU_ATTR_PORT]);
578
579 cfg->udp_config.local_udp_port = port;
580 }
581
582 if (info->attrs[FOU_ATTR_IPPROTO])
583 cfg->protocol = nla_get_u8(info->attrs[FOU_ATTR_IPPROTO]);
584
585 if (info->attrs[FOU_ATTR_TYPE])
586 cfg->type = nla_get_u8(info->attrs[FOU_ATTR_TYPE]);
587
588 if (info->attrs[FOU_ATTR_REMCSUM_NOPARTIAL])
589 cfg->flags |= FOU_F_REMCSUM_NOPARTIAL;
590
591 return 0;
592 }
593
fou_nl_cmd_add_port(struct sk_buff * skb,struct genl_info * info)594 static int fou_nl_cmd_add_port(struct sk_buff *skb, struct genl_info *info)
595 {
596 struct net *net = genl_info_net(info);
597 struct fou_cfg cfg;
598 int err;
599
600 err = parse_nl_config(info, &cfg);
601 if (err)
602 return err;
603
604 return fou_create(net, &cfg, NULL);
605 }
606
fou_nl_cmd_rm_port(struct sk_buff * skb,struct genl_info * info)607 static int fou_nl_cmd_rm_port(struct sk_buff *skb, struct genl_info *info)
608 {
609 struct net *net = genl_info_net(info);
610 struct fou_cfg cfg;
611 int err;
612
613 err = parse_nl_config(info, &cfg);
614 if (err)
615 return err;
616
617 return fou_destroy(net, &cfg);
618 }
619
fou_fill_info(struct fou * fou,struct sk_buff * msg)620 static int fou_fill_info(struct fou *fou, struct sk_buff *msg)
621 {
622 if (nla_put_u8(msg, FOU_ATTR_AF, fou->sock->sk->sk_family) ||
623 nla_put_be16(msg, FOU_ATTR_PORT, fou->port) ||
624 nla_put_u8(msg, FOU_ATTR_IPPROTO, fou->protocol) ||
625 nla_put_u8(msg, FOU_ATTR_TYPE, fou->type))
626 return -1;
627
628 if (fou->flags & FOU_F_REMCSUM_NOPARTIAL)
629 if (nla_put_flag(msg, FOU_ATTR_REMCSUM_NOPARTIAL))
630 return -1;
631 return 0;
632 }
633
fou_dump_info(struct fou * fou,u32 portid,u32 seq,u32 flags,struct sk_buff * skb,u8 cmd)634 static int fou_dump_info(struct fou *fou, u32 portid, u32 seq,
635 u32 flags, struct sk_buff *skb, u8 cmd)
636 {
637 void *hdr;
638
639 hdr = genlmsg_put(skb, portid, seq, &fou_nl_family, flags, cmd);
640 if (!hdr)
641 return -ENOMEM;
642
643 if (fou_fill_info(fou, skb) < 0)
644 goto nla_put_failure;
645
646 genlmsg_end(skb, hdr);
647 return 0;
648
649 nla_put_failure:
650 genlmsg_cancel(skb, hdr);
651 return -EMSGSIZE;
652 }
653
fou_nl_cmd_get_port(struct sk_buff * skb,struct genl_info * info)654 static int fou_nl_cmd_get_port(struct sk_buff *skb, struct genl_info *info)
655 {
656 struct net *net = genl_info_net(info);
657 struct fou_net *fn = net_generic(net, fou_net_id);
658 struct sk_buff *msg;
659 struct fou_cfg cfg;
660 struct fou *fout;
661 __be16 port;
662 int ret;
663
664 ret = parse_nl_config(info, &cfg);
665 if (ret)
666 return ret;
667 port = cfg.udp_config.local_udp_port;
668 if (port == 0)
669 return -EINVAL;
670
671 msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
672 if (!msg)
673 return -ENOMEM;
674
675 ret = -ESRCH;
676 mutex_lock(&fn->fou_lock);
677 list_for_each_entry(fout, &fn->fou_list, list) {
678 if (port == fout->port) {
679 ret = fou_dump_info(fout, info->snd_portid,
680 info->snd_seq, 0, msg,
681 info->genlhdr->cmd);
682 break;
683 }
684 }
685 mutex_unlock(&fn->fou_lock);
686 if (ret < 0)
687 goto out_free;
688
689 return genlmsg_reply(msg, info);
690
691 out_free:
692 nlmsg_free(msg);
693 return ret;
694 }
695
fou_nl_dump(struct sk_buff * skb,struct netlink_callback * cb)696 static int fou_nl_dump(struct sk_buff *skb, struct netlink_callback *cb)
697 {
698 struct net *net = sock_net(skb->sk);
699 struct fou_net *fn = net_generic(net, fou_net_id);
700 struct fou *fout;
701 int idx = 0, ret;
702
703 mutex_lock(&fn->fou_lock);
704 list_for_each_entry(fout, &fn->fou_list, list) {
705 if (idx++ < cb->args[0])
706 continue;
707 ret = fou_dump_info(fout, NETLINK_CB(cb->skb).portid,
708 cb->nlh->nlmsg_seq, NLM_F_MULTI,
709 skb, FOU_CMD_GET);
710 if (ret)
711 break;
712 }
713 mutex_unlock(&fn->fou_lock);
714
715 cb->args[0] = idx;
716 return skb->len;
717 }
718
719 static const struct genl_ops fou_nl_ops[] = {
720 {
721 .cmd = FOU_CMD_ADD,
722 .doit = fou_nl_cmd_add_port,
723 .policy = fou_nl_policy,
724 .flags = GENL_ADMIN_PERM,
725 },
726 {
727 .cmd = FOU_CMD_DEL,
728 .doit = fou_nl_cmd_rm_port,
729 .policy = fou_nl_policy,
730 .flags = GENL_ADMIN_PERM,
731 },
732 {
733 .cmd = FOU_CMD_GET,
734 .doit = fou_nl_cmd_get_port,
735 .dumpit = fou_nl_dump,
736 .policy = fou_nl_policy,
737 },
738 };
739
fou_encap_hlen(struct ip_tunnel_encap * e)740 size_t fou_encap_hlen(struct ip_tunnel_encap *e)
741 {
742 return sizeof(struct udphdr);
743 }
744 EXPORT_SYMBOL(fou_encap_hlen);
745
gue_encap_hlen(struct ip_tunnel_encap * e)746 size_t gue_encap_hlen(struct ip_tunnel_encap *e)
747 {
748 size_t len;
749 bool need_priv = false;
750
751 len = sizeof(struct udphdr) + sizeof(struct guehdr);
752
753 if (e->flags & TUNNEL_ENCAP_FLAG_REMCSUM) {
754 len += GUE_PLEN_REMCSUM;
755 need_priv = true;
756 }
757
758 len += need_priv ? GUE_LEN_PRIV : 0;
759
760 return len;
761 }
762 EXPORT_SYMBOL(gue_encap_hlen);
763
fou_build_udp(struct sk_buff * skb,struct ip_tunnel_encap * e,struct flowi4 * fl4,u8 * protocol,__be16 sport)764 static void fou_build_udp(struct sk_buff *skb, struct ip_tunnel_encap *e,
765 struct flowi4 *fl4, u8 *protocol, __be16 sport)
766 {
767 struct udphdr *uh;
768
769 skb_push(skb, sizeof(struct udphdr));
770 skb_reset_transport_header(skb);
771
772 uh = udp_hdr(skb);
773
774 uh->dest = e->dport;
775 uh->source = sport;
776 uh->len = htons(skb->len);
777 uh->check = 0;
778 udp_set_csum(!(e->flags & TUNNEL_ENCAP_FLAG_CSUM), skb,
779 fl4->saddr, fl4->daddr, skb->len);
780
781 *protocol = IPPROTO_UDP;
782 }
783
fou_build_header(struct sk_buff * skb,struct ip_tunnel_encap * e,u8 * protocol,struct flowi4 * fl4)784 int fou_build_header(struct sk_buff *skb, struct ip_tunnel_encap *e,
785 u8 *protocol, struct flowi4 *fl4)
786 {
787 bool csum = !!(e->flags & TUNNEL_ENCAP_FLAG_CSUM);
788 int type = csum ? SKB_GSO_UDP_TUNNEL_CSUM : SKB_GSO_UDP_TUNNEL;
789 __be16 sport;
790
791 skb = iptunnel_handle_offloads(skb, csum, type);
792
793 if (IS_ERR(skb))
794 return PTR_ERR(skb);
795
796 sport = e->sport ? : udp_flow_src_port(dev_net(skb->dev),
797 skb, 0, 0, false);
798 fou_build_udp(skb, e, fl4, protocol, sport);
799
800 return 0;
801 }
802 EXPORT_SYMBOL(fou_build_header);
803
gue_build_header(struct sk_buff * skb,struct ip_tunnel_encap * e,u8 * protocol,struct flowi4 * fl4)804 int gue_build_header(struct sk_buff *skb, struct ip_tunnel_encap *e,
805 u8 *protocol, struct flowi4 *fl4)
806 {
807 bool csum = !!(e->flags & TUNNEL_ENCAP_FLAG_CSUM);
808 int type = csum ? SKB_GSO_UDP_TUNNEL_CSUM : SKB_GSO_UDP_TUNNEL;
809 struct guehdr *guehdr;
810 size_t hdrlen, optlen = 0;
811 __be16 sport;
812 void *data;
813 bool need_priv = false;
814
815 if ((e->flags & TUNNEL_ENCAP_FLAG_REMCSUM) &&
816 skb->ip_summed == CHECKSUM_PARTIAL) {
817 csum = false;
818 optlen += GUE_PLEN_REMCSUM;
819 type |= SKB_GSO_TUNNEL_REMCSUM;
820 need_priv = true;
821 }
822
823 optlen += need_priv ? GUE_LEN_PRIV : 0;
824
825 skb = iptunnel_handle_offloads(skb, csum, type);
826
827 if (IS_ERR(skb))
828 return PTR_ERR(skb);
829
830 /* Get source port (based on flow hash) before skb_push */
831 sport = e->sport ? : udp_flow_src_port(dev_net(skb->dev),
832 skb, 0, 0, false);
833
834 hdrlen = sizeof(struct guehdr) + optlen;
835
836 skb_push(skb, hdrlen);
837
838 guehdr = (struct guehdr *)skb->data;
839
840 guehdr->control = 0;
841 guehdr->version = 0;
842 guehdr->hlen = optlen >> 2;
843 guehdr->flags = 0;
844 guehdr->proto_ctype = *protocol;
845
846 data = &guehdr[1];
847
848 if (need_priv) {
849 __be32 *flags = data;
850
851 guehdr->flags |= GUE_FLAG_PRIV;
852 *flags = 0;
853 data += GUE_LEN_PRIV;
854
855 if (type & SKB_GSO_TUNNEL_REMCSUM) {
856 u16 csum_start = skb_checksum_start_offset(skb);
857 __be16 *pd = data;
858
859 if (csum_start < hdrlen)
860 return -EINVAL;
861
862 csum_start -= hdrlen;
863 pd[0] = htons(csum_start);
864 pd[1] = htons(csum_start + skb->csum_offset);
865
866 if (!skb_is_gso(skb)) {
867 skb->ip_summed = CHECKSUM_NONE;
868 skb->encapsulation = 0;
869 }
870
871 *flags |= GUE_PFLAG_REMCSUM;
872 data += GUE_PLEN_REMCSUM;
873 }
874
875 }
876
877 fou_build_udp(skb, e, fl4, protocol, sport);
878
879 return 0;
880 }
881 EXPORT_SYMBOL(gue_build_header);
882
883 #ifdef CONFIG_NET_FOU_IP_TUNNELS
884
885 static const struct ip_tunnel_encap_ops fou_iptun_ops = {
886 .encap_hlen = fou_encap_hlen,
887 .build_header = fou_build_header,
888 };
889
890 static const struct ip_tunnel_encap_ops gue_iptun_ops = {
891 .encap_hlen = gue_encap_hlen,
892 .build_header = gue_build_header,
893 };
894
ip_tunnel_encap_add_fou_ops(void)895 static int ip_tunnel_encap_add_fou_ops(void)
896 {
897 int ret;
898
899 ret = ip_tunnel_encap_add_ops(&fou_iptun_ops, TUNNEL_ENCAP_FOU);
900 if (ret < 0) {
901 pr_err("can't add fou ops\n");
902 return ret;
903 }
904
905 ret = ip_tunnel_encap_add_ops(&gue_iptun_ops, TUNNEL_ENCAP_GUE);
906 if (ret < 0) {
907 pr_err("can't add gue ops\n");
908 ip_tunnel_encap_del_ops(&fou_iptun_ops, TUNNEL_ENCAP_FOU);
909 return ret;
910 }
911
912 return 0;
913 }
914
ip_tunnel_encap_del_fou_ops(void)915 static void ip_tunnel_encap_del_fou_ops(void)
916 {
917 ip_tunnel_encap_del_ops(&fou_iptun_ops, TUNNEL_ENCAP_FOU);
918 ip_tunnel_encap_del_ops(&gue_iptun_ops, TUNNEL_ENCAP_GUE);
919 }
920
921 #else
922
ip_tunnel_encap_add_fou_ops(void)923 static int ip_tunnel_encap_add_fou_ops(void)
924 {
925 return 0;
926 }
927
ip_tunnel_encap_del_fou_ops(void)928 static void ip_tunnel_encap_del_fou_ops(void)
929 {
930 }
931
932 #endif
933
fou_init_net(struct net * net)934 static __net_init int fou_init_net(struct net *net)
935 {
936 struct fou_net *fn = net_generic(net, fou_net_id);
937
938 INIT_LIST_HEAD(&fn->fou_list);
939 mutex_init(&fn->fou_lock);
940 return 0;
941 }
942
fou_exit_net(struct net * net)943 static __net_exit void fou_exit_net(struct net *net)
944 {
945 struct fou_net *fn = net_generic(net, fou_net_id);
946 struct fou *fou, *next;
947
948 /* Close all the FOU sockets */
949 mutex_lock(&fn->fou_lock);
950 list_for_each_entry_safe(fou, next, &fn->fou_list, list)
951 fou_release(fou);
952 mutex_unlock(&fn->fou_lock);
953 }
954
955 static struct pernet_operations fou_net_ops = {
956 .init = fou_init_net,
957 .exit = fou_exit_net,
958 .id = &fou_net_id,
959 .size = sizeof(struct fou_net),
960 };
961
fou_init(void)962 static int __init fou_init(void)
963 {
964 int ret;
965
966 ret = register_pernet_device(&fou_net_ops);
967 if (ret)
968 goto exit;
969
970 ret = genl_register_family_with_ops(&fou_nl_family,
971 fou_nl_ops);
972 if (ret < 0)
973 goto unregister;
974
975 ret = ip_tunnel_encap_add_fou_ops();
976 if (ret == 0)
977 return 0;
978
979 genl_unregister_family(&fou_nl_family);
980 unregister:
981 unregister_pernet_device(&fou_net_ops);
982 exit:
983 return ret;
984 }
985
fou_fini(void)986 static void __exit fou_fini(void)
987 {
988 ip_tunnel_encap_del_fou_ops();
989 genl_unregister_family(&fou_nl_family);
990 unregister_pernet_device(&fou_net_ops);
991 }
992
993 module_init(fou_init);
994 module_exit(fou_fini);
995 MODULE_AUTHOR("Tom Herbert <therbert@google.com>");
996 MODULE_LICENSE("GPL");
997