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  
nft_bitwise_eval_bool(u32 * dst,const u32 * src,const struct nft_bitwise * priv)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, sizeof(u32)); i++)
34  		dst[i] = (src[i] & priv->mask.data[i]) ^ priv->xor.data[i];
35  }
36  
nft_bitwise_eval_lshift(u32 * dst,const u32 * src,const struct nft_bitwise * priv)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  
nft_bitwise_eval_rshift(u32 * dst,const u32 * src,const struct nft_bitwise * priv)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  
nft_bitwise_eval(const struct nft_expr * expr,struct nft_regs * regs,const struct nft_pktinfo * pkt)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]	= NLA_POLICY_MAX(NLA_BE32, 255),
90  	[NFTA_BITWISE_DATA]	= { .type = NLA_NESTED },
91  };
92  
nft_bitwise_init_bool(struct nft_bitwise * priv,const struct nlattr * const tb[])93  static int nft_bitwise_init_bool(struct nft_bitwise *priv,
94  				 const struct nlattr *const tb[])
95  {
96  	struct nft_data_desc mask = {
97  		.type	= NFT_DATA_VALUE,
98  		.size	= sizeof(priv->mask),
99  		.len	= priv->len,
100  	};
101  	struct nft_data_desc xor = {
102  		.type	= NFT_DATA_VALUE,
103  		.size	= sizeof(priv->xor),
104  		.len	= priv->len,
105  	};
106  	int err;
107  
108  	if (tb[NFTA_BITWISE_DATA])
109  		return -EINVAL;
110  
111  	if (!tb[NFTA_BITWISE_MASK] ||
112  	    !tb[NFTA_BITWISE_XOR])
113  		return -EINVAL;
114  
115  	err = nft_data_init(NULL, &priv->mask, &mask, tb[NFTA_BITWISE_MASK]);
116  	if (err < 0)
117  		return err;
118  
119  	err = nft_data_init(NULL, &priv->xor, &xor, tb[NFTA_BITWISE_XOR]);
120  	if (err < 0)
121  		goto err_xor_err;
122  
123  	return 0;
124  
125  err_xor_err:
126  	nft_data_release(&priv->mask, mask.type);
127  
128  	return err;
129  }
130  
nft_bitwise_init_shift(struct nft_bitwise * priv,const struct nlattr * const tb[])131  static int nft_bitwise_init_shift(struct nft_bitwise *priv,
132  				  const struct nlattr *const tb[])
133  {
134  	struct nft_data_desc desc = {
135  		.type	= NFT_DATA_VALUE,
136  		.size	= sizeof(priv->data),
137  		.len	= sizeof(u32),
138  	};
139  	int err;
140  
141  	if (tb[NFTA_BITWISE_MASK] ||
142  	    tb[NFTA_BITWISE_XOR])
143  		return -EINVAL;
144  
145  	if (!tb[NFTA_BITWISE_DATA])
146  		return -EINVAL;
147  
148  	err = nft_data_init(NULL, &priv->data, &desc, tb[NFTA_BITWISE_DATA]);
149  	if (err < 0)
150  		return err;
151  
152  	if (priv->data.data[0] >= BITS_PER_TYPE(u32)) {
153  		nft_data_release(&priv->data, desc.type);
154  		return -EINVAL;
155  	}
156  
157  	return 0;
158  }
159  
nft_bitwise_init(const struct nft_ctx * ctx,const struct nft_expr * expr,const struct nlattr * const tb[])160  static int nft_bitwise_init(const struct nft_ctx *ctx,
161  			    const struct nft_expr *expr,
162  			    const struct nlattr * const tb[])
163  {
164  	struct nft_bitwise *priv = nft_expr_priv(expr);
165  	u32 len;
166  	int err;
167  
168  	err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len);
169  	if (err < 0)
170  		return err;
171  
172  	priv->len = len;
173  
174  	err = nft_parse_register_load(ctx, tb[NFTA_BITWISE_SREG], &priv->sreg,
175  				      priv->len);
176  	if (err < 0)
177  		return err;
178  
179  	err = nft_parse_register_store(ctx, tb[NFTA_BITWISE_DREG],
180  				       &priv->dreg, NULL, NFT_DATA_VALUE,
181  				       priv->len);
182  	if (err < 0)
183  		return err;
184  
185  	if (tb[NFTA_BITWISE_OP]) {
186  		priv->op = ntohl(nla_get_be32(tb[NFTA_BITWISE_OP]));
187  		switch (priv->op) {
188  		case NFT_BITWISE_BOOL:
189  		case NFT_BITWISE_LSHIFT:
190  		case NFT_BITWISE_RSHIFT:
191  			break;
192  		default:
193  			return -EOPNOTSUPP;
194  		}
195  	} else {
196  		priv->op = NFT_BITWISE_BOOL;
197  	}
198  
199  	switch(priv->op) {
200  	case NFT_BITWISE_BOOL:
201  		err = nft_bitwise_init_bool(priv, tb);
202  		break;
203  	case NFT_BITWISE_LSHIFT:
204  	case NFT_BITWISE_RSHIFT:
205  		err = nft_bitwise_init_shift(priv, tb);
206  		break;
207  	}
208  
209  	return err;
210  }
211  
nft_bitwise_dump_bool(struct sk_buff * skb,const struct nft_bitwise * priv)212  static int nft_bitwise_dump_bool(struct sk_buff *skb,
213  				 const struct nft_bitwise *priv)
214  {
215  	if (nft_data_dump(skb, NFTA_BITWISE_MASK, &priv->mask,
216  			  NFT_DATA_VALUE, priv->len) < 0)
217  		return -1;
218  
219  	if (nft_data_dump(skb, NFTA_BITWISE_XOR, &priv->xor,
220  			  NFT_DATA_VALUE, priv->len) < 0)
221  		return -1;
222  
223  	return 0;
224  }
225  
nft_bitwise_dump_shift(struct sk_buff * skb,const struct nft_bitwise * priv)226  static int nft_bitwise_dump_shift(struct sk_buff *skb,
227  				  const struct nft_bitwise *priv)
228  {
229  	if (nft_data_dump(skb, NFTA_BITWISE_DATA, &priv->data,
230  			  NFT_DATA_VALUE, sizeof(u32)) < 0)
231  		return -1;
232  	return 0;
233  }
234  
nft_bitwise_dump(struct sk_buff * skb,const struct nft_expr * expr,bool reset)235  static int nft_bitwise_dump(struct sk_buff *skb,
236  			    const struct nft_expr *expr, bool reset)
237  {
238  	const struct nft_bitwise *priv = nft_expr_priv(expr);
239  	int err = 0;
240  
241  	if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
242  		return -1;
243  	if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
244  		return -1;
245  	if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(priv->len)))
246  		return -1;
247  	if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(priv->op)))
248  		return -1;
249  
250  	switch (priv->op) {
251  	case NFT_BITWISE_BOOL:
252  		err = nft_bitwise_dump_bool(skb, priv);
253  		break;
254  	case NFT_BITWISE_LSHIFT:
255  	case NFT_BITWISE_RSHIFT:
256  		err = nft_bitwise_dump_shift(skb, priv);
257  		break;
258  	}
259  
260  	return err;
261  }
262  
263  static struct nft_data zero;
264  
nft_bitwise_offload(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_expr * expr)265  static int nft_bitwise_offload(struct nft_offload_ctx *ctx,
266  			       struct nft_flow_rule *flow,
267  			       const struct nft_expr *expr)
268  {
269  	const struct nft_bitwise *priv = nft_expr_priv(expr);
270  	struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
271  
272  	if (priv->op != NFT_BITWISE_BOOL)
273  		return -EOPNOTSUPP;
274  
275  	if (memcmp(&priv->xor, &zero, sizeof(priv->xor)) ||
276  	    priv->sreg != priv->dreg || priv->len != reg->len)
277  		return -EOPNOTSUPP;
278  
279  	memcpy(&reg->mask, &priv->mask, sizeof(priv->mask));
280  
281  	return 0;
282  }
283  
nft_bitwise_reduce(struct nft_regs_track * track,const struct nft_expr * expr)284  static bool nft_bitwise_reduce(struct nft_regs_track *track,
285  			       const struct nft_expr *expr)
286  {
287  	const struct nft_bitwise *priv = nft_expr_priv(expr);
288  	const struct nft_bitwise *bitwise;
289  	unsigned int regcount;
290  	u8 dreg;
291  	int i;
292  
293  	if (!track->regs[priv->sreg].selector)
294  		return false;
295  
296  	bitwise = nft_expr_priv(track->regs[priv->dreg].selector);
297  	if (track->regs[priv->sreg].selector == track->regs[priv->dreg].selector &&
298  	    track->regs[priv->sreg].num_reg == 0 &&
299  	    track->regs[priv->dreg].bitwise &&
300  	    track->regs[priv->dreg].bitwise->ops == expr->ops &&
301  	    priv->sreg == bitwise->sreg &&
302  	    priv->dreg == bitwise->dreg &&
303  	    priv->op == bitwise->op &&
304  	    priv->len == bitwise->len &&
305  	    !memcmp(&priv->mask, &bitwise->mask, sizeof(priv->mask)) &&
306  	    !memcmp(&priv->xor, &bitwise->xor, sizeof(priv->xor)) &&
307  	    !memcmp(&priv->data, &bitwise->data, sizeof(priv->data))) {
308  		track->cur = expr;
309  		return true;
310  	}
311  
312  	if (track->regs[priv->sreg].bitwise ||
313  	    track->regs[priv->sreg].num_reg != 0) {
314  		nft_reg_track_cancel(track, priv->dreg, priv->len);
315  		return false;
316  	}
317  
318  	if (priv->sreg != priv->dreg) {
319  		nft_reg_track_update(track, track->regs[priv->sreg].selector,
320  				     priv->dreg, priv->len);
321  	}
322  
323  	dreg = priv->dreg;
324  	regcount = DIV_ROUND_UP(priv->len, NFT_REG32_SIZE);
325  	for (i = 0; i < regcount; i++, dreg++)
326  		track->regs[dreg].bitwise = expr;
327  
328  	return false;
329  }
330  
331  static const struct nft_expr_ops nft_bitwise_ops = {
332  	.type		= &nft_bitwise_type,
333  	.size		= NFT_EXPR_SIZE(sizeof(struct nft_bitwise)),
334  	.eval		= nft_bitwise_eval,
335  	.init		= nft_bitwise_init,
336  	.dump		= nft_bitwise_dump,
337  	.reduce		= nft_bitwise_reduce,
338  	.offload	= nft_bitwise_offload,
339  };
340  
341  static int
nft_bitwise_extract_u32_data(const struct nlattr * const tb,u32 * out)342  nft_bitwise_extract_u32_data(const struct nlattr * const tb, u32 *out)
343  {
344  	struct nft_data data;
345  	struct nft_data_desc desc = {
346  		.type	= NFT_DATA_VALUE,
347  		.size	= sizeof(data),
348  		.len	= sizeof(u32),
349  	};
350  	int err;
351  
352  	err = nft_data_init(NULL, &data, &desc, tb);
353  	if (err < 0)
354  		return err;
355  
356  	*out = data.data[0];
357  
358  	return 0;
359  }
360  
nft_bitwise_fast_init(const struct nft_ctx * ctx,const struct nft_expr * expr,const struct nlattr * const tb[])361  static int nft_bitwise_fast_init(const struct nft_ctx *ctx,
362  				 const struct nft_expr *expr,
363  				 const struct nlattr * const tb[])
364  {
365  	struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
366  	int err;
367  
368  	err = nft_parse_register_load(ctx, tb[NFTA_BITWISE_SREG], &priv->sreg,
369  				      sizeof(u32));
370  	if (err < 0)
371  		return err;
372  
373  	err = nft_parse_register_store(ctx, tb[NFTA_BITWISE_DREG], &priv->dreg,
374  				       NULL, NFT_DATA_VALUE, sizeof(u32));
375  	if (err < 0)
376  		return err;
377  
378  	if (tb[NFTA_BITWISE_DATA])
379  		return -EINVAL;
380  
381  	if (!tb[NFTA_BITWISE_MASK] ||
382  	    !tb[NFTA_BITWISE_XOR])
383  		return -EINVAL;
384  
385  	err = nft_bitwise_extract_u32_data(tb[NFTA_BITWISE_MASK], &priv->mask);
386  	if (err < 0)
387  		return err;
388  
389  	err = nft_bitwise_extract_u32_data(tb[NFTA_BITWISE_XOR], &priv->xor);
390  	if (err < 0)
391  		return err;
392  
393  	return 0;
394  }
395  
396  static int
nft_bitwise_fast_dump(struct sk_buff * skb,const struct nft_expr * expr,bool reset)397  nft_bitwise_fast_dump(struct sk_buff *skb,
398  		      const struct nft_expr *expr, bool reset)
399  {
400  	const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
401  	struct nft_data data;
402  
403  	if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
404  		return -1;
405  	if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
406  		return -1;
407  	if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(sizeof(u32))))
408  		return -1;
409  	if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(NFT_BITWISE_BOOL)))
410  		return -1;
411  
412  	data.data[0] = priv->mask;
413  	if (nft_data_dump(skb, NFTA_BITWISE_MASK, &data,
414  			  NFT_DATA_VALUE, sizeof(u32)) < 0)
415  		return -1;
416  
417  	data.data[0] = priv->xor;
418  	if (nft_data_dump(skb, NFTA_BITWISE_XOR, &data,
419  			  NFT_DATA_VALUE, sizeof(u32)) < 0)
420  		return -1;
421  
422  	return 0;
423  }
424  
nft_bitwise_fast_offload(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_expr * expr)425  static int nft_bitwise_fast_offload(struct nft_offload_ctx *ctx,
426  				    struct nft_flow_rule *flow,
427  				    const struct nft_expr *expr)
428  {
429  	const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
430  	struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
431  
432  	if (priv->xor || priv->sreg != priv->dreg || reg->len != sizeof(u32))
433  		return -EOPNOTSUPP;
434  
435  	reg->mask.data[0] = priv->mask;
436  	return 0;
437  }
438  
nft_bitwise_fast_reduce(struct nft_regs_track * track,const struct nft_expr * expr)439  static bool nft_bitwise_fast_reduce(struct nft_regs_track *track,
440  				    const struct nft_expr *expr)
441  {
442  	const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
443  	const struct nft_bitwise_fast_expr *bitwise;
444  
445  	if (!track->regs[priv->sreg].selector)
446  		return false;
447  
448  	bitwise = nft_expr_priv(track->regs[priv->dreg].selector);
449  	if (track->regs[priv->sreg].selector == track->regs[priv->dreg].selector &&
450  	    track->regs[priv->dreg].bitwise &&
451  	    track->regs[priv->dreg].bitwise->ops == expr->ops &&
452  	    priv->sreg == bitwise->sreg &&
453  	    priv->dreg == bitwise->dreg &&
454  	    priv->mask == bitwise->mask &&
455  	    priv->xor == bitwise->xor) {
456  		track->cur = expr;
457  		return true;
458  	}
459  
460  	if (track->regs[priv->sreg].bitwise) {
461  		nft_reg_track_cancel(track, priv->dreg, NFT_REG32_SIZE);
462  		return false;
463  	}
464  
465  	if (priv->sreg != priv->dreg) {
466  		track->regs[priv->dreg].selector =
467  			track->regs[priv->sreg].selector;
468  	}
469  	track->regs[priv->dreg].bitwise = expr;
470  
471  	return false;
472  }
473  
474  const struct nft_expr_ops nft_bitwise_fast_ops = {
475  	.type		= &nft_bitwise_type,
476  	.size		= NFT_EXPR_SIZE(sizeof(struct nft_bitwise_fast_expr)),
477  	.eval		= NULL, /* inlined */
478  	.init		= nft_bitwise_fast_init,
479  	.dump		= nft_bitwise_fast_dump,
480  	.reduce		= nft_bitwise_fast_reduce,
481  	.offload	= nft_bitwise_fast_offload,
482  };
483  
484  static const struct nft_expr_ops *
nft_bitwise_select_ops(const struct nft_ctx * ctx,const struct nlattr * const tb[])485  nft_bitwise_select_ops(const struct nft_ctx *ctx,
486  		       const struct nlattr * const tb[])
487  {
488  	int err;
489  	u32 len;
490  
491  	if (!tb[NFTA_BITWISE_LEN] ||
492  	    !tb[NFTA_BITWISE_SREG] ||
493  	    !tb[NFTA_BITWISE_DREG])
494  		return ERR_PTR(-EINVAL);
495  
496  	err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len);
497  	if (err < 0)
498  		return ERR_PTR(err);
499  
500  	if (len != sizeof(u32))
501  		return &nft_bitwise_ops;
502  
503  	if (tb[NFTA_BITWISE_OP] &&
504  	    ntohl(nla_get_be32(tb[NFTA_BITWISE_OP])) != NFT_BITWISE_BOOL)
505  		return &nft_bitwise_ops;
506  
507  	return &nft_bitwise_fast_ops;
508  }
509  
510  struct nft_expr_type nft_bitwise_type __read_mostly = {
511  	.name		= "bitwise",
512  	.select_ops	= nft_bitwise_select_ops,
513  	.policy		= nft_bitwise_policy,
514  	.maxattr	= NFTA_BITWISE_MAX,
515  	.owner		= THIS_MODULE,
516  };
517  
nft_expr_reduce_bitwise(struct nft_regs_track * track,const struct nft_expr * expr)518  bool nft_expr_reduce_bitwise(struct nft_regs_track *track,
519  			     const struct nft_expr *expr)
520  {
521  	const struct nft_expr *last = track->last;
522  	const struct nft_expr *next;
523  
524  	if (expr == last)
525  		return false;
526  
527  	next = nft_expr_next(expr);
528  	if (next->ops == &nft_bitwise_ops)
529  		return nft_bitwise_reduce(track, next);
530  	else if (next->ops == &nft_bitwise_fast_ops)
531  		return nft_bitwise_fast_reduce(track, next);
532  
533  	return false;
534  }
535  EXPORT_SYMBOL_GPL(nft_expr_reduce_bitwise);
536