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