netfilter/nft_payload.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
   4  * Copyright (c) 2016 Pablo Neira Ayuso <pablo@netfilter.org>
   5  *
   6  * Development of this code funded by Astaro AG (http://www.astaro.com/)
   7  */
   8
   9 #include <linux/kernel.h>
  10 #include <linux/if_vlan.h>
  11 #include <linux/init.h>
  12 #include <linux/module.h>
  13 #include <linux/netlink.h>
  14 #include <linux/netfilter.h>
  15 #include <linux/netfilter/nf_tables.h>
  16 #include <net/netfilter/nf_tables_core.h>
  17 #include <net/netfilter/nf_tables.h>
  18 #include <net/netfilter/nf_tables_offload.h>
  19 /* For layer 4 checksum field offset. */
  20 #include <linux/tcp.h>
  21 #include <linux/udp.h>
  22 #include <net/gre.h>
  23 #include <linux/icmpv6.h>
  24 #include <linux/ip.h>
  25 #include <linux/ipv6.h>
  26 #include <net/sctp/checksum.h>
  27
  28 static bool nft_payload_rebuild_vlan_hdr(const struct sk_buff *skb, int mac_off,
  29                                          struct vlan_ethhdr *veth)
  30 {
  31         if (skb_copy_bits(skb, mac_off, veth, ETH_HLEN))
  32                 return false;
  33
  34         veth->h_vlan_proto = skb->vlan_proto;
  35         veth->h_vlan_TCI = htons(skb_vlan_tag_get(skb));
  36         veth->h_vlan_encapsulated_proto = skb->protocol;
  37
  38         return true;
  39 }
  40
  41 /* add vlan header into the user buffer for if tag was removed by offloads */
  42 static bool
  43 nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u8 offset, u8 len)
  44 {
  45         int mac_off = skb_mac_header(skb) - skb->data;
  46         u8 *vlanh, *dst_u8 = (u8 *) d;
  47         struct vlan_ethhdr veth;
  48         u8 vlan_hlen = 0;
  49
  50         if ((skb->protocol == htons(ETH_P_8021AD) ||
  51              skb->protocol == htons(ETH_P_8021Q)) &&
  52             offset >= VLAN_ETH_HLEN && offset < VLAN_ETH_HLEN + VLAN_HLEN)
  53                 vlan_hlen += VLAN_HLEN;
  54
  55         vlanh = (u8 *) &veth;
  56         if (offset < VLAN_ETH_HLEN + vlan_hlen) {
  57                 u8 ethlen = len;
  58
  59                 if (vlan_hlen &&
  60                     skb_copy_bits(skb, mac_off, &veth, VLAN_ETH_HLEN) < 0)
  61                         return false;
  62                 else if (!nft_payload_rebuild_vlan_hdr(skb, mac_off, &veth))
  63                         return false;
  64
  65                 if (offset + len > VLAN_ETH_HLEN + vlan_hlen)
  66                         ethlen -= offset + len - VLAN_ETH_HLEN - vlan_hlen;
  67
  68                 memcpy(dst_u8, vlanh + offset - vlan_hlen, ethlen);
  69
  70                 len -= ethlen;
  71                 if (len == 0)
  72                         return true;
  73
  74                 dst_u8 += ethlen;
  75                 offset = ETH_HLEN + vlan_hlen;
  76         } else {
  77                 offset -= VLAN_HLEN + vlan_hlen;
  78         }
  79
  80         return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0;
  81 }
  82
  83 static int __nft_payload_inner_offset(struct nft_pktinfo *pkt)
  84 {
  85         unsigned int thoff = nft_thoff(pkt);
  86
  87         if (!(pkt->flags & NFT_PKTINFO_L4PROTO) || pkt->fragoff)
  88                 return -1;
  89
  90         switch (pkt->tprot) {
  91         case IPPROTO_UDP:
  92                 pkt->inneroff = thoff + sizeof(struct udphdr);
  93                 break;
  94         case IPPROTO_TCP: {
  95                 struct tcphdr *th, _tcph;
  96
  97                 th = skb_header_pointer(pkt->skb, thoff, sizeof(_tcph), &_tcph);
  98                 if (!th)
  99                         return -1;
 100
 101                 pkt->inneroff = thoff + __tcp_hdrlen(th);
 102                 }
 103                 break;
 104         case IPPROTO_GRE: {
 105                 u32 offset = sizeof(struct gre_base_hdr);
 106                 struct gre_base_hdr *gre, _gre;
 107                 __be16 version;
 108
 109                 gre = skb_header_pointer(pkt->skb, thoff, sizeof(_gre), &_gre);
 110                 if (!gre)
 111                         return -1;
 112
 113                 version = gre->flags & GRE_VERSION;
 114                 switch (version) {
 115                 case GRE_VERSION_0:
 116                         if (gre->flags & GRE_ROUTING)
 117                                 return -1;
 118
 119                         if (gre->flags & GRE_CSUM) {
 120                                 offset += sizeof_field(struct gre_full_hdr, csum) +
 121                                           sizeof_field(struct gre_full_hdr, reserved1);
 122                         }
 123                         if (gre->flags & GRE_KEY)
 124                                 offset += sizeof_field(struct gre_full_hdr, key);
 125
 126                         if (gre->flags & GRE_SEQ)
 127                                 offset += sizeof_field(struct gre_full_hdr, seq);
 128                         break;
 129                 default:
 130                         return -1;
 131                 }
 132
 133                 pkt->inneroff = thoff + offset;
 134                 }
 135                 break;
 136         case IPPROTO_IPIP:
 137                 pkt->inneroff = thoff;
 138                 break;
 139         default:
 140                 return -1;
 141         }
 142
 143         pkt->flags |= NFT_PKTINFO_INNER;
 144
 145         return 0;
 146 }
 147
 148 int nft_payload_inner_offset(const struct nft_pktinfo *pkt)
 149 {
 150         if (!(pkt->flags & NFT_PKTINFO_INNER) &&
 151             __nft_payload_inner_offset((struct nft_pktinfo *)pkt) < 0)
 152                 return -1;
 153
 154         return pkt->inneroff;
 155 }
 156
 157 static bool nft_payload_need_vlan_copy(const struct nft_payload *priv)
 158 {
 159         unsigned int len = priv->offset + priv->len;
 160
 161         /* data past ether src/dst requested, copy needed */
 162         if (len > offsetof(struct ethhdr, h_proto))
 163                 return true;
 164
 165         return false;
 166 }
 167
 168 void nft_payload_eval(const struct nft_expr *expr,
 169                       struct nft_regs *regs,
 170                       const struct nft_pktinfo *pkt)
 171 {
 172         const struct nft_payload *priv = nft_expr_priv(expr);
 173         const struct sk_buff *skb = pkt->skb;
 174         u32 *dest = &regs->data[priv->dreg];
 175         int offset;
 176
 177         if (priv->len % NFT_REG32_SIZE)
 178                 dest[priv->len / NFT_REG32_SIZE] = 0;
 179
 180         switch (priv->base) {
 181         case NFT_PAYLOAD_LL_HEADER:
 182                 if (!skb_mac_header_was_set(skb) || skb_mac_header_len(skb) == 0)
 183                         goto err;
 184
 185                 if (skb_vlan_tag_present(skb) &&
 186                     nft_payload_need_vlan_copy(priv)) {
 187                         if (!nft_payload_copy_vlan(dest, skb,
 188                                                    priv->offset, priv->len))
 189                                 goto err;
 190                         return;
 191                 }
 192                 offset = skb_mac_header(skb) - skb->data;
 193                 break;
 194         case NFT_PAYLOAD_NETWORK_HEADER:
 195                 offset = skb_network_offset(skb);
 196                 break;
 197         case NFT_PAYLOAD_TRANSPORT_HEADER:
 198                 if (!(pkt->flags & NFT_PKTINFO_L4PROTO) || pkt->fragoff)
 199                         goto err;
 200                 offset = nft_thoff(pkt);
 201                 break;
 202         case NFT_PAYLOAD_INNER_HEADER:
 203                 offset = nft_payload_inner_offset(pkt);
 204                 if (offset < 0)
 205                         goto err;
 206                 break;
 207         default:
 208                 WARN_ON_ONCE(1);
 209                 goto err;
 210         }
 211         offset += priv->offset;
 212
 213         if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
 214                 goto err;
 215         return;
 216 err:
 217         regs->verdict.code = NFT_BREAK;
 218 }
 219
 220 static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
 221         [NFTA_PAYLOAD_SREG]             = { .type = NLA_U32 },
 222         [NFTA_PAYLOAD_DREG]             = { .type = NLA_U32 },
 223         [NFTA_PAYLOAD_BASE]             = { .type = NLA_U32 },
 224         [NFTA_PAYLOAD_OFFSET]           = NLA_POLICY_MAX(NLA_BE32, 255),
 225         [NFTA_PAYLOAD_LEN]              = NLA_POLICY_MAX(NLA_BE32, 255),
 226         [NFTA_PAYLOAD_CSUM_TYPE]        = { .type = NLA_U32 },
 227         [NFTA_PAYLOAD_CSUM_OFFSET]      = NLA_POLICY_MAX(NLA_BE32, 255),
 228         [NFTA_PAYLOAD_CSUM_FLAGS]       = { .type = NLA_U32 },
 229 };
 230
 231 static int nft_payload_init(const struct nft_ctx *ctx,
 232                             const struct nft_expr *expr,
 233                             const struct nlattr * const tb[])
 234 {
 235         struct nft_payload *priv = nft_expr_priv(expr);
 236
 237         priv->base   = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
 238         priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
 239         priv->len    = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
 240
 241         return nft_parse_register_store(ctx, tb[NFTA_PAYLOAD_DREG],
 242                                         &priv->dreg, NULL, NFT_DATA_VALUE,
 243                                         priv->len);
 244 }
 245
 246 static int nft_payload_dump(struct sk_buff *skb,
 247                             const struct nft_expr *expr, bool reset)
 248 {
 249         const struct nft_payload *priv = nft_expr_priv(expr);
 250
 251         if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) ||
 252             nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
 253             nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
 254             nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
 255                 goto nla_put_failure;
 256         return 0;
 257
 258 nla_put_failure:
 259         return -1;
 260 }
 261
 262 static bool nft_payload_reduce(struct nft_regs_track *track,
 263                                const struct nft_expr *expr)
 264 {
 265         const struct nft_payload *priv = nft_expr_priv(expr);
 266         const struct nft_payload *payload;
 267
 268         if (!nft_reg_track_cmp(track, expr, priv->dreg)) {
 269                 nft_reg_track_update(track, expr, priv->dreg, priv->len);
 270                 return false;
 271         }
 272
 273         payload = nft_expr_priv(track->regs[priv->dreg].selector);
 274         if (priv->base != payload->base ||
 275             priv->offset != payload->offset ||
 276             priv->len != payload->len) {
 277                 nft_reg_track_update(track, expr, priv->dreg, priv->len);
 278                 return false;
 279         }
 280
 281         if (!track->regs[priv->dreg].bitwise)
 282                 return true;
 283
 284         return nft_expr_reduce_bitwise(track, expr);
 285 }
 286
 287 static bool nft_payload_offload_mask(struct nft_offload_reg *reg,
 288                                      u32 priv_len, u32 field_len)
 289 {
 290         unsigned int remainder, delta, k;
 291         struct nft_data mask = {};
 292         __be32 remainder_mask;
 293
 294         if (priv_len == field_len) {
 295                 memset(&reg->mask, 0xff, priv_len);
 296                 return true;
 297         } else if (priv_len > field_len) {
 298                 return false;
 299         }
 300
 301         memset(&mask, 0xff, field_len);
 302         remainder = priv_len % sizeof(u32);
 303         if (remainder) {
 304                 k = priv_len / sizeof(u32);
 305                 delta = field_len - priv_len;
 306                 remainder_mask = htonl(~((1 << (delta * BITS_PER_BYTE)) - 1));
 307                 mask.data[k] = (__force u32)remainder_mask;
 308         }
 309
 310         memcpy(&reg->mask, &mask, field_len);
 311
 312         return true;
 313 }
 314
 315 static int nft_payload_offload_ll(struct nft_offload_ctx *ctx,
 316                                   struct nft_flow_rule *flow,
 317                                   const struct nft_payload *priv)
 318 {
 319         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 320
 321         switch (priv->offset) {
 322         case offsetof(struct ethhdr, h_source):
 323                 if (!nft_payload_offload_mask(reg, priv->len, ETH_ALEN))
 324                         return -EOPNOTSUPP;
 325
 326                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_ETH_ADDRS, eth_addrs,
 327                                   src, ETH_ALEN, reg);
 328                 break;
 329         case offsetof(struct ethhdr, h_dest):
 330                 if (!nft_payload_offload_mask(reg, priv->len, ETH_ALEN))
 331                         return -EOPNOTSUPP;
 332
 333                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_ETH_ADDRS, eth_addrs,
 334                                   dst, ETH_ALEN, reg);
 335                 break;
 336         case offsetof(struct ethhdr, h_proto):
 337                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 338                         return -EOPNOTSUPP;
 339
 340                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_BASIC, basic,
 341                                   n_proto, sizeof(__be16), reg);
 342                 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_NETWORK);
 343                 break;
 344         case offsetof(struct vlan_ethhdr, h_vlan_TCI):
 345                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 346                         return -EOPNOTSUPP;
 347
 348                 NFT_OFFLOAD_MATCH_FLAGS(FLOW_DISSECTOR_KEY_VLAN, vlan,
 349                                         vlan_tci, sizeof(__be16), reg,
 350                                         NFT_OFFLOAD_F_NETWORK2HOST);
 351                 break;
 352         case offsetof(struct vlan_ethhdr, h_vlan_encapsulated_proto):
 353                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 354                         return -EOPNOTSUPP;
 355
 356                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_VLAN, vlan,
 357                                   vlan_tpid, sizeof(__be16), reg);
 358                 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_NETWORK);
 359                 break;
 360         case offsetof(struct vlan_ethhdr, h_vlan_TCI) + sizeof(struct vlan_hdr):
 361                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 362                         return -EOPNOTSUPP;
 363
 364                 NFT_OFFLOAD_MATCH_FLAGS(FLOW_DISSECTOR_KEY_CVLAN, cvlan,
 365                                         vlan_tci, sizeof(__be16), reg,
 366                                         NFT_OFFLOAD_F_NETWORK2HOST);
 367                 break;
 368         case offsetof(struct vlan_ethhdr, h_vlan_encapsulated_proto) +
 369                                                         sizeof(struct vlan_hdr):
 370                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 371                         return -EOPNOTSUPP;
 372
 373                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_CVLAN, cvlan,
 374                                   vlan_tpid, sizeof(__be16), reg);
 375                 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_NETWORK);
 376                 break;
 377         default:
 378                 return -EOPNOTSUPP;
 379         }
 380
 381         return 0;
 382 }
 383
 384 static int nft_payload_offload_ip(struct nft_offload_ctx *ctx,
 385                                   struct nft_flow_rule *flow,
 386                                   const struct nft_payload *priv)
 387 {
 388         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 389
 390         switch (priv->offset) {
 391         case offsetof(struct iphdr, saddr):
 392                 if (!nft_payload_offload_mask(reg, priv->len,
 393                                               sizeof(struct in_addr)))
 394                         return -EOPNOTSUPP;
 395
 396                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV4_ADDRS, ipv4, src,
 397                                   sizeof(struct in_addr), reg);
 398                 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV4_ADDRS);
 399                 break;
 400         case offsetof(struct iphdr, daddr):
 401                 if (!nft_payload_offload_mask(reg, priv->len,
 402                                               sizeof(struct in_addr)))
 403                         return -EOPNOTSUPP;
 404
 405                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV4_ADDRS, ipv4, dst,
 406                                   sizeof(struct in_addr), reg);
 407                 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV4_ADDRS);
 408                 break;
 409         case offsetof(struct iphdr, protocol):
 410                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__u8)))
 411                         return -EOPNOTSUPP;
 412
 413                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_BASIC, basic, ip_proto,
 414                                   sizeof(__u8), reg);
 415                 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_TRANSPORT);
 416                 break;
 417         default:
 418                 return -EOPNOTSUPP;
 419         }
 420
 421         return 0;
 422 }
 423
 424 static int nft_payload_offload_ip6(struct nft_offload_ctx *ctx,
 425                                   struct nft_flow_rule *flow,
 426                                   const struct nft_payload *priv)
 427 {
 428         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 429
 430         switch (priv->offset) {
 431         case offsetof(struct ipv6hdr, saddr):
 432                 if (!nft_payload_offload_mask(reg, priv->len,
 433                                               sizeof(struct in6_addr)))
 434                         return -EOPNOTSUPP;
 435
 436                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6, src,
 437                                   sizeof(struct in6_addr), reg);
 438                 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV6_ADDRS);
 439                 break;
 440         case offsetof(struct ipv6hdr, daddr):
 441                 if (!nft_payload_offload_mask(reg, priv->len,
 442                                               sizeof(struct in6_addr)))
 443                         return -EOPNOTSUPP;
 444
 445                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6, dst,
 446                                   sizeof(struct in6_addr), reg);
 447                 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV6_ADDRS);
 448                 break;
 449         case offsetof(struct ipv6hdr, nexthdr):
 450                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__u8)))
 451                         return -EOPNOTSUPP;
 452
 453                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_BASIC, basic, ip_proto,
 454                                   sizeof(__u8), reg);
 455                 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_TRANSPORT);
 456                 break;
 457         default:
 458                 return -EOPNOTSUPP;
 459         }
 460
 461         return 0;
 462 }
 463
 464 static int nft_payload_offload_nh(struct nft_offload_ctx *ctx,
 465                                   struct nft_flow_rule *flow,
 466                                   const struct nft_payload *priv)
 467 {
 468         int err;
 469
 470         switch (ctx->dep.l3num) {
 471         case htons(ETH_P_IP):
 472                 err = nft_payload_offload_ip(ctx, flow, priv);
 473                 break;
 474         case htons(ETH_P_IPV6):
 475                 err = nft_payload_offload_ip6(ctx, flow, priv);
 476                 break;
 477         default:
 478                 return -EOPNOTSUPP;
 479         }
 480
 481         return err;
 482 }
 483
 484 static int nft_payload_offload_tcp(struct nft_offload_ctx *ctx,
 485                                    struct nft_flow_rule *flow,
 486                                    const struct nft_payload *priv)
 487 {
 488         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 489
 490         switch (priv->offset) {
 491         case offsetof(struct tcphdr, source):
 492                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 493                         return -EOPNOTSUPP;
 494
 495                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, src,
 496                                   sizeof(__be16), reg);
 497                 break;
 498         case offsetof(struct tcphdr, dest):
 499                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 500                         return -EOPNOTSUPP;
 501
 502                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, dst,
 503                                   sizeof(__be16), reg);
 504                 break;
 505         default:
 506                 return -EOPNOTSUPP;
 507         }
 508
 509         return 0;
 510 }
 511
 512 static int nft_payload_offload_udp(struct nft_offload_ctx *ctx,
 513                                    struct nft_flow_rule *flow,
 514                                    const struct nft_payload *priv)
 515 {
 516         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 517
 518         switch (priv->offset) {
 519         case offsetof(struct udphdr, source):
 520                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 521                         return -EOPNOTSUPP;
 522
 523                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, src,
 524                                   sizeof(__be16), reg);
 525                 break;
 526         case offsetof(struct udphdr, dest):
 527                 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
 528                         return -EOPNOTSUPP;
 529
 530                 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, dst,
 531                                   sizeof(__be16), reg);
 532                 break;
 533         default:
 534                 return -EOPNOTSUPP;
 535         }
 536
 537         return 0;
 538 }
 539
 540 static int nft_payload_offload_th(struct nft_offload_ctx *ctx,
 541                                   struct nft_flow_rule *flow,
 542                                   const struct nft_payload *priv)
 543 {
 544         int err;
 545
 546         switch (ctx->dep.protonum) {
 547         case IPPROTO_TCP:
 548                 err = nft_payload_offload_tcp(ctx, flow, priv);
 549                 break;
 550         case IPPROTO_UDP:
 551                 err = nft_payload_offload_udp(ctx, flow, priv);
 552                 break;
 553         default:
 554                 return -EOPNOTSUPP;
 555         }
 556
 557         return err;
 558 }
 559
 560 static int nft_payload_offload(struct nft_offload_ctx *ctx,
 561                                struct nft_flow_rule *flow,
 562                                const struct nft_expr *expr)
 563 {
 564         const struct nft_payload *priv = nft_expr_priv(expr);
 565         int err;
 566
 567         switch (priv->base) {
 568         case NFT_PAYLOAD_LL_HEADER:
 569                 err = nft_payload_offload_ll(ctx, flow, priv);
 570                 break;
 571         case NFT_PAYLOAD_NETWORK_HEADER:
 572                 err = nft_payload_offload_nh(ctx, flow, priv);
 573                 break;
 574         case NFT_PAYLOAD_TRANSPORT_HEADER:
 575                 err = nft_payload_offload_th(ctx, flow, priv);
 576                 break;
 577         default:
 578                 err = -EOPNOTSUPP;
 579                 break;
 580         }
 581         return err;
 582 }
 583
 584 static const struct nft_expr_ops nft_payload_ops = {
 585         .type           = &nft_payload_type,
 586         .size           = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
 587         .eval           = nft_payload_eval,
 588         .init           = nft_payload_init,
 589         .dump           = nft_payload_dump,
 590         .reduce         = nft_payload_reduce,
 591         .offload        = nft_payload_offload,
 592 };
 593
 594 const struct nft_expr_ops nft_payload_fast_ops = {
 595         .type           = &nft_payload_type,
 596         .size           = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
 597         .eval           = nft_payload_eval,
 598         .init           = nft_payload_init,
 599         .dump           = nft_payload_dump,
 600         .reduce         = nft_payload_reduce,
 601         .offload        = nft_payload_offload,
 602 };
 603
 604 void nft_payload_inner_eval(const struct nft_expr *expr, struct nft_regs *regs,
 605                             const struct nft_pktinfo *pkt,
 606                             struct nft_inner_tun_ctx *tun_ctx)
 607 {
 608         const struct nft_payload *priv = nft_expr_priv(expr);
 609         const struct sk_buff *skb = pkt->skb;
 610         u32 *dest = &regs->data[priv->dreg];
 611         int offset;
 612
 613         if (priv->len % NFT_REG32_SIZE)
 614                 dest[priv->len / NFT_REG32_SIZE] = 0;
 615
 616         switch (priv->base) {
 617         case NFT_PAYLOAD_TUN_HEADER:
 618                 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_TUN))
 619                         goto err;
 620
 621                 offset = tun_ctx->inner_tunoff;
 622                 break;
 623         case NFT_PAYLOAD_LL_HEADER:
 624                 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_LL))
 625                         goto err;
 626
 627                 offset = tun_ctx->inner_lloff;
 628                 break;
 629         case NFT_PAYLOAD_NETWORK_HEADER:
 630                 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_NH))
 631                         goto err;
 632
 633                 offset = tun_ctx->inner_nhoff;
 634                 break;
 635         case NFT_PAYLOAD_TRANSPORT_HEADER:
 636                 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_TH))
 637                         goto err;
 638
 639                 offset = tun_ctx->inner_thoff;
 640                 break;
 641         default:
 642                 WARN_ON_ONCE(1);
 643                 goto err;
 644         }
 645         offset += priv->offset;
 646
 647         if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
 648                 goto err;
 649
 650         return;
 651 err:
 652         regs->verdict.code = NFT_BREAK;
 653 }
 654
 655 static int nft_payload_inner_init(const struct nft_ctx *ctx,
 656                                   const struct nft_expr *expr,
 657                                   const struct nlattr * const tb[])
 658 {
 659         struct nft_payload *priv = nft_expr_priv(expr);
 660         u32 base;
 661
 662         base   = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
 663         switch (base) {
 664         case NFT_PAYLOAD_TUN_HEADER:
 665         case NFT_PAYLOAD_LL_HEADER:
 666         case NFT_PAYLOAD_NETWORK_HEADER:
 667         case NFT_PAYLOAD_TRANSPORT_HEADER:
 668                 break;
 669         default:
 670                 return -EOPNOTSUPP;
 671         }
 672
 673         priv->base   = base;
 674         priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
 675         priv->len    = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
 676
 677         return nft_parse_register_store(ctx, tb[NFTA_PAYLOAD_DREG],
 678                                         &priv->dreg, NULL, NFT_DATA_VALUE,
 679                                         priv->len);
 680 }
 681
 682 static const struct nft_expr_ops nft_payload_inner_ops = {
 683         .type           = &nft_payload_type,
 684         .size           = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
 685         .init           = nft_payload_inner_init,
 686         .dump           = nft_payload_dump,
 687         /* direct call to nft_payload_inner_eval(). */
 688 };
 689
 690 static inline void nft_csum_replace(__sum16 *sum, __wsum fsum, __wsum tsum)
 691 {
 692         *sum = csum_fold(csum_add(csum_sub(~csum_unfold(*sum), fsum), tsum));
 693         if (*sum == 0)
 694                 *sum = CSUM_MANGLED_0;
 695 }
 696
 697 static bool nft_payload_udp_checksum(struct sk_buff *skb, unsigned int thoff)
 698 {
 699         struct udphdr *uh, _uh;
 700
 701         uh = skb_header_pointer(skb, thoff, sizeof(_uh), &_uh);
 702         if (!uh)
 703                 return false;
 704
 705         return (__force bool)uh->check;
 706 }
 707
 708 static int nft_payload_l4csum_offset(const struct nft_pktinfo *pkt,
 709                                      struct sk_buff *skb,
 710                                      unsigned int *l4csum_offset)
 711 {
 712         if (pkt->fragoff)
 713                 return -1;
 714
 715         switch (pkt->tprot) {
 716         case IPPROTO_TCP:
 717                 *l4csum_offset = offsetof(struct tcphdr, check);
 718                 break;
 719         case IPPROTO_UDP:
 720                 if (!nft_payload_udp_checksum(skb, nft_thoff(pkt)))
 721                         return -1;
 722                 fallthrough;
 723         case IPPROTO_UDPLITE:
 724                 *l4csum_offset = offsetof(struct udphdr, check);
 725                 break;
 726         case IPPROTO_ICMPV6:
 727                 *l4csum_offset = offsetof(struct icmp6hdr, icmp6_cksum);
 728                 break;
 729         default:
 730                 return -1;
 731         }
 732
 733         *l4csum_offset += nft_thoff(pkt);
 734         return 0;
 735 }
 736
 737 static int nft_payload_csum_sctp(struct sk_buff *skb, int offset)
 738 {
 739         struct sctphdr *sh;
 740
 741         if (skb_ensure_writable(skb, offset + sizeof(*sh)))
 742                 return -1;
 743
 744         sh = (struct sctphdr *)(skb->data + offset);
 745         sh->checksum = sctp_compute_cksum(skb, offset);
 746         skb->ip_summed = CHECKSUM_UNNECESSARY;
 747         return 0;
 748 }
 749
 750 static int nft_payload_l4csum_update(const struct nft_pktinfo *pkt,
 751                                      struct sk_buff *skb,
 752                                      __wsum fsum, __wsum tsum)
 753 {
 754         int l4csum_offset;
 755         __sum16 sum;
 756
 757         /* If we cannot determine layer 4 checksum offset or this packet doesn't
 758          * require layer 4 checksum recalculation, skip this packet.
 759          */
 760         if (nft_payload_l4csum_offset(pkt, skb, &l4csum_offset) < 0)
 761                 return 0;
 762
 763         if (skb_copy_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
 764                 return -1;
 765
 766         /* Checksum mangling for an arbitrary amount of bytes, based on
 767          * inet_proto_csum_replace*() functions.
 768          */
 769         if (skb->ip_summed != CHECKSUM_PARTIAL) {
 770                 nft_csum_replace(&sum, fsum, tsum);
 771                 if (skb->ip_summed == CHECKSUM_COMPLETE) {
 772                         skb->csum = ~csum_add(csum_sub(~(skb->csum), fsum),
 773                                               tsum);
 774                 }
 775         } else {
 776                 sum = ~csum_fold(csum_add(csum_sub(csum_unfold(sum), fsum),
 777                                           tsum));
 778         }
 779
 780         if (skb_ensure_writable(skb, l4csum_offset + sizeof(sum)) ||
 781             skb_store_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
 782                 return -1;
 783
 784         return 0;
 785 }
 786
 787 static int nft_payload_csum_inet(struct sk_buff *skb, const u32 *src,
 788                                  __wsum fsum, __wsum tsum, int csum_offset)
 789 {
 790         __sum16 sum;
 791
 792         if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
 793                 return -1;
 794
 795         nft_csum_replace(&sum, fsum, tsum);
 796         if (skb_ensure_writable(skb, csum_offset + sizeof(sum)) ||
 797             skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
 798                 return -1;
 799
 800         return 0;
 801 }
 802
 803 struct nft_payload_set {
 804         enum nft_payload_bases  base:8;
 805         u8                      offset;
 806         u8                      len;
 807         u8                      sreg;
 808         u8                      csum_type;
 809         u8                      csum_offset;
 810         u8                      csum_flags;
 811 };
 812
 813 static void nft_payload_set_eval(const struct nft_expr *expr,
 814                                  struct nft_regs *regs,
 815                                  const struct nft_pktinfo *pkt)
 816 {
 817         const struct nft_payload_set *priv = nft_expr_priv(expr);
 818         struct sk_buff *skb = pkt->skb;
 819         const u32 *src = &regs->data[priv->sreg];
 820         int offset, csum_offset;
 821         __wsum fsum, tsum;
 822
 823         switch (priv->base) {
 824         case NFT_PAYLOAD_LL_HEADER:
 825                 if (!skb_mac_header_was_set(skb))
 826                         goto err;
 827                 offset = skb_mac_header(skb) - skb->data;
 828                 break;
 829         case NFT_PAYLOAD_NETWORK_HEADER:
 830                 offset = skb_network_offset(skb);
 831                 break;
 832         case NFT_PAYLOAD_TRANSPORT_HEADER:
 833                 if (!(pkt->flags & NFT_PKTINFO_L4PROTO) || pkt->fragoff)
 834                         goto err;
 835                 offset = nft_thoff(pkt);
 836                 break;
 837         case NFT_PAYLOAD_INNER_HEADER:
 838                 offset = nft_payload_inner_offset(pkt);
 839                 if (offset < 0)
 840                         goto err;
 841                 break;
 842         default:
 843                 WARN_ON_ONCE(1);
 844                 goto err;
 845         }
 846
 847         csum_offset = offset + priv->csum_offset;
 848         offset += priv->offset;
 849
 850         if ((priv->csum_type == NFT_PAYLOAD_CSUM_INET || priv->csum_flags) &&
 851             ((priv->base != NFT_PAYLOAD_TRANSPORT_HEADER &&
 852               priv->base != NFT_PAYLOAD_INNER_HEADER) ||
 853              skb->ip_summed != CHECKSUM_PARTIAL)) {
 854                 fsum = skb_checksum(skb, offset, priv->len, 0);
 855                 tsum = csum_partial(src, priv->len, 0);
 856
 857                 if (priv->csum_type == NFT_PAYLOAD_CSUM_INET &&
 858                     nft_payload_csum_inet(skb, src, fsum, tsum, csum_offset))
 859                         goto err;
 860
 861                 if (priv->csum_flags &&
 862                     nft_payload_l4csum_update(pkt, skb, fsum, tsum) < 0)
 863                         goto err;
 864         }
 865
 866         if (skb_ensure_writable(skb, max(offset + priv->len, 0)) ||
 867             skb_store_bits(skb, offset, src, priv->len) < 0)
 868                 goto err;
 869
 870         if (priv->csum_type == NFT_PAYLOAD_CSUM_SCTP &&
 871             pkt->tprot == IPPROTO_SCTP &&
 872             skb->ip_summed != CHECKSUM_PARTIAL) {
 873                 if (pkt->fragoff == 0 &&
 874                     nft_payload_csum_sctp(skb, nft_thoff(pkt)))
 875                         goto err;
 876         }
 877
 878         return;
 879 err:
 880         regs->verdict.code = NFT_BREAK;
 881 }
 882
 883 static int nft_payload_set_init(const struct nft_ctx *ctx,
 884                                 const struct nft_expr *expr,
 885                                 const struct nlattr * const tb[])
 886 {
 887         struct nft_payload_set *priv = nft_expr_priv(expr);
 888         u32 csum_offset, csum_type = NFT_PAYLOAD_CSUM_NONE;
 889         int err;
 890
 891         priv->base        = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
 892         priv->offset      = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
 893         priv->len         = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
 894
 895         if (tb[NFTA_PAYLOAD_CSUM_TYPE])
 896                 csum_type = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE]));
 897         if (tb[NFTA_PAYLOAD_CSUM_OFFSET]) {
 898                 err = nft_parse_u32_check(tb[NFTA_PAYLOAD_CSUM_OFFSET], U8_MAX,
 899                                           &csum_offset);
 900                 if (err < 0)
 901                         return err;
 902
 903                 priv->csum_offset = csum_offset;
 904         }
 905         if (tb[NFTA_PAYLOAD_CSUM_FLAGS]) {
 906                 u32 flags;
 907
 908                 flags = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_FLAGS]));
 909                 if (flags & ~NFT_PAYLOAD_L4CSUM_PSEUDOHDR)
 910                         return -EINVAL;
 911
 912                 priv->csum_flags = flags;
 913         }
 914
 915         switch (csum_type) {
 916         case NFT_PAYLOAD_CSUM_NONE:
 917         case NFT_PAYLOAD_CSUM_INET:
 918                 break;
 919         case NFT_PAYLOAD_CSUM_SCTP:
 920                 if (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER)
 921                         return -EINVAL;
 922
 923                 if (priv->csum_offset != offsetof(struct sctphdr, checksum))
 924                         return -EINVAL;
 925                 break;
 926         default:
 927                 return -EOPNOTSUPP;
 928         }
 929         priv->csum_type = csum_type;
 930
 931         return nft_parse_register_load(tb[NFTA_PAYLOAD_SREG], &priv->sreg,
 932                                        priv->len);
 933 }
 934
 935 static int nft_payload_set_dump(struct sk_buff *skb,
 936                                 const struct nft_expr *expr, bool reset)
 937 {
 938         const struct nft_payload_set *priv = nft_expr_priv(expr);
 939
 940         if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) ||
 941             nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
 942             nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
 943             nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) ||
 944             nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) ||
 945             nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET,
 946                          htonl(priv->csum_offset)) ||
 947             nla_put_be32(skb, NFTA_PAYLOAD_CSUM_FLAGS, htonl(priv->csum_flags)))
 948                 goto nla_put_failure;
 949         return 0;
 950
 951 nla_put_failure:
 952         return -1;
 953 }
 954
 955 static bool nft_payload_set_reduce(struct nft_regs_track *track,
 956                                    const struct nft_expr *expr)
 957 {
 958         int i;
 959
 960         for (i = 0; i < NFT_REG32_NUM; i++) {
 961                 if (!track->regs[i].selector)
 962                         continue;
 963
 964                 if (track->regs[i].selector->ops != &nft_payload_ops &&
 965                     track->regs[i].selector->ops != &nft_payload_fast_ops)
 966                         continue;
 967
 968                 __nft_reg_track_cancel(track, i);
 969         }
 970
 971         return false;
 972 }
 973
 974 static const struct nft_expr_ops nft_payload_set_ops = {
 975         .type           = &nft_payload_type,
 976         .size           = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)),
 977         .eval           = nft_payload_set_eval,
 978         .init           = nft_payload_set_init,
 979         .dump           = nft_payload_set_dump,
 980         .reduce         = nft_payload_set_reduce,
 981 };
 982
 983 static const struct nft_expr_ops *
 984 nft_payload_select_ops(const struct nft_ctx *ctx,
 985                        const struct nlattr * const tb[])
 986 {
 987         enum nft_payload_bases base;
 988         unsigned int offset, len;
 989         int err;
 990
 991         if (tb[NFTA_PAYLOAD_BASE] == NULL ||
 992             tb[NFTA_PAYLOAD_OFFSET] == NULL ||
 993             tb[NFTA_PAYLOAD_LEN] == NULL)
 994                 return ERR_PTR(-EINVAL);
 995
 996         base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
 997         switch (base) {
 998         case NFT_PAYLOAD_LL_HEADER:
 999         case NFT_PAYLOAD_NETWORK_HEADER:
1000         case NFT_PAYLOAD_TRANSPORT_HEADER:
1001         case NFT_PAYLOAD_INNER_HEADER:
1002                 break;
1003         default:
1004                 return ERR_PTR(-EOPNOTSUPP);
1005         }
1006
1007         if (tb[NFTA_PAYLOAD_SREG] != NULL) {
1008                 if (tb[NFTA_PAYLOAD_DREG] != NULL)
1009                         return ERR_PTR(-EINVAL);
1010                 return &nft_payload_set_ops;
1011         }
1012
1013         if (tb[NFTA_PAYLOAD_DREG] == NULL)
1014                 return ERR_PTR(-EINVAL);
1015
1016         err = nft_parse_u32_check(tb[NFTA_PAYLOAD_OFFSET], U8_MAX, &offset);
1017         if (err < 0)
1018                 return ERR_PTR(err);
1019
1020         err = nft_parse_u32_check(tb[NFTA_PAYLOAD_LEN], U8_MAX, &len);
1021         if (err < 0)
1022                 return ERR_PTR(err);
1023
1024         if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
1025             base != NFT_PAYLOAD_LL_HEADER && base != NFT_PAYLOAD_INNER_HEADER)
1026                 return &nft_payload_fast_ops;
1027         else
1028                 return &nft_payload_ops;
1029 }
1030
1031 struct nft_expr_type nft_payload_type __read_mostly = {
1032         .name           = "payload",
1033         .select_ops     = nft_payload_select_ops,
1034         .inner_ops      = &nft_payload_inner_ops,
1035         .policy         = nft_payload_policy,
1036         .maxattr        = NFTA_PAYLOAD_MAX,
1037         .owner          = THIS_MODULE,
1038 };