1/*
  2 * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
  3 *
  4 * This program is free software; you can redistribute it and/or modify
  5 * it under the terms of the GNU General Public License version 2 as
  6 * published by the Free Software Foundation.
  7 *
  8 * Development of this code funded by Astaro AG (http://www.astaro.com/)
  9 */
 10
 11#include <linux/kernel.h>
 12#include <linux/init.h>
 13#include <linux/module.h>
 14#include <linux/netlink.h>
 15#include <linux/netfilter.h>
 16#include <linux/netfilter/nf_tables.h>
 17#include <net/netfilter/nf_tables_core.h>
 18#include <net/netfilter/nf_tables.h>
 19
 20struct nft_bitwise {
 21	enum nft_registers	sreg:8;
 22	enum nft_registers	dreg:8;
 23	u8			len;
 24	struct nft_data		mask;
 25	struct nft_data		xor;
 26};
 27
 28static void nft_bitwise_eval(const struct nft_expr *expr,
 29			     struct nft_regs *regs,
 30			     const struct nft_pktinfo *pkt)
 31{
 32	const struct nft_bitwise *priv = nft_expr_priv(expr);
 33	const u32 *src = &regs->data[priv->sreg];
 34	u32 *dst = &regs->data[priv->dreg];
 35	unsigned int i;
 36
 37	for (i = 0; i < DIV_ROUND_UP(priv->len, 4); i++)
 38		dst[i] = (src[i] & priv->mask.data[i]) ^ priv->xor.data[i];
 39}
 40
 41static const struct nla_policy nft_bitwise_policy[NFTA_BITWISE_MAX + 1] = {
 42	[NFTA_BITWISE_SREG]	= { .type = NLA_U32 },
 43	[NFTA_BITWISE_DREG]	= { .type = NLA_U32 },
 44	[NFTA_BITWISE_LEN]	= { .type = NLA_U32 },
 45	[NFTA_BITWISE_MASK]	= { .type = NLA_NESTED },
 46	[NFTA_BITWISE_XOR]	= { .type = NLA_NESTED },
 47};
 48
 49static int nft_bitwise_init(const struct nft_ctx *ctx,
 50			    const struct nft_expr *expr,
 51			    const struct nlattr * const tb[])
 52{
 53	struct nft_bitwise *priv = nft_expr_priv(expr);
 54	struct nft_data_desc d1, d2;
 55	u32 len;
 56	int err;
 57
 58	if (tb[NFTA_BITWISE_SREG] == NULL ||
 59	    tb[NFTA_BITWISE_DREG] == NULL ||
 60	    tb[NFTA_BITWISE_LEN] == NULL ||
 61	    tb[NFTA_BITWISE_MASK] == NULL ||
 62	    tb[NFTA_BITWISE_XOR] == NULL)
 63		return -EINVAL;
 64
 65	err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len);
 66	if (err < 0)
 67		return err;
 68
 69	priv->len = len;
 70
 71	priv->sreg = nft_parse_register(tb[NFTA_BITWISE_SREG]);
 72	err = nft_validate_register_load(priv->sreg, priv->len);
 73	if (err < 0)
 74		return err;
 75
 76	priv->dreg = nft_parse_register(tb[NFTA_BITWISE_DREG]);
 77	err = nft_validate_register_store(ctx, priv->dreg, NULL,
 78					  NFT_DATA_VALUE, priv->len);
 79	if (err < 0)
 80		return err;
 81
 82	err = nft_data_init(NULL, &priv->mask, sizeof(priv->mask), &d1,
 83			    tb[NFTA_BITWISE_MASK]);
 84	if (err < 0)
 85		return err;
 86	if (d1.len != priv->len)
 87		return -EINVAL;
 88
 89	err = nft_data_init(NULL, &priv->xor, sizeof(priv->xor), &d2,
 90			    tb[NFTA_BITWISE_XOR]);
 91	if (err < 0)
 92		return err;
 93	if (d2.len != priv->len)
 94		return -EINVAL;
 95
 96	return 0;
 97}
 98
 99static int nft_bitwise_dump(struct sk_buff *skb, const struct nft_expr *expr)
100{
101	const struct nft_bitwise *priv = nft_expr_priv(expr);
102
103	if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg))
104		goto nla_put_failure;
105	if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg))
106		goto nla_put_failure;
107	if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(priv->len)))
108		goto nla_put_failure;
109
110	if (nft_data_dump(skb, NFTA_BITWISE_MASK, &priv->mask,
111			  NFT_DATA_VALUE, priv->len) < 0)
112		goto nla_put_failure;
113
114	if (nft_data_dump(skb, NFTA_BITWISE_XOR, &priv->xor,
115			  NFT_DATA_VALUE, priv->len) < 0)
116		goto nla_put_failure;
117
118	return 0;
119
120nla_put_failure:
121	return -1;
122}
123
124static const struct nft_expr_ops nft_bitwise_ops = {
125	.type		= &nft_bitwise_type,
126	.size		= NFT_EXPR_SIZE(sizeof(struct nft_bitwise)),
127	.eval		= nft_bitwise_eval,
128	.init		= nft_bitwise_init,
129	.dump		= nft_bitwise_dump,
130};
131
132struct nft_expr_type nft_bitwise_type __read_mostly = {
133	.name		= "bitwise",
134	.ops		= &nft_bitwise_ops,
135	.policy		= nft_bitwise_policy,
136	.maxattr	= NFTA_BITWISE_MAX,
137	.owner		= THIS_MODULE,
138};