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