net/netfilter/nft_bitwise.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
   4  *
   5  * Development of this code funded by Astaro AG (http://www.astaro.com/)
   6  */
   7
   8 #include <linux/kernel.h>
   9 #include <linux/init.h>
  10 #include <linux/module.h>
  11 #include <linux/netlink.h>
  12 #include <linux/netfilter.h>
  13 #include <linux/netfilter/nf_tables.h>
  14 #include <net/netfilter/nf_tables_core.h>
  15 #include <net/netfilter/nf_tables.h>
  16 #include <net/netfilter/nf_tables_offload.h>
  17
  18 struct nft_bitwise {
  19         u8                      sreg;
  20         u8                      dreg;
  21         enum nft_bitwise_ops    op:8;
  22         u8                      len;
  23         struct nft_data         mask;
  24         struct nft_data         xor;
  25         struct nft_data         data;
  26 };
  27
  28 static void nft_bitwise_eval_bool(u32 *dst, const u32 *src,
  29                                   const struct nft_bitwise *priv)
  30 {
  31         unsigned int i;
  32
  33         for (i = 0; i < DIV_ROUND_UP(priv->len, 4); i++)
  34                 dst[i] = (src[i] & priv->mask.data[i]) ^ priv->xor.data[i];
  35 }
  36
  37 static void nft_bitwise_eval_lshift(u32 *dst, const u32 *src,
  38                                     const struct nft_bitwise *priv)
  39 {
  40         u32 shift = priv->data.data[0];
  41         unsigned int i;
  42         u32 carry = 0;
  43
  44         for (i = DIV_ROUND_UP(priv->len, sizeof(u32)); i > 0; i--) {
  45                 dst[i - 1] = (src[i - 1] << shift) | carry;
  46                 carry = src[i - 1] >> (BITS_PER_TYPE(u32) - shift);
  47         }
  48 }
  49
  50 static void nft_bitwise_eval_rshift(u32 *dst, const u32 *src,
  51                                     const struct nft_bitwise *priv)
  52 {
  53         u32 shift = priv->data.data[0];
  54         unsigned int i;
  55         u32 carry = 0;
  56
  57         for (i = 0; i < DIV_ROUND_UP(priv->len, sizeof(u32)); i++) {
  58                 dst[i] = carry | (src[i] >> shift);
  59                 carry = src[i] << (BITS_PER_TYPE(u32) - shift);
  60         }
  61 }
  62
  63 void nft_bitwise_eval(const struct nft_expr *expr,
  64                       struct nft_regs *regs, const struct nft_pktinfo *pkt)
  65 {
  66         const struct nft_bitwise *priv = nft_expr_priv(expr);
  67         const u32 *src = &regs->data[priv->sreg];
  68         u32 *dst = &regs->data[priv->dreg];
  69
  70         switch (priv->op) {
  71         case NFT_BITWISE_BOOL:
  72                 nft_bitwise_eval_bool(dst, src, priv);
  73                 break;
  74         case NFT_BITWISE_LSHIFT:
  75                 nft_bitwise_eval_lshift(dst, src, priv);
  76                 break;
  77         case NFT_BITWISE_RSHIFT:
  78                 nft_bitwise_eval_rshift(dst, src, priv);
  79                 break;
  80         }
  81 }
  82
  83 static const struct nla_policy nft_bitwise_policy[NFTA_BITWISE_MAX + 1] = {
  84         [NFTA_BITWISE_SREG]     = { .type = NLA_U32 },
  85         [NFTA_BITWISE_DREG]     = { .type = NLA_U32 },
  86         [NFTA_BITWISE_LEN]      = { .type = NLA_U32 },
  87         [NFTA_BITWISE_MASK]     = { .type = NLA_NESTED },
  88         [NFTA_BITWISE_XOR]      = { .type = NLA_NESTED },
  89         [NFTA_BITWISE_OP]       = { .type = NLA_U32 },
  90         [NFTA_BITWISE_DATA]     = { .type = NLA_NESTED },
  91 };
  92
  93 static int nft_bitwise_init_bool(struct nft_bitwise *priv,
  94                                  const struct nlattr *const tb[])
  95 {
  96         struct nft_data_desc mask, xor;
  97         int err;
  98
  99         if (tb[NFTA_BITWISE_DATA])
 100                 return -EINVAL;
 101
 102         if (!tb[NFTA_BITWISE_MASK] ||
 103             !tb[NFTA_BITWISE_XOR])
 104                 return -EINVAL;
 105
 106         err = nft_data_init(NULL, &priv->mask, sizeof(priv->mask), &mask,
 107                             tb[NFTA_BITWISE_MASK]);
 108         if (err < 0)
 109                 return err;
 110         if (mask.type != NFT_DATA_VALUE || mask.len != priv->len) {
 111                 err = -EINVAL;
 112                 goto err1;
 113         }
 114
 115         err = nft_data_init(NULL, &priv->xor, sizeof(priv->xor), &xor,
 116                             tb[NFTA_BITWISE_XOR]);
 117         if (err < 0)
 118                 goto err1;
 119         if (xor.type != NFT_DATA_VALUE || xor.len != priv->len) {
 120                 err = -EINVAL;
 121                 goto err2;
 122         }
 123
 124         return 0;
 125 err2:
 126         nft_data_release(&priv->xor, xor.type);
 127 err1:
 128         nft_data_release(&priv->mask, mask.type);
 129         return err;
 130 }
 131
 132 static int nft_bitwise_init_shift(struct nft_bitwise *priv,
 133                                   const struct nlattr *const tb[])
 134 {
 135         struct nft_data_desc d;
 136         int err;
 137
 138         if (tb[NFTA_BITWISE_MASK] ||
 139             tb[NFTA_BITWISE_XOR])
 140                 return -EINVAL;
 141
 142         if (!tb[NFTA_BITWISE_DATA])
 143                 return -EINVAL;
 144
 145         err = nft_data_init(NULL, &priv->data, sizeof(priv->data), &d,
 146                             tb[NFTA_BITWISE_DATA]);
 147         if (err < 0)
 148                 return err;
 149         if (d.type != NFT_DATA_VALUE || d.len != sizeof(u32) ||
 150             priv->data.data[0] >= BITS_PER_TYPE(u32)) {
 151                 nft_data_release(&priv->data, d.type);
 152                 return -EINVAL;
 153         }
 154
 155         return 0;
 156 }
 157
 158 static int nft_bitwise_init(const struct nft_ctx *ctx,
 159                             const struct nft_expr *expr,
 160                             const struct nlattr * const tb[])
 161 {
 162         struct nft_bitwise *priv = nft_expr_priv(expr);
 163         u32 len;
 164         int err;
 165
 166         err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len);
 167         if (err < 0)
 168                 return err;
 169
 170         priv->len = len;
 171
 172         err = nft_parse_register_load(tb[NFTA_BITWISE_SREG], &priv->sreg,
 173                                       priv->len);
 174         if (err < 0)
 175                 return err;
 176
 177         err = nft_parse_register_store(ctx, tb[NFTA_BITWISE_DREG],
 178                                        &priv->dreg, NULL, NFT_DATA_VALUE,
 179                                        priv->len);
 180         if (err < 0)
 181                 return err;
 182
 183         if (tb[NFTA_BITWISE_OP]) {
 184                 priv->op = ntohl(nla_get_be32(tb[NFTA_BITWISE_OP]));
 185                 switch (priv->op) {
 186                 case NFT_BITWISE_BOOL:
 187                 case NFT_BITWISE_LSHIFT:
 188                 case NFT_BITWISE_RSHIFT:
 189                         break;
 190                 default:
 191                         return -EOPNOTSUPP;
 192                 }
 193         } else {
 194                 priv->op = NFT_BITWISE_BOOL;
 195         }
 196
 197         switch(priv->op) {
 198         case NFT_BITWISE_BOOL:
 199                 err = nft_bitwise_init_bool(priv, tb);
 200                 break;
 201         case NFT_BITWISE_LSHIFT:
 202         case NFT_BITWISE_RSHIFT:
 203                 err = nft_bitwise_init_shift(priv, tb);
 204                 break;
 205         }
 206
 207         return err;
 208 }
 209
 210 static int nft_bitwise_dump_bool(struct sk_buff *skb,
 211                                  const struct nft_bitwise *priv)
 212 {
 213         if (nft_data_dump(skb, NFTA_BITWISE_MASK, &priv->mask,
 214                           NFT_DATA_VALUE, priv->len) < 0)
 215                 return -1;
 216
 217         if (nft_data_dump(skb, NFTA_BITWISE_XOR, &priv->xor,
 218                           NFT_DATA_VALUE, priv->len) < 0)
 219                 return -1;
 220
 221         return 0;
 222 }
 223
 224 static int nft_bitwise_dump_shift(struct sk_buff *skb,
 225                                   const struct nft_bitwise *priv)
 226 {
 227         if (nft_data_dump(skb, NFTA_BITWISE_DATA, &priv->data,
 228                           NFT_DATA_VALUE, sizeof(u32)) < 0)
 229                 return -1;
 230         return 0;
 231 }
 232
 233 static int nft_bitwise_dump(struct sk_buff *skb, const struct nft_expr *expr)
 234 {
 235         const struct nft_bitwise *priv = nft_expr_priv(expr);
 236         int err = 0;
 237
 238         if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
 239                 return -1;
 240         if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
 241                 return -1;
 242         if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(priv->len)))
 243                 return -1;
 244         if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(priv->op)))
 245                 return -1;
 246
 247         switch (priv->op) {
 248         case NFT_BITWISE_BOOL:
 249                 err = nft_bitwise_dump_bool(skb, priv);
 250                 break;
 251         case NFT_BITWISE_LSHIFT:
 252         case NFT_BITWISE_RSHIFT:
 253                 err = nft_bitwise_dump_shift(skb, priv);
 254                 break;
 255         }
 256
 257         return err;
 258 }
 259
 260 static struct nft_data zero;
 261
 262 static int nft_bitwise_offload(struct nft_offload_ctx *ctx,
 263                                struct nft_flow_rule *flow,
 264                                const struct nft_expr *expr)
 265 {
 266         const struct nft_bitwise *priv = nft_expr_priv(expr);
 267         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 268
 269         if (priv->op != NFT_BITWISE_BOOL)
 270                 return -EOPNOTSUPP;
 271
 272         if (memcmp(&priv->xor, &zero, sizeof(priv->xor)) ||
 273             priv->sreg != priv->dreg || priv->len != reg->len)
 274                 return -EOPNOTSUPP;
 275
 276         memcpy(&reg->mask, &priv->mask, sizeof(priv->mask));
 277
 278         return 0;
 279 }
 280
 281 static const struct nft_expr_ops nft_bitwise_ops = {
 282         .type           = &nft_bitwise_type,
 283         .size           = NFT_EXPR_SIZE(sizeof(struct nft_bitwise)),
 284         .eval           = nft_bitwise_eval,
 285         .init           = nft_bitwise_init,
 286         .dump           = nft_bitwise_dump,
 287         .offload        = nft_bitwise_offload,
 288 };
 289
 290 static int
 291 nft_bitwise_extract_u32_data(const struct nlattr * const tb, u32 *out)
 292 {
 293         struct nft_data_desc desc;
 294         struct nft_data data;
 295         int err = 0;
 296
 297         err = nft_data_init(NULL, &data, sizeof(data), &desc, tb);
 298         if (err < 0)
 299                 return err;
 300
 301         if (desc.type != NFT_DATA_VALUE || desc.len != sizeof(u32)) {
 302                 err = -EINVAL;
 303                 goto err;
 304         }
 305         *out = data.data[0];
 306 err:
 307         nft_data_release(&data, desc.type);
 308         return err;
 309 }
 310
 311 static int nft_bitwise_fast_init(const struct nft_ctx *ctx,
 312                                  const struct nft_expr *expr,
 313                                  const struct nlattr * const tb[])
 314 {
 315         struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
 316         int err;
 317
 318         err = nft_parse_register_load(tb[NFTA_BITWISE_SREG], &priv->sreg,
 319                                       sizeof(u32));
 320         if (err < 0)
 321                 return err;
 322
 323         err = nft_parse_register_store(ctx, tb[NFTA_BITWISE_DREG], &priv->dreg,
 324                                        NULL, NFT_DATA_VALUE, sizeof(u32));
 325         if (err < 0)
 326                 return err;
 327
 328         if (tb[NFTA_BITWISE_DATA])
 329                 return -EINVAL;
 330
 331         if (!tb[NFTA_BITWISE_MASK] ||
 332             !tb[NFTA_BITWISE_XOR])
 333                 return -EINVAL;
 334
 335         err = nft_bitwise_extract_u32_data(tb[NFTA_BITWISE_MASK], &priv->mask);
 336         if (err < 0)
 337                 return err;
 338
 339         err = nft_bitwise_extract_u32_data(tb[NFTA_BITWISE_XOR], &priv->xor);
 340         if (err < 0)
 341                 return err;
 342
 343         return 0;
 344 }
 345
 346 static int
 347 nft_bitwise_fast_dump(struct sk_buff *skb, const struct nft_expr *expr)
 348 {
 349         const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
 350         struct nft_data data;
 351
 352         if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
 353                 return -1;
 354         if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
 355                 return -1;
 356         if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(sizeof(u32))))
 357                 return -1;
 358         if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(NFT_BITWISE_BOOL)))
 359                 return -1;
 360
 361         data.data[0] = priv->mask;
 362         if (nft_data_dump(skb, NFTA_BITWISE_MASK, &data,
 363                           NFT_DATA_VALUE, sizeof(u32)) < 0)
 364                 return -1;
 365
 366         data.data[0] = priv->xor;
 367         if (nft_data_dump(skb, NFTA_BITWISE_XOR, &data,
 368                           NFT_DATA_VALUE, sizeof(u32)) < 0)
 369                 return -1;
 370
 371         return 0;
 372 }
 373
 374 static int nft_bitwise_fast_offload(struct nft_offload_ctx *ctx,
 375                                     struct nft_flow_rule *flow,
 376                                     const struct nft_expr *expr)
 377 {
 378         const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
 379         struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
 380
 381         if (priv->xor || priv->sreg != priv->dreg || reg->len != sizeof(u32))
 382                 return -EOPNOTSUPP;
 383
 384         reg->mask.data[0] = priv->mask;
 385         return 0;
 386 }
 387
 388 const struct nft_expr_ops nft_bitwise_fast_ops = {
 389         .type           = &nft_bitwise_type,
 390         .size           = NFT_EXPR_SIZE(sizeof(struct nft_bitwise_fast_expr)),
 391         .eval           = NULL, /* inlined */
 392         .init           = nft_bitwise_fast_init,
 393         .dump           = nft_bitwise_fast_dump,
 394         .offload        = nft_bitwise_fast_offload,
 395 };
 396
 397 static const struct nft_expr_ops *
 398 nft_bitwise_select_ops(const struct nft_ctx *ctx,
 399                        const struct nlattr * const tb[])
 400 {
 401         int err;
 402         u32 len;
 403
 404         if (!tb[NFTA_BITWISE_LEN] ||
 405             !tb[NFTA_BITWISE_SREG] ||
 406             !tb[NFTA_BITWISE_DREG])
 407                 return ERR_PTR(-EINVAL);
 408
 409         err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len);
 410         if (err < 0)
 411                 return ERR_PTR(err);
 412
 413         if (len != sizeof(u32))
 414                 return &nft_bitwise_ops;
 415
 416         if (tb[NFTA_BITWISE_OP] &&
 417             ntohl(nla_get_be32(tb[NFTA_BITWISE_OP])) != NFT_BITWISE_BOOL)
 418                 return &nft_bitwise_ops;
 419
 420         return &nft_bitwise_fast_ops;
 421 }
 422
 423 struct nft_expr_type nft_bitwise_type __read_mostly = {
 424         .name           = "bitwise",
 425         .select_ops     = nft_bitwise_select_ops,
 426         .policy         = nft_bitwise_policy,
 427         .maxattr        = NFTA_BITWISE_MAX,
 428         .owner          = THIS_MODULE,
 429 };