1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net> 4 * 5 * Development of this code funded by Astaro AG (http://www.astaro.com/) 6 */ 7 8 #include <linux/kernel.h> 9 #include <linux/init.h> 10 #include <linux/module.h> 11 #include <linux/netlink.h> 12 #include <linux/netfilter.h> 13 #include <linux/netfilter/nf_tables.h> 14 #include <net/netfilter/nf_tables_core.h> 15 #include <net/netfilter/nf_tables.h> 16 #include <net/netfilter/nf_tables_offload.h> 17 18 struct nft_bitwise { 19 enum nft_registers sreg:8; 20 enum nft_registers dreg:8; 21 enum nft_bitwise_ops op:8; 22 u8 len; 23 struct nft_data mask; 24 struct nft_data xor; 25 struct nft_data data; 26 }; 27 28 static void nft_bitwise_eval_bool(u32 *dst, const u32 *src, 29 const struct nft_bitwise *priv) 30 { 31 unsigned int i; 32 33 for (i = 0; i < DIV_ROUND_UP(priv->len, 4); i++) 34 dst[i] = (src[i] & priv->mask.data[i]) ^ priv->xor.data[i]; 35 } 36 37 static void nft_bitwise_eval_lshift(u32 *dst, const u32 *src, 38 const struct nft_bitwise *priv) 39 { 40 u32 shift = priv->data.data[0]; 41 unsigned int i; 42 u32 carry = 0; 43 44 for (i = DIV_ROUND_UP(priv->len, sizeof(u32)); i > 0; i--) { 45 dst[i - 1] = (src[i - 1] << shift) | carry; 46 carry = src[i - 1] >> (BITS_PER_TYPE(u32) - shift); 47 } 48 } 49 50 static void nft_bitwise_eval_rshift(u32 *dst, const u32 *src, 51 const struct nft_bitwise *priv) 52 { 53 u32 shift = priv->data.data[0]; 54 unsigned int i; 55 u32 carry = 0; 56 57 for (i = 0; i < DIV_ROUND_UP(priv->len, sizeof(u32)); i++) { 58 dst[i] = carry | (src[i] >> shift); 59 carry = src[i] << (BITS_PER_TYPE(u32) - shift); 60 } 61 } 62 63 void nft_bitwise_eval(const struct nft_expr *expr, 64 struct nft_regs *regs, const struct nft_pktinfo *pkt) 65 { 66 const struct nft_bitwise *priv = nft_expr_priv(expr); 67 const u32 *src = ®s->data[priv->sreg]; 68 u32 *dst = ®s->data[priv->dreg]; 69 70 switch (priv->op) { 71 case NFT_BITWISE_BOOL: 72 nft_bitwise_eval_bool(dst, src, priv); 73 break; 74 case NFT_BITWISE_LSHIFT: 75 nft_bitwise_eval_lshift(dst, src, priv); 76 break; 77 case NFT_BITWISE_RSHIFT: 78 nft_bitwise_eval_rshift(dst, src, priv); 79 break; 80 } 81 } 82 83 static const struct nla_policy nft_bitwise_policy[NFTA_BITWISE_MAX + 1] = { 84 [NFTA_BITWISE_SREG] = { .type = NLA_U32 }, 85 [NFTA_BITWISE_DREG] = { .type = NLA_U32 }, 86 [NFTA_BITWISE_LEN] = { .type = NLA_U32 }, 87 [NFTA_BITWISE_MASK] = { .type = NLA_NESTED }, 88 [NFTA_BITWISE_XOR] = { .type = NLA_NESTED }, 89 [NFTA_BITWISE_OP] = { .type = NLA_U32 }, 90 [NFTA_BITWISE_DATA] = { .type = NLA_NESTED }, 91 }; 92 93 static int nft_bitwise_init_bool(struct nft_bitwise *priv, 94 const struct nlattr *const tb[]) 95 { 96 struct nft_data_desc d1, d2; 97 int err; 98 99 if (tb[NFTA_BITWISE_DATA]) 100 return -EINVAL; 101 102 if (!tb[NFTA_BITWISE_MASK] || 103 !tb[NFTA_BITWISE_XOR]) 104 return -EINVAL; 105 106 err = nft_data_init(NULL, &priv->mask, sizeof(priv->mask), &d1, 107 tb[NFTA_BITWISE_MASK]); 108 if (err < 0) 109 return err; 110 if (d1.type != NFT_DATA_VALUE || d1.len != priv->len) { 111 err = -EINVAL; 112 goto err1; 113 } 114 115 err = nft_data_init(NULL, &priv->xor, sizeof(priv->xor), &d2, 116 tb[NFTA_BITWISE_XOR]); 117 if (err < 0) 118 goto err1; 119 if (d2.type != NFT_DATA_VALUE || d2.len != priv->len) { 120 err = -EINVAL; 121 goto err2; 122 } 123 124 return 0; 125 err2: 126 nft_data_release(&priv->xor, d2.type); 127 err1: 128 nft_data_release(&priv->mask, d1.type); 129 return err; 130 } 131 132 static int nft_bitwise_init_shift(struct nft_bitwise *priv, 133 const struct nlattr *const tb[]) 134 { 135 struct nft_data_desc d; 136 int err; 137 138 if (tb[NFTA_BITWISE_MASK] || 139 tb[NFTA_BITWISE_XOR]) 140 return -EINVAL; 141 142 if (!tb[NFTA_BITWISE_DATA]) 143 return -EINVAL; 144 145 err = nft_data_init(NULL, &priv->data, sizeof(priv->data), &d, 146 tb[NFTA_BITWISE_DATA]); 147 if (err < 0) 148 return err; 149 if (d.type != NFT_DATA_VALUE || d.len != sizeof(u32) || 150 priv->data.data[0] >= BITS_PER_TYPE(u32)) { 151 nft_data_release(&priv->data, d.type); 152 return -EINVAL; 153 } 154 155 return 0; 156 } 157 158 static int nft_bitwise_init(const struct nft_ctx *ctx, 159 const struct nft_expr *expr, 160 const struct nlattr * const tb[]) 161 { 162 struct nft_bitwise *priv = nft_expr_priv(expr); 163 u32 len; 164 int err; 165 166 if (!tb[NFTA_BITWISE_SREG] || 167 !tb[NFTA_BITWISE_DREG] || 168 !tb[NFTA_BITWISE_LEN]) 169 return -EINVAL; 170 171 err = nft_parse_u32_check(tb[NFTA_BITWISE_LEN], U8_MAX, &len); 172 if (err < 0) 173 return err; 174 175 priv->len = len; 176 177 priv->sreg = nft_parse_register(tb[NFTA_BITWISE_SREG]); 178 err = nft_validate_register_load(priv->sreg, priv->len); 179 if (err < 0) 180 return err; 181 182 priv->dreg = nft_parse_register(tb[NFTA_BITWISE_DREG]); 183 err = nft_validate_register_store(ctx, priv->dreg, NULL, 184 NFT_DATA_VALUE, priv->len); 185 if (err < 0) 186 return err; 187 188 if (tb[NFTA_BITWISE_OP]) { 189 priv->op = ntohl(nla_get_be32(tb[NFTA_BITWISE_OP])); 190 switch (priv->op) { 191 case NFT_BITWISE_BOOL: 192 case NFT_BITWISE_LSHIFT: 193 case NFT_BITWISE_RSHIFT: 194 break; 195 default: 196 return -EOPNOTSUPP; 197 } 198 } else { 199 priv->op = NFT_BITWISE_BOOL; 200 } 201 202 switch(priv->op) { 203 case NFT_BITWISE_BOOL: 204 err = nft_bitwise_init_bool(priv, tb); 205 break; 206 case NFT_BITWISE_LSHIFT: 207 case NFT_BITWISE_RSHIFT: 208 err = nft_bitwise_init_shift(priv, tb); 209 break; 210 } 211 212 return err; 213 } 214 215 static int nft_bitwise_dump_bool(struct sk_buff *skb, 216 const struct nft_bitwise *priv) 217 { 218 if (nft_data_dump(skb, NFTA_BITWISE_MASK, &priv->mask, 219 NFT_DATA_VALUE, priv->len) < 0) 220 return -1; 221 222 if (nft_data_dump(skb, NFTA_BITWISE_XOR, &priv->xor, 223 NFT_DATA_VALUE, priv->len) < 0) 224 return -1; 225 226 return 0; 227 } 228 229 static int nft_bitwise_dump_shift(struct sk_buff *skb, 230 const struct nft_bitwise *priv) 231 { 232 if (nft_data_dump(skb, NFTA_BITWISE_DATA, &priv->data, 233 NFT_DATA_VALUE, sizeof(u32)) < 0) 234 return -1; 235 return 0; 236 } 237 238 static int nft_bitwise_dump(struct sk_buff *skb, const struct nft_expr *expr) 239 { 240 const struct nft_bitwise *priv = nft_expr_priv(expr); 241 int err = 0; 242 243 if (nft_dump_register(skb, NFTA_BITWISE_SREG, priv->sreg)) 244 return -1; 245 if (nft_dump_register(skb, NFTA_BITWISE_DREG, priv->dreg)) 246 return -1; 247 if (nla_put_be32(skb, NFTA_BITWISE_LEN, htonl(priv->len))) 248 return -1; 249 if (nla_put_be32(skb, NFTA_BITWISE_OP, htonl(priv->op))) 250 return -1; 251 252 switch (priv->op) { 253 case NFT_BITWISE_BOOL: 254 err = nft_bitwise_dump_bool(skb, priv); 255 break; 256 case NFT_BITWISE_LSHIFT: 257 case NFT_BITWISE_RSHIFT: 258 err = nft_bitwise_dump_shift(skb, priv); 259 break; 260 } 261 262 return err; 263 } 264 265 static struct nft_data zero; 266 267 static int nft_bitwise_offload(struct nft_offload_ctx *ctx, 268 struct nft_flow_rule *flow, 269 const struct nft_expr *expr) 270 { 271 const struct nft_bitwise *priv = nft_expr_priv(expr); 272 struct nft_offload_reg *reg = &ctx->regs[priv->dreg]; 273 274 if (priv->op != NFT_BITWISE_BOOL) 275 return -EOPNOTSUPP; 276 277 if (memcmp(&priv->xor, &zero, sizeof(priv->xor)) || 278 priv->sreg != priv->dreg || priv->len != reg->len) 279 return -EOPNOTSUPP; 280 281 memcpy(®->mask, &priv->mask, sizeof(priv->mask)); 282 283 return 0; 284 } 285 286 static const struct nft_expr_ops nft_bitwise_ops = { 287 .type = &nft_bitwise_type, 288 .size = NFT_EXPR_SIZE(sizeof(struct nft_bitwise)), 289 .eval = nft_bitwise_eval, 290 .init = nft_bitwise_init, 291 .dump = nft_bitwise_dump, 292 .offload = nft_bitwise_offload, 293 }; 294 295 struct nft_expr_type nft_bitwise_type __read_mostly = { 296 .name = "bitwise", 297 .ops = &nft_bitwise_ops, 298 .policy = nft_bitwise_policy, 299 .maxattr = NFTA_BITWISE_MAX, 300 .owner = THIS_MODULE, 301 }; 302