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]	= { .type = NLA_U32 },
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(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)235 static int nft_bitwise_dump(struct sk_buff *skb, const struct nft_expr *expr)
236 {
237 	const struct nft_bitwise *priv = nft_expr_priv(expr);
238 	int err = 0;
239 
240 	if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
241 		return -1;
242 	if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
243 		return -1;
244 	if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(priv->len)))
245 		return -1;
246 	if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(priv->op)))
247 		return -1;
248 
249 	switch (priv->op) {
250 	case NFT_BITWISE_BOOL:
251 		err = nft_bitwise_dump_bool(skb, priv);
252 		break;
253 	case NFT_BITWISE_LSHIFT:
254 	case NFT_BITWISE_RSHIFT:
255 		err = nft_bitwise_dump_shift(skb, priv);
256 		break;
257 	}
258 
259 	return err;
260 }
261 
262 static struct nft_data zero;
263 
nft_bitwise_offload(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_expr * expr)264 static int nft_bitwise_offload(struct nft_offload_ctx *ctx,
265 			       struct nft_flow_rule *flow,
266 			       const struct nft_expr *expr)
267 {
268 	const struct nft_bitwise *priv = nft_expr_priv(expr);
269 	struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
270 
271 	if (priv->op != NFT_BITWISE_BOOL)
272 		return -EOPNOTSUPP;
273 
274 	if (memcmp(&priv->xor, &zero, sizeof(priv->xor)) ||
275 	    priv->sreg != priv->dreg || priv->len != reg->len)
276 		return -EOPNOTSUPP;
277 
278 	memcpy(&reg->mask, &priv->mask, sizeof(priv->mask));
279 
280 	return 0;
281 }
282 
nft_bitwise_reduce(struct nft_regs_track * track,const struct nft_expr * expr)283 static bool nft_bitwise_reduce(struct nft_regs_track *track,
284 			       const struct nft_expr *expr)
285 {
286 	const struct nft_bitwise *priv = nft_expr_priv(expr);
287 	const struct nft_bitwise *bitwise;
288 	unsigned int regcount;
289 	u8 dreg;
290 	int i;
291 
292 	if (!track->regs[priv->sreg].selector)
293 		return false;
294 
295 	bitwise = nft_expr_priv(track->regs[priv->dreg].selector);
296 	if (track->regs[priv->sreg].selector == track->regs[priv->dreg].selector &&
297 	    track->regs[priv->sreg].num_reg == 0 &&
298 	    track->regs[priv->dreg].bitwise &&
299 	    track->regs[priv->dreg].bitwise->ops == expr->ops &&
300 	    priv->sreg == bitwise->sreg &&
301 	    priv->dreg == bitwise->dreg &&
302 	    priv->op == bitwise->op &&
303 	    priv->len == bitwise->len &&
304 	    !memcmp(&priv->mask, &bitwise->mask, sizeof(priv->mask)) &&
305 	    !memcmp(&priv->xor, &bitwise->xor, sizeof(priv->xor)) &&
306 	    !memcmp(&priv->data, &bitwise->data, sizeof(priv->data))) {
307 		track->cur = expr;
308 		return true;
309 	}
310 
311 	if (track->regs[priv->sreg].bitwise ||
312 	    track->regs[priv->sreg].num_reg != 0) {
313 		nft_reg_track_cancel(track, priv->dreg, priv->len);
314 		return false;
315 	}
316 
317 	if (priv->sreg != priv->dreg) {
318 		nft_reg_track_update(track, track->regs[priv->sreg].selector,
319 				     priv->dreg, priv->len);
320 	}
321 
322 	dreg = priv->dreg;
323 	regcount = DIV_ROUND_UP(priv->len, NFT_REG32_SIZE);
324 	for (i = 0; i < regcount; i++, dreg++)
325 		track->regs[priv->dreg].bitwise = expr;
326 
327 	return false;
328 }
329 
330 static const struct nft_expr_ops nft_bitwise_ops = {
331 	.type		= &nft_bitwise_type,
332 	.size		= NFT_EXPR_SIZE(sizeof(struct nft_bitwise)),
333 	.eval		= nft_bitwise_eval,
334 	.init		= nft_bitwise_init,
335 	.dump		= nft_bitwise_dump,
336 	.reduce		= nft_bitwise_reduce,
337 	.offload	= nft_bitwise_offload,
338 };
339 
340 static int
nft_bitwise_extract_u32_data(const struct nlattr * const tb,u32 * out)341 nft_bitwise_extract_u32_data(const struct nlattr * const tb, u32 *out)
342 {
343 	struct nft_data data;
344 	struct nft_data_desc desc = {
345 		.type	= NFT_DATA_VALUE,
346 		.size	= sizeof(data),
347 		.len	= sizeof(u32),
348 	};
349 	int err;
350 
351 	err = nft_data_init(NULL, &data, &desc, tb);
352 	if (err < 0)
353 		return err;
354 
355 	*out = data.data[0];
356 
357 	return 0;
358 }
359 
nft_bitwise_fast_init(const struct nft_ctx * ctx,const struct nft_expr * expr,const struct nlattr * const tb[])360 static int nft_bitwise_fast_init(const struct nft_ctx *ctx,
361 				 const struct nft_expr *expr,
362 				 const struct nlattr * const tb[])
363 {
364 	struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
365 	int err;
366 
367 	err = nft_parse_register_load(tb[NFTA_BITWISE_SREG], &priv->sreg,
368 				      sizeof(u32));
369 	if (err < 0)
370 		return err;
371 
372 	err = nft_parse_register_store(ctx, tb[NFTA_BITWISE_DREG], &priv->dreg,
373 				       NULL, NFT_DATA_VALUE, sizeof(u32));
374 	if (err < 0)
375 		return err;
376 
377 	if (tb[NFTA_BITWISE_DATA])
378 		return -EINVAL;
379 
380 	if (!tb[NFTA_BITWISE_MASK] ||
381 	    !tb[NFTA_BITWISE_XOR])
382 		return -EINVAL;
383 
384 	err = nft_bitwise_extract_u32_data(tb[NFTA_BITWISE_MASK], &priv->mask);
385 	if (err < 0)
386 		return err;
387 
388 	err = nft_bitwise_extract_u32_data(tb[NFTA_BITWISE_XOR], &priv->xor);
389 	if (err < 0)
390 		return err;
391 
392 	return 0;
393 }
394 
395 static int
nft_bitwise_fast_dump(struct sk_buff * skb,const struct nft_expr * expr)396 nft_bitwise_fast_dump(struct sk_buff *skb, const struct nft_expr *expr)
397 {
398 	const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
399 	struct nft_data data;
400 
401 	if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
402 		return -1;
403 	if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
404 		return -1;
405 	if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(sizeof(u32))))
406 		return -1;
407 	if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(NFT_BITWISE_BOOL)))
408 		return -1;
409 
410 	data.data[0] = priv->mask;
411 	if (nft_data_dump(skb, NFTA_BITWISE_MASK, &data,
412 			  NFT_DATA_VALUE, sizeof(u32)) < 0)
413 		return -1;
414 
415 	data.data[0] = priv->xor;
416 	if (nft_data_dump(skb, NFTA_BITWISE_XOR, &data,
417 			  NFT_DATA_VALUE, sizeof(u32)) < 0)
418 		return -1;
419 
420 	return 0;
421 }
422 
nft_bitwise_fast_offload(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_expr * expr)423 static int nft_bitwise_fast_offload(struct nft_offload_ctx *ctx,
424 				    struct nft_flow_rule *flow,
425 				    const struct nft_expr *expr)
426 {
427 	const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
428 	struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
429 
430 	if (priv->xor || priv->sreg != priv->dreg || reg->len != sizeof(u32))
431 		return -EOPNOTSUPP;
432 
433 	reg->mask.data[0] = priv->mask;
434 	return 0;
435 }
436 
nft_bitwise_fast_reduce(struct nft_regs_track * track,const struct nft_expr * expr)437 static bool nft_bitwise_fast_reduce(struct nft_regs_track *track,
438 				    const struct nft_expr *expr)
439 {
440 	const struct nft_bitwise_fast_expr *priv = nft_expr_priv(expr);
441 	const struct nft_bitwise_fast_expr *bitwise;
442 
443 	if (!track->regs[priv->sreg].selector)
444 		return false;
445 
446 	bitwise = nft_expr_priv(track->regs[priv->dreg].selector);
447 	if (track->regs[priv->sreg].selector == track->regs[priv->dreg].selector &&
448 	    track->regs[priv->dreg].bitwise &&
449 	    track->regs[priv->dreg].bitwise->ops == expr->ops &&
450 	    priv->sreg == bitwise->sreg &&
451 	    priv->dreg == bitwise->dreg &&
452 	    priv->mask == bitwise->mask &&
453 	    priv->xor == bitwise->xor) {
454 		track->cur = expr;
455 		return true;
456 	}
457 
458 	if (track->regs[priv->sreg].bitwise) {
459 		nft_reg_track_cancel(track, priv->dreg, NFT_REG32_SIZE);
460 		return false;
461 	}
462 
463 	if (priv->sreg != priv->dreg) {
464 		track->regs[priv->dreg].selector =
465 			track->regs[priv->sreg].selector;
466 	}
467 	track->regs[priv->dreg].bitwise = expr;
468 
469 	return false;
470 }
471 
472 const struct nft_expr_ops nft_bitwise_fast_ops = {
473 	.type		= &nft_bitwise_type,
474 	.size		= NFT_EXPR_SIZE(sizeof(struct nft_bitwise_fast_expr)),
475 	.eval		= NULL, /* inlined */
476 	.init		= nft_bitwise_fast_init,
477 	.dump		= nft_bitwise_fast_dump,
478 	.reduce		= nft_bitwise_fast_reduce,
479 	.offload	= nft_bitwise_fast_offload,
480 };
481 
482 static const struct nft_expr_ops *
nft_bitwise_select_ops(const struct nft_ctx * ctx,const struct nlattr * const tb[])483 nft_bitwise_select_ops(const struct nft_ctx *ctx,
484 		       const struct nlattr * const tb[])
485 {
486 	int err;
487 	u32 len;
488 
489 	if (!tb[NFTA_BITWISE_LEN] ||
490 	    !tb[NFTA_BITWISE_SREG] ||
491 	    !tb[NFTA_BITWISE_DREG])
492 		return ERR_PTR(-EINVAL);
493 
494 	err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len);
495 	if (err < 0)
496 		return ERR_PTR(err);
497 
498 	if (len != sizeof(u32))
499 		return &nft_bitwise_ops;
500 
501 	if (tb[NFTA_BITWISE_OP] &&
502 	    ntohl(nla_get_be32(tb[NFTA_BITWISE_OP])) != NFT_BITWISE_BOOL)
503 		return &nft_bitwise_ops;
504 
505 	return &nft_bitwise_fast_ops;
506 }
507 
508 struct nft_expr_type nft_bitwise_type __read_mostly = {
509 	.name		= "bitwise",
510 	.select_ops	= nft_bitwise_select_ops,
511 	.policy		= nft_bitwise_policy,
512 	.maxattr	= NFTA_BITWISE_MAX,
513 	.owner		= THIS_MODULE,
514 };
515 
nft_expr_reduce_bitwise(struct nft_regs_track * track,const struct nft_expr * expr)516 bool nft_expr_reduce_bitwise(struct nft_regs_track *track,
517 			     const struct nft_expr *expr)
518 {
519 	const struct nft_expr *last = track->last;
520 	const struct nft_expr *next;
521 
522 	if (expr == last)
523 		return false;
524 
525 	next = nft_expr_next(expr);
526 	if (next->ops == &nft_bitwise_ops)
527 		return nft_bitwise_reduce(track, next);
528 	else if (next->ops == &nft_bitwise_fast_ops)
529 		return nft_bitwise_fast_reduce(track, next);
530 
531 	return false;
532 }
533 EXPORT_SYMBOL_GPL(nft_expr_reduce_bitwise);
534