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