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