2 * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
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.
8 * Development of this code funded by Astaro AG (http://www.astaro.com/)
11 #include <linux/kernel.h>
12 #include <linux/if_vlan.h>
13 #include <linux/init.h>
14 #include <linux/module.h>
15 #include <linux/netlink.h>
16 #include <linux/netfilter.h>
17 #include <linux/netfilter/nf_tables.h>
18 #include <net/netfilter/nf_tables_core.h>
19 #include <net/netfilter/nf_tables.h>
21 /* add vlan header into the user buffer for if tag was removed by offloads */
23 nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u8 offset, u8 len)
25 int mac_off = skb_mac_header(skb) - skb->data;
26 u8 vlan_len, *vlanh, *dst_u8 = (u8 *) d;
27 struct vlan_ethhdr veth;
30 if (offset < ETH_HLEN) {
31 u8 ethlen = min_t(u8, len, ETH_HLEN - offset);
33 if (skb_copy_bits(skb, mac_off, &veth, ETH_HLEN))
36 veth.h_vlan_proto = skb->vlan_proto;
38 memcpy(dst_u8, vlanh + offset, ethlen);
46 } else if (offset >= VLAN_ETH_HLEN) {
51 veth.h_vlan_TCI = htons(skb_vlan_tag_get(skb));
52 veth.h_vlan_encapsulated_proto = skb->protocol;
56 vlan_len = min_t(u8, len, VLAN_ETH_HLEN - offset);
57 memcpy(dst_u8, vlanh, vlan_len);
65 return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0;
68 static void nft_payload_eval(const struct nft_expr *expr,
69 struct nft_regs *regs,
70 const struct nft_pktinfo *pkt)
72 const struct nft_payload *priv = nft_expr_priv(expr);
73 const struct sk_buff *skb = pkt->skb;
74 u32 *dest = ®s->data[priv->dreg];
77 if (priv->len % NFT_REG32_SIZE)
78 dest[priv->len / NFT_REG32_SIZE] = 0;
81 case NFT_PAYLOAD_LL_HEADER:
82 if (!skb_mac_header_was_set(skb))
85 if (skb_vlan_tag_present(skb)) {
86 if (!nft_payload_copy_vlan(dest, skb,
87 priv->offset, priv->len))
91 offset = skb_mac_header(skb) - skb->data;
93 case NFT_PAYLOAD_NETWORK_HEADER:
94 offset = skb_network_offset(skb);
96 case NFT_PAYLOAD_TRANSPORT_HEADER:
99 offset = pkt->xt.thoff;
104 offset += priv->offset;
106 if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
110 regs->verdict.code = NFT_BREAK;
113 static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
114 [NFTA_PAYLOAD_SREG] = { .type = NLA_U32 },
115 [NFTA_PAYLOAD_DREG] = { .type = NLA_U32 },
116 [NFTA_PAYLOAD_BASE] = { .type = NLA_U32 },
117 [NFTA_PAYLOAD_OFFSET] = { .type = NLA_U32 },
118 [NFTA_PAYLOAD_LEN] = { .type = NLA_U32 },
119 [NFTA_PAYLOAD_CSUM_TYPE] = { .type = NLA_U32 },
120 [NFTA_PAYLOAD_CSUM_OFFSET] = { .type = NLA_U32 },
123 static int nft_payload_init(const struct nft_ctx *ctx,
124 const struct nft_expr *expr,
125 const struct nlattr * const tb[])
127 struct nft_payload *priv = nft_expr_priv(expr);
129 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
130 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
131 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
132 priv->dreg = nft_parse_register(tb[NFTA_PAYLOAD_DREG]);
134 return nft_validate_register_store(ctx, priv->dreg, NULL,
135 NFT_DATA_VALUE, priv->len);
138 static int nft_payload_dump(struct sk_buff *skb, const struct nft_expr *expr)
140 const struct nft_payload *priv = nft_expr_priv(expr);
142 if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) ||
143 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
144 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
145 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
146 goto nla_put_failure;
153 static struct nft_expr_type nft_payload_type;
154 static const struct nft_expr_ops nft_payload_ops = {
155 .type = &nft_payload_type,
156 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
157 .eval = nft_payload_eval,
158 .init = nft_payload_init,
159 .dump = nft_payload_dump,
162 const struct nft_expr_ops nft_payload_fast_ops = {
163 .type = &nft_payload_type,
164 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
165 .eval = nft_payload_eval,
166 .init = nft_payload_init,
167 .dump = nft_payload_dump,
170 static void nft_payload_set_eval(const struct nft_expr *expr,
171 struct nft_regs *regs,
172 const struct nft_pktinfo *pkt)
174 const struct nft_payload_set *priv = nft_expr_priv(expr);
175 struct sk_buff *skb = pkt->skb;
176 const u32 *src = ®s->data[priv->sreg];
177 int offset, csum_offset;
181 switch (priv->base) {
182 case NFT_PAYLOAD_LL_HEADER:
183 if (!skb_mac_header_was_set(skb))
185 offset = skb_mac_header(skb) - skb->data;
187 case NFT_PAYLOAD_NETWORK_HEADER:
188 offset = skb_network_offset(skb);
190 case NFT_PAYLOAD_TRANSPORT_HEADER:
193 offset = pkt->xt.thoff;
199 csum_offset = offset + priv->csum_offset;
200 offset += priv->offset;
202 if (priv->csum_type == NFT_PAYLOAD_CSUM_INET &&
203 (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER ||
204 skb->ip_summed != CHECKSUM_PARTIAL)) {
205 if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
208 fsum = skb_checksum(skb, offset, priv->len, 0);
209 tsum = csum_partial(src, priv->len, 0);
210 sum = csum_fold(csum_add(csum_sub(~csum_unfold(sum), fsum),
213 sum = CSUM_MANGLED_0;
215 if (!skb_make_writable(skb, csum_offset + sizeof(sum)) ||
216 skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
220 if (!skb_make_writable(skb, max(offset + priv->len, 0)) ||
221 skb_store_bits(skb, offset, src, priv->len) < 0)
226 regs->verdict.code = NFT_BREAK;
229 static int nft_payload_set_init(const struct nft_ctx *ctx,
230 const struct nft_expr *expr,
231 const struct nlattr * const tb[])
233 struct nft_payload_set *priv = nft_expr_priv(expr);
235 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
236 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
237 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
238 priv->sreg = nft_parse_register(tb[NFTA_PAYLOAD_SREG]);
240 if (tb[NFTA_PAYLOAD_CSUM_TYPE])
242 ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE]));
243 if (tb[NFTA_PAYLOAD_CSUM_OFFSET])
245 ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_OFFSET]));
247 switch (priv->csum_type) {
248 case NFT_PAYLOAD_CSUM_NONE:
249 case NFT_PAYLOAD_CSUM_INET:
255 return nft_validate_register_load(priv->sreg, priv->len);
258 static int nft_payload_set_dump(struct sk_buff *skb, const struct nft_expr *expr)
260 const struct nft_payload_set *priv = nft_expr_priv(expr);
262 if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) ||
263 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
264 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
265 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) ||
266 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) ||
267 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET,
268 htonl(priv->csum_offset)))
269 goto nla_put_failure;
276 static const struct nft_expr_ops nft_payload_set_ops = {
277 .type = &nft_payload_type,
278 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)),
279 .eval = nft_payload_set_eval,
280 .init = nft_payload_set_init,
281 .dump = nft_payload_set_dump,
284 static const struct nft_expr_ops *
285 nft_payload_select_ops(const struct nft_ctx *ctx,
286 const struct nlattr * const tb[])
288 enum nft_payload_bases base;
289 unsigned int offset, len;
291 if (tb[NFTA_PAYLOAD_BASE] == NULL ||
292 tb[NFTA_PAYLOAD_OFFSET] == NULL ||
293 tb[NFTA_PAYLOAD_LEN] == NULL)
294 return ERR_PTR(-EINVAL);
296 base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
298 case NFT_PAYLOAD_LL_HEADER:
299 case NFT_PAYLOAD_NETWORK_HEADER:
300 case NFT_PAYLOAD_TRANSPORT_HEADER:
303 return ERR_PTR(-EOPNOTSUPP);
306 if (tb[NFTA_PAYLOAD_SREG] != NULL) {
307 if (tb[NFTA_PAYLOAD_DREG] != NULL)
308 return ERR_PTR(-EINVAL);
309 return &nft_payload_set_ops;
312 if (tb[NFTA_PAYLOAD_DREG] == NULL)
313 return ERR_PTR(-EINVAL);
315 offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
316 len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
318 if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
319 base != NFT_PAYLOAD_LL_HEADER)
320 return &nft_payload_fast_ops;
322 return &nft_payload_ops;
325 static struct nft_expr_type nft_payload_type __read_mostly = {
327 .select_ops = nft_payload_select_ops,
328 .policy = nft_payload_policy,
329 .maxattr = NFTA_PAYLOAD_MAX,
330 .owner = THIS_MODULE,
333 int __init nft_payload_module_init(void)
335 return nft_register_expr(&nft_payload_type);
338 void nft_payload_module_exit(void)
340 nft_unregister_expr(&nft_payload_type);