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