net/ipv4/fou.c

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