1
2
3
4
5
6
7
8#include <asm/unaligned.h>
9#include <linux/kernel.h>
10#include <linux/init.h>
11#include <linux/module.h>
12#include <linux/netlink.h>
13#include <linux/netfilter.h>
14#include <linux/netfilter/nf_tables.h>
15#include <net/netfilter/nf_tables_core.h>
16#include <net/netfilter/nf_tables.h>
17
18struct nft_byteorder {
19 u8 sreg;
20 u8 dreg;
21 enum nft_byteorder_ops op:8;
22 u8 len;
23 u8 size;
24};
25
26void nft_byteorder_eval(const struct nft_expr *expr,
27 struct nft_regs *regs,
28 const struct nft_pktinfo *pkt)
29{
30 const struct nft_byteorder *priv = nft_expr_priv(expr);
31 u32 *src = ®s->data[priv->sreg];
32 u32 *dst = ®s->data[priv->dreg];
33 union { u32 u32; u16 u16; } *s, *d;
34 unsigned int i;
35
36 s = (void *)src;
37 d = (void *)dst;
38
39 switch (priv->size) {
40 case 8: {
41 u64 src64;
42
43 switch (priv->op) {
44 case NFT_BYTEORDER_NTOH:
45 for (i = 0; i < priv->len / 8; i++) {
46 src64 = nft_reg_load64(&src[i]);
47 nft_reg_store64(&dst[i], be64_to_cpu(src64));
48 }
49 break;
50 case NFT_BYTEORDER_HTON:
51 for (i = 0; i < priv->len / 8; i++) {
52 src64 = (__force __u64)
53 cpu_to_be64(nft_reg_load64(&src[i]));
54 nft_reg_store64(&dst[i], src64);
55 }
56 break;
57 }
58 break;
59 }
60 case 4:
61 switch (priv->op) {
62 case NFT_BYTEORDER_NTOH:
63 for (i = 0; i < priv->len / 4; i++)
64 d[i].u32 = ntohl((__force __be32)s[i].u32);
65 break;
66 case NFT_BYTEORDER_HTON:
67 for (i = 0; i < priv->len / 4; i++)
68 d[i].u32 = (__force __u32)htonl(s[i].u32);
69 break;
70 }
71 break;
72 case 2:
73 switch (priv->op) {
74 case NFT_BYTEORDER_NTOH:
75 for (i = 0; i < priv->len / 2; i++)
76 d[i].u16 = ntohs((__force __be16)s[i].u16);
77 break;
78 case NFT_BYTEORDER_HTON:
79 for (i = 0; i < priv->len / 2; i++)
80 d[i].u16 = (__force __u16)htons(s[i].u16);
81 break;
82 }
83 break;
84 }
85}
86
87static const struct nla_policy nft_byteorder_policy[NFTA_BYTEORDER_MAX + 1] = {
88 [NFTA_BYTEORDER_SREG] = { .type = NLA_U32 },
89 [NFTA_BYTEORDER_DREG] = { .type = NLA_U32 },
90 [NFTA_BYTEORDER_OP] = { .type = NLA_U32 },
91 [NFTA_BYTEORDER_LEN] = { .type = NLA_U32 },
92 [NFTA_BYTEORDER_SIZE] = { .type = NLA_U32 },
93};
94
95static int nft_byteorder_init(const struct nft_ctx *ctx,
96 const struct nft_expr *expr,
97 const struct nlattr * const tb[])
98{
99 struct nft_byteorder *priv = nft_expr_priv(expr);
100 u32 size, len;
101 int err;
102
103 if (tb[NFTA_BYTEORDER_SREG] == NULL ||
104 tb[NFTA_BYTEORDER_DREG] == NULL ||
105 tb[NFTA_BYTEORDER_LEN] == NULL ||
106 tb[NFTA_BYTEORDER_SIZE] == NULL ||
107 tb[NFTA_BYTEORDER_OP] == NULL)
108 return -EINVAL;
109
110 priv->op = ntohl(nla_get_be32(tb[NFTA_BYTEORDER_OP]));
111 switch (priv->op) {
112 case NFT_BYTEORDER_NTOH:
113 case NFT_BYTEORDER_HTON:
114 break;
115 default:
116 return -EINVAL;
117 }
118
119 err = nft_parse_u32_check(tb[NFTA_BYTEORDER_SIZE], U8_MAX, &size);
120 if (err < 0)
121 return err;
122
123 priv->size = size;
124
125 switch (priv->size) {
126 case 2:
127 case 4:
128 case 8:
129 break;
130 default:
131 return -EINVAL;
132 }
133
134 err = nft_parse_u32_check(tb[NFTA_BYTEORDER_LEN], U8_MAX, &len);
135 if (err < 0)
136 return err;
137
138 priv->len = len;
139
140 err = nft_parse_register_load(tb[NFTA_BYTEORDER_SREG], &priv->sreg,
141 priv->len);
142 if (err < 0)
143 return err;
144
145 return nft_parse_register_store(ctx, tb[NFTA_BYTEORDER_DREG],
146 &priv->dreg, NULL, NFT_DATA_VALUE,
147 priv->len);
148}
149
150static int nft_byteorder_dump(struct sk_buff *skb, const struct nft_expr *expr)
151{
152 const struct nft_byteorder *priv = nft_expr_priv(expr);
153
154 if (nft_dump_register(skb, NFTA_BYTEORDER_SREG, priv->sreg))
155 goto nla_put_failure;
156 if (nft_dump_register(skb, NFTA_BYTEORDER_DREG, priv->dreg))
157 goto nla_put_failure;
158 if (nla_put_be32(skb, NFTA_BYTEORDER_OP, htonl(priv->op)))
159 goto nla_put_failure;
160 if (nla_put_be32(skb, NFTA_BYTEORDER_LEN, htonl(priv->len)))
161 goto nla_put_failure;
162 if (nla_put_be32(skb, NFTA_BYTEORDER_SIZE, htonl(priv->size)))
163 goto nla_put_failure;
164 return 0;
165
166nla_put_failure:
167 return -1;
168}
169
170static const struct nft_expr_ops nft_byteorder_ops = {
171 .type = &nft_byteorder_type,
172 .size = NFT_EXPR_SIZE(sizeof(struct nft_byteorder)),
173 .eval = nft_byteorder_eval,
174 .init = nft_byteorder_init,
175 .dump = nft_byteorder_dump,
176};
177
178struct nft_expr_type nft_byteorder_type __read_mostly = {
179 .name = "byteorder",
180 .ops = &nft_byteorder_ops,
181 .policy = nft_byteorder_policy,
182 .maxattr = NFTA_BYTEORDER_MAX,
183 .owner = THIS_MODULE,
184};
185
