| /* |
| * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net> |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License version 2 as |
| * published by the Free Software Foundation. |
| * |
| * Development of this code funded by Astaro AG (http://www.astaro.com/) |
| */ |
| |
| #include <linux/kernel.h> |
| #include <linux/if_vlan.h> |
| #include <linux/init.h> |
| #include <linux/module.h> |
| #include <linux/netlink.h> |
| #include <linux/netfilter.h> |
| #include <linux/netfilter/nf_tables.h> |
| #include <net/netfilter/nf_tables_core.h> |
| #include <net/netfilter/nf_tables.h> |
| |
| /* add vlan header into the user buffer for if tag was removed by offloads */ |
| static bool |
| nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u8 offset, u8 len) |
| { |
| int mac_off = skb_mac_header(skb) - skb->data; |
| u8 vlan_len, *vlanh, *dst_u8 = (u8 *) d; |
| struct vlan_ethhdr veth; |
| |
| vlanh = (u8 *) &veth; |
| if (offset < ETH_HLEN) { |
| u8 ethlen = min_t(u8, len, ETH_HLEN - offset); |
| |
| if (skb_copy_bits(skb, mac_off, &veth, ETH_HLEN)) |
| return false; |
| |
| veth.h_vlan_proto = skb->vlan_proto; |
| |
| memcpy(dst_u8, vlanh + offset, ethlen); |
| |
| len -= ethlen; |
| if (len == 0) |
| return true; |
| |
| dst_u8 += ethlen; |
| offset = ETH_HLEN; |
| } else if (offset >= VLAN_ETH_HLEN) { |
| offset -= VLAN_HLEN; |
| goto skip; |
| } |
| |
| veth.h_vlan_TCI = htons(skb_vlan_tag_get(skb)); |
| veth.h_vlan_encapsulated_proto = skb->protocol; |
| |
| vlanh += offset; |
| |
| vlan_len = min_t(u8, len, VLAN_ETH_HLEN - offset); |
| memcpy(dst_u8, vlanh, vlan_len); |
| |
| len -= vlan_len; |
| if (!len) |
| return true; |
| |
| dst_u8 += vlan_len; |
| skip: |
| return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0; |
| } |
| |
| static void nft_payload_eval(const struct nft_expr *expr, |
| struct nft_regs *regs, |
| const struct nft_pktinfo *pkt) |
| { |
| const struct nft_payload *priv = nft_expr_priv(expr); |
| const struct sk_buff *skb = pkt->skb; |
| u32 *dest = ®s->data[priv->dreg]; |
| int offset; |
| |
| dest[priv->len / NFT_REG32_SIZE] = 0; |
| switch (priv->base) { |
| case NFT_PAYLOAD_LL_HEADER: |
| if (!skb_mac_header_was_set(skb)) |
| goto err; |
| |
| if (skb_vlan_tag_present(skb)) { |
| if (!nft_payload_copy_vlan(dest, skb, |
| priv->offset, priv->len)) |
| goto err; |
| return; |
| } |
| offset = skb_mac_header(skb) - skb->data; |
| break; |
| case NFT_PAYLOAD_NETWORK_HEADER: |
| offset = skb_network_offset(skb); |
| break; |
| case NFT_PAYLOAD_TRANSPORT_HEADER: |
| offset = pkt->xt.thoff; |
| break; |
| default: |
| BUG(); |
| } |
| offset += priv->offset; |
| |
| if (skb_copy_bits(skb, offset, dest, priv->len) < 0) |
| goto err; |
| return; |
| err: |
| regs->verdict.code = NFT_BREAK; |
| } |
| |
| static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = { |
| [NFTA_PAYLOAD_SREG] = { .type = NLA_U32 }, |
| [NFTA_PAYLOAD_DREG] = { .type = NLA_U32 }, |
| [NFTA_PAYLOAD_BASE] = { .type = NLA_U32 }, |
| [NFTA_PAYLOAD_OFFSET] = { .type = NLA_U32 }, |
| [NFTA_PAYLOAD_LEN] = { .type = NLA_U32 }, |
| [NFTA_PAYLOAD_CSUM_TYPE] = { .type = NLA_U32 }, |
| [NFTA_PAYLOAD_CSUM_OFFSET] = { .type = NLA_U32 }, |
| }; |
| |
| static int nft_payload_init(const struct nft_ctx *ctx, |
| const struct nft_expr *expr, |
| const struct nlattr * const tb[]) |
| { |
| struct nft_payload *priv = nft_expr_priv(expr); |
| |
| priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE])); |
| priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET])); |
| priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN])); |
| priv->dreg = nft_parse_register(tb[NFTA_PAYLOAD_DREG]); |
| |
| return nft_validate_register_store(ctx, priv->dreg, NULL, |
| NFT_DATA_VALUE, priv->len); |
| } |
| |
| static int nft_payload_dump(struct sk_buff *skb, const struct nft_expr *expr) |
| { |
| const struct nft_payload *priv = nft_expr_priv(expr); |
| |
| if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) || |
| nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) || |
| nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) || |
| nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len))) |
| goto nla_put_failure; |
| return 0; |
| |
| nla_put_failure: |
| return -1; |
| } |
| |
| static struct nft_expr_type nft_payload_type; |
| static const struct nft_expr_ops nft_payload_ops = { |
| .type = &nft_payload_type, |
| .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)), |
| .eval = nft_payload_eval, |
| .init = nft_payload_init, |
| .dump = nft_payload_dump, |
| }; |
| |
| const struct nft_expr_ops nft_payload_fast_ops = { |
| .type = &nft_payload_type, |
| .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)), |
| .eval = nft_payload_eval, |
| .init = nft_payload_init, |
| .dump = nft_payload_dump, |
| }; |
| |
| static void nft_payload_set_eval(const struct nft_expr *expr, |
| struct nft_regs *regs, |
| const struct nft_pktinfo *pkt) |
| { |
| const struct nft_payload_set *priv = nft_expr_priv(expr); |
| struct sk_buff *skb = pkt->skb; |
| const u32 *src = ®s->data[priv->sreg]; |
| int offset, csum_offset; |
| __wsum fsum, tsum; |
| __sum16 sum; |
| |
| switch (priv->base) { |
| case NFT_PAYLOAD_LL_HEADER: |
| if (!skb_mac_header_was_set(skb)) |
| goto err; |
| offset = skb_mac_header(skb) - skb->data; |
| break; |
| case NFT_PAYLOAD_NETWORK_HEADER: |
| offset = skb_network_offset(skb); |
| break; |
| case NFT_PAYLOAD_TRANSPORT_HEADER: |
| offset = pkt->xt.thoff; |
| break; |
| default: |
| BUG(); |
| } |
| |
| csum_offset = offset + priv->csum_offset; |
| offset += priv->offset; |
| |
| if (priv->csum_type == NFT_PAYLOAD_CSUM_INET && |
| (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER || |
| skb->ip_summed != CHECKSUM_PARTIAL)) { |
| if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0) |
| goto err; |
| |
| fsum = skb_checksum(skb, offset, priv->len, 0); |
| tsum = csum_partial(src, priv->len, 0); |
| sum = csum_fold(csum_add(csum_sub(~csum_unfold(sum), fsum), |
| tsum)); |
| if (sum == 0) |
| sum = CSUM_MANGLED_0; |
| |
| if (!skb_make_writable(skb, csum_offset + sizeof(sum)) || |
| skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0) |
| goto err; |
| } |
| |
| if (!skb_make_writable(skb, max(offset + priv->len, 0)) || |
| skb_store_bits(skb, offset, src, priv->len) < 0) |
| goto err; |
| |
| return; |
| err: |
| regs->verdict.code = NFT_BREAK; |
| } |
| |
| static int nft_payload_set_init(const struct nft_ctx *ctx, |
| const struct nft_expr *expr, |
| const struct nlattr * const tb[]) |
| { |
| struct nft_payload_set *priv = nft_expr_priv(expr); |
| |
| priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE])); |
| priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET])); |
| priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN])); |
| priv->sreg = nft_parse_register(tb[NFTA_PAYLOAD_SREG]); |
| |
| if (tb[NFTA_PAYLOAD_CSUM_TYPE]) |
| priv->csum_type = |
| ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE])); |
| if (tb[NFTA_PAYLOAD_CSUM_OFFSET]) |
| priv->csum_offset = |
| ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_OFFSET])); |
| |
| switch (priv->csum_type) { |
| case NFT_PAYLOAD_CSUM_NONE: |
| case NFT_PAYLOAD_CSUM_INET: |
| break; |
| default: |
| return -EOPNOTSUPP; |
| } |
| |
| return nft_validate_register_load(priv->sreg, priv->len); |
| } |
| |
| static int nft_payload_set_dump(struct sk_buff *skb, const struct nft_expr *expr) |
| { |
| const struct nft_payload_set *priv = nft_expr_priv(expr); |
| |
| if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) || |
| nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) || |
| nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) || |
| nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) || |
| nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) || |
| nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET, |
| htonl(priv->csum_offset))) |
| goto nla_put_failure; |
| return 0; |
| |
| nla_put_failure: |
| return -1; |
| } |
| |
| static const struct nft_expr_ops nft_payload_set_ops = { |
| .type = &nft_payload_type, |
| .size = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)), |
| .eval = nft_payload_set_eval, |
| .init = nft_payload_set_init, |
| .dump = nft_payload_set_dump, |
| }; |
| |
| static const struct nft_expr_ops * |
| nft_payload_select_ops(const struct nft_ctx *ctx, |
| const struct nlattr * const tb[]) |
| { |
| enum nft_payload_bases base; |
| unsigned int offset, len; |
| |
| if (tb[NFTA_PAYLOAD_BASE] == NULL || |
| tb[NFTA_PAYLOAD_OFFSET] == NULL || |
| tb[NFTA_PAYLOAD_LEN] == NULL) |
| return ERR_PTR(-EINVAL); |
| |
| base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE])); |
| switch (base) { |
| case NFT_PAYLOAD_LL_HEADER: |
| case NFT_PAYLOAD_NETWORK_HEADER: |
| case NFT_PAYLOAD_TRANSPORT_HEADER: |
| break; |
| default: |
| return ERR_PTR(-EOPNOTSUPP); |
| } |
| |
| if (tb[NFTA_PAYLOAD_SREG] != NULL) { |
| if (tb[NFTA_PAYLOAD_DREG] != NULL) |
| return ERR_PTR(-EINVAL); |
| return &nft_payload_set_ops; |
| } |
| |
| if (tb[NFTA_PAYLOAD_DREG] == NULL) |
| return ERR_PTR(-EINVAL); |
| |
| offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET])); |
| len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN])); |
| |
| if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) && |
| base != NFT_PAYLOAD_LL_HEADER) |
| return &nft_payload_fast_ops; |
| else |
| return &nft_payload_ops; |
| } |
| |
| static struct nft_expr_type nft_payload_type __read_mostly = { |
| .name = "payload", |
| .select_ops = nft_payload_select_ops, |
| .policy = nft_payload_policy, |
| .maxattr = NFTA_PAYLOAD_MAX, |
| .owner = THIS_MODULE, |
| }; |
| |
| int __init nft_payload_module_init(void) |
| { |
| return nft_register_expr(&nft_payload_type); |
| } |
| |
| void nft_payload_module_exit(void) |
| { |
| nft_unregister_expr(&nft_payload_type); |
| } |