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/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> 20 21 /* add vlan header into the user buffer for if tag was removed by offloads */ 22 static bool 23 nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u8 offset, u8 len) 24 { 25 int mac_off = skb_mac_header(skb) - skb->data; 26 u8 vlan_len, *vlanh, *dst_u8 = (u8 *) d; 27 struct vlan_ethhdr veth; 28 29 vlanh = (u8 *) &veth; 30 if (offset < ETH_HLEN) { 31 u8 ethlen = min_t(u8, len, ETH_HLEN - offset); 32 33 if (skb_copy_bits(skb, mac_off, &veth, ETH_HLEN)) 34 return false; 35 36 veth.h_vlan_proto = skb->vlan_proto; 37 38 memcpy(dst_u8, vlanh + offset, ethlen); 39 40 len -= ethlen; 41 if (len == 0) 42 return true; 43 44 dst_u8 += ethlen; 45 offset = ETH_HLEN; 46 } else if (offset >= VLAN_ETH_HLEN) { 47 offset -= VLAN_HLEN; 48 goto skip; 49 } 50 51 veth.h_vlan_TCI = htons(skb_vlan_tag_get(skb)); 52 veth.h_vlan_encapsulated_proto = skb->protocol; 53 54 vlanh += offset; 55 56 vlan_len = min_t(u8, len, VLAN_ETH_HLEN - offset); 57 memcpy(dst_u8, vlanh, vlan_len); 58 59 len -= vlan_len; 60 if (!len) 61 return true; 62 63 dst_u8 += vlan_len; 64 skip: 65 return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0; 66 } 67 68 static void nft_payload_eval(const struct nft_expr *expr, 69 struct nft_regs *regs, 70 const struct nft_pktinfo *pkt) 71 { 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]; 75 int offset; 76 77 dest[priv->len / NFT_REG32_SIZE] = 0; 78 switch (priv->base) { 79 case NFT_PAYLOAD_LL_HEADER: 80 if (!skb_mac_header_was_set(skb)) 81 goto err; 82 83 if (skb_vlan_tag_present(skb)) { 84 if (!nft_payload_copy_vlan(dest, skb, 85 priv->offset, priv->len)) 86 goto err; 87 return; 88 } 89 offset = skb_mac_header(skb) - skb->data; 90 break; 91 case NFT_PAYLOAD_NETWORK_HEADER: 92 offset = skb_network_offset(skb); 93 break; 94 case NFT_PAYLOAD_TRANSPORT_HEADER: 95 offset = pkt->xt.thoff; 96 break; 97 default: 98 BUG(); 99 } 100 offset += priv->offset; 101 102 if (skb_copy_bits(skb, offset, dest, priv->len) < 0) 103 goto err; 104 return; 105 err: 106 regs->verdict.code = NFT_BREAK; 107 } 108 109 static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = { 110 [NFTA_PAYLOAD_SREG] = { .type = NLA_U32 }, 111 [NFTA_PAYLOAD_DREG] = { .type = NLA_U32 }, 112 [NFTA_PAYLOAD_BASE] = { .type = NLA_U32 }, 113 [NFTA_PAYLOAD_OFFSET] = { .type = NLA_U32 }, 114 [NFTA_PAYLOAD_LEN] = { .type = NLA_U32 }, 115 [NFTA_PAYLOAD_CSUM_TYPE] = { .type = NLA_U32 }, 116 [NFTA_PAYLOAD_CSUM_OFFSET] = { .type = NLA_U32 }, 117 }; 118 119 static int nft_payload_init(const struct nft_ctx *ctx, 120 const struct nft_expr *expr, 121 const struct nlattr * const tb[]) 122 { 123 struct nft_payload *priv = nft_expr_priv(expr); 124 125 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE])); 126 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET])); 127 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN])); 128 priv->dreg = nft_parse_register(tb[NFTA_PAYLOAD_DREG]); 129 130 return nft_validate_register_store(ctx, priv->dreg, NULL, 131 NFT_DATA_VALUE, priv->len); 132 } 133 134 static int nft_payload_dump(struct sk_buff *skb, const struct nft_expr *expr) 135 { 136 const struct nft_payload *priv = nft_expr_priv(expr); 137 138 if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) || 139 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) || 140 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) || 141 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len))) 142 goto nla_put_failure; 143 return 0; 144 145 nla_put_failure: 146 return -1; 147 } 148 149 static struct nft_expr_type nft_payload_type; 150 static const struct nft_expr_ops nft_payload_ops = { 151 .type = &nft_payload_type, 152 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)), 153 .eval = nft_payload_eval, 154 .init = nft_payload_init, 155 .dump = nft_payload_dump, 156 }; 157 158 const struct nft_expr_ops nft_payload_fast_ops = { 159 .type = &nft_payload_type, 160 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)), 161 .eval = nft_payload_eval, 162 .init = nft_payload_init, 163 .dump = nft_payload_dump, 164 }; 165 166 static void nft_payload_set_eval(const struct nft_expr *expr, 167 struct nft_regs *regs, 168 const struct nft_pktinfo *pkt) 169 { 170 const struct nft_payload_set *priv = nft_expr_priv(expr); 171 struct sk_buff *skb = pkt->skb; 172 const u32 *src = ®s->data[priv->sreg]; 173 int offset, csum_offset; 174 __wsum fsum, tsum; 175 __sum16 sum; 176 177 switch (priv->base) { 178 case NFT_PAYLOAD_LL_HEADER: 179 if (!skb_mac_header_was_set(skb)) 180 goto err; 181 offset = skb_mac_header(skb) - skb->data; 182 break; 183 case NFT_PAYLOAD_NETWORK_HEADER: 184 offset = skb_network_offset(skb); 185 break; 186 case NFT_PAYLOAD_TRANSPORT_HEADER: 187 offset = pkt->xt.thoff; 188 break; 189 default: 190 BUG(); 191 } 192 193 csum_offset = offset + priv->csum_offset; 194 offset += priv->offset; 195 196 if (priv->csum_type == NFT_PAYLOAD_CSUM_INET && 197 (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER || 198 skb->ip_summed != CHECKSUM_PARTIAL)) { 199 if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0) 200 goto err; 201 202 fsum = skb_checksum(skb, offset, priv->len, 0); 203 tsum = csum_partial(src, priv->len, 0); 204 sum = csum_fold(csum_add(csum_sub(~csum_unfold(sum), fsum), 205 tsum)); 206 if (sum == 0) 207 sum = CSUM_MANGLED_0; 208 209 if (!skb_make_writable(skb, csum_offset + sizeof(sum)) || 210 skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0) 211 goto err; 212 } 213 214 if (!skb_make_writable(skb, max(offset + priv->len, 0)) || 215 skb_store_bits(skb, offset, src, priv->len) < 0) 216 goto err; 217 218 return; 219 err: 220 regs->verdict.code = NFT_BREAK; 221 } 222 223 static int nft_payload_set_init(const struct nft_ctx *ctx, 224 const struct nft_expr *expr, 225 const struct nlattr * const tb[]) 226 { 227 struct nft_payload_set *priv = nft_expr_priv(expr); 228 229 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE])); 230 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET])); 231 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN])); 232 priv->sreg = nft_parse_register(tb[NFTA_PAYLOAD_SREG]); 233 234 if (tb[NFTA_PAYLOAD_CSUM_TYPE]) 235 priv->csum_type = 236 ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE])); 237 if (tb[NFTA_PAYLOAD_CSUM_OFFSET]) 238 priv->csum_offset = 239 ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_OFFSET])); 240 241 switch (priv->csum_type) { 242 case NFT_PAYLOAD_CSUM_NONE: 243 case NFT_PAYLOAD_CSUM_INET: 244 break; 245 default: 246 return -EOPNOTSUPP; 247 } 248 249 return nft_validate_register_load(priv->sreg, priv->len); 250 } 251 252 static int nft_payload_set_dump(struct sk_buff *skb, const struct nft_expr *expr) 253 { 254 const struct nft_payload_set *priv = nft_expr_priv(expr); 255 256 if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) || 257 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) || 258 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) || 259 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) || 260 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) || 261 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET, 262 htonl(priv->csum_offset))) 263 goto nla_put_failure; 264 return 0; 265 266 nla_put_failure: 267 return -1; 268 } 269 270 static const struct nft_expr_ops nft_payload_set_ops = { 271 .type = &nft_payload_type, 272 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)), 273 .eval = nft_payload_set_eval, 274 .init = nft_payload_set_init, 275 .dump = nft_payload_set_dump, 276 }; 277 278 static const struct nft_expr_ops * 279 nft_payload_select_ops(const struct nft_ctx *ctx, 280 const struct nlattr * const tb[]) 281 { 282 enum nft_payload_bases base; 283 unsigned int offset, len; 284 285 if (tb[NFTA_PAYLOAD_BASE] == NULL || 286 tb[NFTA_PAYLOAD_OFFSET] == NULL || 287 tb[NFTA_PAYLOAD_LEN] == NULL) 288 return ERR_PTR(-EINVAL); 289 290 base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE])); 291 switch (base) { 292 case NFT_PAYLOAD_LL_HEADER: 293 case NFT_PAYLOAD_NETWORK_HEADER: 294 case NFT_PAYLOAD_TRANSPORT_HEADER: 295 break; 296 default: 297 return ERR_PTR(-EOPNOTSUPP); 298 } 299 300 if (tb[NFTA_PAYLOAD_SREG] != NULL) { 301 if (tb[NFTA_PAYLOAD_DREG] != NULL) 302 return ERR_PTR(-EINVAL); 303 return &nft_payload_set_ops; 304 } 305 306 if (tb[NFTA_PAYLOAD_DREG] == NULL) 307 return ERR_PTR(-EINVAL); 308 309 offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET])); 310 len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN])); 311 312 if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) && 313 base != NFT_PAYLOAD_LL_HEADER) 314 return &nft_payload_fast_ops; 315 else 316 return &nft_payload_ops; 317 } 318 319 static struct nft_expr_type nft_payload_type __read_mostly = { 320 .name = "payload", 321 .select_ops = nft_payload_select_ops, 322 .policy = nft_payload_policy, 323 .maxattr = NFTA_PAYLOAD_MAX, 324 .owner = THIS_MODULE, 325 }; 326 327 int __init nft_payload_module_init(void) 328 { 329 return nft_register_expr(&nft_payload_type); 330 } 331 332 void nft_payload_module_exit(void) 333 { 334 nft_unregister_expr(&nft_payload_type); 335 } 336