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