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
20static void nft_payload_eval(const struct nft_expr *expr,
21			     struct nft_regs *regs,
22			     const struct nft_pktinfo *pkt)
23{
24	const struct nft_payload *priv = nft_expr_priv(expr);
25	const struct sk_buff *skb = pkt->skb;
26	u32 *dest = &regs->data[priv->dreg];
27	int offset;
28
29	switch (priv->base) {
30	case NFT_PAYLOAD_LL_HEADER:
31		if (!skb_mac_header_was_set(skb))
32			goto err;
33		offset = skb_mac_header(skb) - skb->data;
34		break;
35	case NFT_PAYLOAD_NETWORK_HEADER:
36		offset = skb_network_offset(skb);
37		break;
38	case NFT_PAYLOAD_TRANSPORT_HEADER:
39		offset = pkt->xt.thoff;
40		break;
41	default:
42		BUG();
43	}
44	offset += priv->offset;
45
46	dest[priv->len / NFT_REG32_SIZE] = 0;
47	if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
48		goto err;
49	return;
50err:
51	regs->verdict.code = NFT_BREAK;
52}
53
54static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
55	[NFTA_PAYLOAD_DREG]	= { .type = NLA_U32 },
56	[NFTA_PAYLOAD_BASE]	= { .type = NLA_U32 },
57	[NFTA_PAYLOAD_OFFSET]	= { .type = NLA_U32 },
58	[NFTA_PAYLOAD_LEN]	= { .type = NLA_U32 },
59};
60
61static int nft_payload_init(const struct nft_ctx *ctx,
62			    const struct nft_expr *expr,
63			    const struct nlattr * const tb[])
64{
65	struct nft_payload *priv = nft_expr_priv(expr);
66
67	priv->base   = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
68	priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
69	priv->len    = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
70	priv->dreg   = nft_parse_register(tb[NFTA_PAYLOAD_DREG]);
71
72	return nft_validate_register_store(ctx, priv->dreg, NULL,
73					   NFT_DATA_VALUE, priv->len);
74}
75
76static int nft_payload_dump(struct sk_buff *skb, const struct nft_expr *expr)
77{
78	const struct nft_payload *priv = nft_expr_priv(expr);
79
80	if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) ||
81	    nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
82	    nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
83	    nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
84		goto nla_put_failure;
85	return 0;
86
87nla_put_failure:
88	return -1;
89}
90
91static struct nft_expr_type nft_payload_type;
92static const struct nft_expr_ops nft_payload_ops = {
93	.type		= &nft_payload_type,
94	.size		= NFT_EXPR_SIZE(sizeof(struct nft_payload)),
95	.eval		= nft_payload_eval,
96	.init		= nft_payload_init,
97	.dump		= nft_payload_dump,
98};
99
100const struct nft_expr_ops nft_payload_fast_ops = {
101	.type		= &nft_payload_type,
102	.size		= NFT_EXPR_SIZE(sizeof(struct nft_payload)),
103	.eval		= nft_payload_eval,
104	.init		= nft_payload_init,
105	.dump		= nft_payload_dump,
106};
107
108static const struct nft_expr_ops *
109nft_payload_select_ops(const struct nft_ctx *ctx,
110		       const struct nlattr * const tb[])
111{
112	enum nft_payload_bases base;
113	unsigned int offset, len;
114
115	if (tb[NFTA_PAYLOAD_DREG] == NULL ||
116	    tb[NFTA_PAYLOAD_BASE] == NULL ||
117	    tb[NFTA_PAYLOAD_OFFSET] == NULL ||
118	    tb[NFTA_PAYLOAD_LEN] == NULL)
119		return ERR_PTR(-EINVAL);
120
121	base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
122	switch (base) {
123	case NFT_PAYLOAD_LL_HEADER:
124	case NFT_PAYLOAD_NETWORK_HEADER:
125	case NFT_PAYLOAD_TRANSPORT_HEADER:
126		break;
127	default:
128		return ERR_PTR(-EOPNOTSUPP);
129	}
130
131	offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
132	len    = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
133
134	if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
135	    base != NFT_PAYLOAD_LL_HEADER)
136		return &nft_payload_fast_ops;
137	else
138		return &nft_payload_ops;
139}
140
141static struct nft_expr_type nft_payload_type __read_mostly = {
142	.name		= "payload",
143	.select_ops	= nft_payload_select_ops,
144	.policy		= nft_payload_policy,
145	.maxattr	= NFTA_PAYLOAD_MAX,
146	.owner		= THIS_MODULE,
147};
148
149int __init nft_payload_module_init(void)
150{
151	return nft_register_expr(&nft_payload_type);
152}
153
154void nft_payload_module_exit(void)
155{
156	nft_unregister_expr(&nft_payload_type);
157}
158