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