1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * net/sched/act_skbmod.c skb data modifier
4 *
5 * Copyright (c) 2016 Jamal Hadi Salim <jhs@mojatatu.com>
6 */
7
8 #include <linux/module.h>
9 #include <linux/if_arp.h>
10 #include <linux/init.h>
11 #include <linux/kernel.h>
12 #include <linux/skbuff.h>
13 #include <linux/rtnetlink.h>
14 #include <net/inet_ecn.h>
15 #include <net/netlink.h>
16 #include <net/pkt_sched.h>
17 #include <net/pkt_cls.h>
18
19 #include <linux/tc_act/tc_skbmod.h>
20 #include <net/tc_act/tc_skbmod.h>
21
22 static struct tc_action_ops act_skbmod_ops;
23
tcf_skbmod_act(struct sk_buff * skb,const struct tc_action * a,struct tcf_result * res)24 static int tcf_skbmod_act(struct sk_buff *skb, const struct tc_action *a,
25 struct tcf_result *res)
26 {
27 struct tcf_skbmod *d = to_skbmod(a);
28 int action, max_edit_len, err;
29 struct tcf_skbmod_params *p;
30 u64 flags;
31
32 tcf_lastuse_update(&d->tcf_tm);
33 bstats_update(this_cpu_ptr(d->common.cpu_bstats), skb);
34
35 action = READ_ONCE(d->tcf_action);
36 if (unlikely(action == TC_ACT_SHOT))
37 goto drop;
38
39 max_edit_len = skb_mac_header_len(skb);
40 p = rcu_dereference_bh(d->skbmod_p);
41 flags = p->flags;
42
43 /* tcf_skbmod_init() guarantees "flags" to be one of the following:
44 * 1. a combination of SKBMOD_F_{DMAC,SMAC,ETYPE}
45 * 2. SKBMOD_F_SWAPMAC
46 * 3. SKBMOD_F_ECN
47 * SKBMOD_F_ECN only works with IP packets; all other flags only work with Ethernet
48 * packets.
49 */
50 if (flags == SKBMOD_F_ECN) {
51 switch (skb_protocol(skb, true)) {
52 case cpu_to_be16(ETH_P_IP):
53 case cpu_to_be16(ETH_P_IPV6):
54 max_edit_len += skb_network_header_len(skb);
55 break;
56 default:
57 goto out;
58 }
59 } else if (!skb->dev || skb->dev->type != ARPHRD_ETHER) {
60 goto out;
61 }
62
63 err = skb_ensure_writable(skb, max_edit_len);
64 if (unlikely(err)) /* best policy is to drop on the floor */
65 goto drop;
66
67 if (flags & SKBMOD_F_DMAC)
68 ether_addr_copy(eth_hdr(skb)->h_dest, p->eth_dst);
69 if (flags & SKBMOD_F_SMAC)
70 ether_addr_copy(eth_hdr(skb)->h_source, p->eth_src);
71 if (flags & SKBMOD_F_ETYPE)
72 eth_hdr(skb)->h_proto = p->eth_type;
73
74 if (flags & SKBMOD_F_SWAPMAC) {
75 u16 tmpaddr[ETH_ALEN / 2]; /* ether_addr_copy() requirement */
76 /*XXX: I am sure we can come up with more efficient swapping*/
77 ether_addr_copy((u8 *)tmpaddr, eth_hdr(skb)->h_dest);
78 ether_addr_copy(eth_hdr(skb)->h_dest, eth_hdr(skb)->h_source);
79 ether_addr_copy(eth_hdr(skb)->h_source, (u8 *)tmpaddr);
80 }
81
82 if (flags & SKBMOD_F_ECN)
83 INET_ECN_set_ce(skb);
84
85 out:
86 return action;
87
88 drop:
89 qstats_overlimit_inc(this_cpu_ptr(d->common.cpu_qstats));
90 return TC_ACT_SHOT;
91 }
92
93 static const struct nla_policy skbmod_policy[TCA_SKBMOD_MAX + 1] = {
94 [TCA_SKBMOD_PARMS] = { .len = sizeof(struct tc_skbmod) },
95 [TCA_SKBMOD_DMAC] = { .len = ETH_ALEN },
96 [TCA_SKBMOD_SMAC] = { .len = ETH_ALEN },
97 [TCA_SKBMOD_ETYPE] = { .type = NLA_U16 },
98 };
99
tcf_skbmod_init(struct net * net,struct nlattr * nla,struct nlattr * est,struct tc_action ** a,struct tcf_proto * tp,u32 flags,struct netlink_ext_ack * extack)100 static int tcf_skbmod_init(struct net *net, struct nlattr *nla,
101 struct nlattr *est, struct tc_action **a,
102 struct tcf_proto *tp, u32 flags,
103 struct netlink_ext_ack *extack)
104 {
105 struct tc_action_net *tn = net_generic(net, act_skbmod_ops.net_id);
106 bool ovr = flags & TCA_ACT_FLAGS_REPLACE;
107 bool bind = flags & TCA_ACT_FLAGS_BIND;
108 struct nlattr *tb[TCA_SKBMOD_MAX + 1];
109 struct tcf_skbmod_params *p, *p_old;
110 struct tcf_chain *goto_ch = NULL;
111 struct tc_skbmod *parm;
112 u32 lflags = 0, index;
113 struct tcf_skbmod *d;
114 bool exists = false;
115 u8 *daddr = NULL;
116 u8 *saddr = NULL;
117 u16 eth_type = 0;
118 int ret = 0, err;
119
120 if (!nla)
121 return -EINVAL;
122
123 err = nla_parse_nested_deprecated(tb, TCA_SKBMOD_MAX, nla,
124 skbmod_policy, NULL);
125 if (err < 0)
126 return err;
127
128 if (!tb[TCA_SKBMOD_PARMS])
129 return -EINVAL;
130
131 if (tb[TCA_SKBMOD_DMAC]) {
132 daddr = nla_data(tb[TCA_SKBMOD_DMAC]);
133 lflags |= SKBMOD_F_DMAC;
134 }
135
136 if (tb[TCA_SKBMOD_SMAC]) {
137 saddr = nla_data(tb[TCA_SKBMOD_SMAC]);
138 lflags |= SKBMOD_F_SMAC;
139 }
140
141 if (tb[TCA_SKBMOD_ETYPE]) {
142 eth_type = nla_get_u16(tb[TCA_SKBMOD_ETYPE]);
143 lflags |= SKBMOD_F_ETYPE;
144 }
145
146 parm = nla_data(tb[TCA_SKBMOD_PARMS]);
147 index = parm->index;
148 if (parm->flags & SKBMOD_F_SWAPMAC)
149 lflags = SKBMOD_F_SWAPMAC;
150 if (parm->flags & SKBMOD_F_ECN)
151 lflags = SKBMOD_F_ECN;
152
153 err = tcf_idr_check_alloc(tn, &index, a, bind);
154 if (err < 0)
155 return err;
156 exists = err;
157 if (exists && bind)
158 return 0;
159
160 if (!lflags) {
161 if (exists)
162 tcf_idr_release(*a, bind);
163 else
164 tcf_idr_cleanup(tn, index);
165 return -EINVAL;
166 }
167
168 if (!exists) {
169 ret = tcf_idr_create(tn, index, est, a,
170 &act_skbmod_ops, bind, true, flags);
171 if (ret) {
172 tcf_idr_cleanup(tn, index);
173 return ret;
174 }
175
176 ret = ACT_P_CREATED;
177 } else if (!ovr) {
178 tcf_idr_release(*a, bind);
179 return -EEXIST;
180 }
181 err = tcf_action_check_ctrlact(parm->action, tp, &goto_ch, extack);
182 if (err < 0)
183 goto release_idr;
184
185 d = to_skbmod(*a);
186
187 p = kzalloc(sizeof(struct tcf_skbmod_params), GFP_KERNEL);
188 if (unlikely(!p)) {
189 err = -ENOMEM;
190 goto put_chain;
191 }
192
193 p->flags = lflags;
194
195 if (ovr)
196 spin_lock_bh(&d->tcf_lock);
197 /* Protected by tcf_lock if overwriting existing action. */
198 goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch);
199 p_old = rcu_dereference_protected(d->skbmod_p, 1);
200
201 if (lflags & SKBMOD_F_DMAC)
202 ether_addr_copy(p->eth_dst, daddr);
203 if (lflags & SKBMOD_F_SMAC)
204 ether_addr_copy(p->eth_src, saddr);
205 if (lflags & SKBMOD_F_ETYPE)
206 p->eth_type = htons(eth_type);
207
208 rcu_assign_pointer(d->skbmod_p, p);
209 if (ovr)
210 spin_unlock_bh(&d->tcf_lock);
211
212 if (p_old)
213 kfree_rcu(p_old, rcu);
214 if (goto_ch)
215 tcf_chain_put_by_act(goto_ch);
216
217 return ret;
218 put_chain:
219 if (goto_ch)
220 tcf_chain_put_by_act(goto_ch);
221 release_idr:
222 tcf_idr_release(*a, bind);
223 return err;
224 }
225
tcf_skbmod_cleanup(struct tc_action * a)226 static void tcf_skbmod_cleanup(struct tc_action *a)
227 {
228 struct tcf_skbmod *d = to_skbmod(a);
229 struct tcf_skbmod_params *p;
230
231 p = rcu_dereference_protected(d->skbmod_p, 1);
232 if (p)
233 kfree_rcu(p, rcu);
234 }
235
tcf_skbmod_dump(struct sk_buff * skb,struct tc_action * a,int bind,int ref)236 static int tcf_skbmod_dump(struct sk_buff *skb, struct tc_action *a,
237 int bind, int ref)
238 {
239 struct tcf_skbmod *d = to_skbmod(a);
240 unsigned char *b = skb_tail_pointer(skb);
241 struct tcf_skbmod_params *p;
242 struct tc_skbmod opt = {
243 .index = d->tcf_index,
244 .refcnt = refcount_read(&d->tcf_refcnt) - ref,
245 .bindcnt = atomic_read(&d->tcf_bindcnt) - bind,
246 };
247 struct tcf_t t;
248
249 spin_lock_bh(&d->tcf_lock);
250 opt.action = d->tcf_action;
251 p = rcu_dereference_protected(d->skbmod_p,
252 lockdep_is_held(&d->tcf_lock));
253 opt.flags = p->flags;
254 if (nla_put(skb, TCA_SKBMOD_PARMS, sizeof(opt), &opt))
255 goto nla_put_failure;
256 if ((p->flags & SKBMOD_F_DMAC) &&
257 nla_put(skb, TCA_SKBMOD_DMAC, ETH_ALEN, p->eth_dst))
258 goto nla_put_failure;
259 if ((p->flags & SKBMOD_F_SMAC) &&
260 nla_put(skb, TCA_SKBMOD_SMAC, ETH_ALEN, p->eth_src))
261 goto nla_put_failure;
262 if ((p->flags & SKBMOD_F_ETYPE) &&
263 nla_put_u16(skb, TCA_SKBMOD_ETYPE, ntohs(p->eth_type)))
264 goto nla_put_failure;
265
266 tcf_tm_dump(&t, &d->tcf_tm);
267 if (nla_put_64bit(skb, TCA_SKBMOD_TM, sizeof(t), &t, TCA_SKBMOD_PAD))
268 goto nla_put_failure;
269
270 spin_unlock_bh(&d->tcf_lock);
271 return skb->len;
272 nla_put_failure:
273 spin_unlock_bh(&d->tcf_lock);
274 nlmsg_trim(skb, b);
275 return -1;
276 }
277
278 static struct tc_action_ops act_skbmod_ops = {
279 .kind = "skbmod",
280 .id = TCA_ACT_SKBMOD,
281 .owner = THIS_MODULE,
282 .act = tcf_skbmod_act,
283 .dump = tcf_skbmod_dump,
284 .init = tcf_skbmod_init,
285 .cleanup = tcf_skbmod_cleanup,
286 .size = sizeof(struct tcf_skbmod),
287 };
288
skbmod_init_net(struct net * net)289 static __net_init int skbmod_init_net(struct net *net)
290 {
291 struct tc_action_net *tn = net_generic(net, act_skbmod_ops.net_id);
292
293 return tc_action_net_init(net, tn, &act_skbmod_ops);
294 }
295
skbmod_exit_net(struct list_head * net_list)296 static void __net_exit skbmod_exit_net(struct list_head *net_list)
297 {
298 tc_action_net_exit(net_list, act_skbmod_ops.net_id);
299 }
300
301 static struct pernet_operations skbmod_net_ops = {
302 .init = skbmod_init_net,
303 .exit_batch = skbmod_exit_net,
304 .id = &act_skbmod_ops.net_id,
305 .size = sizeof(struct tc_action_net),
306 };
307
308 MODULE_AUTHOR("Jamal Hadi Salim, <jhs@mojatatu.com>");
309 MODULE_DESCRIPTION("SKB data mod-ing");
310 MODULE_LICENSE("GPL");
311
skbmod_init_module(void)312 static int __init skbmod_init_module(void)
313 {
314 return tcf_register_action(&act_skbmod_ops, &skbmod_net_ops);
315 }
316
skbmod_cleanup_module(void)317 static void __exit skbmod_cleanup_module(void)
318 {
319 tcf_unregister_action(&act_skbmod_ops, &skbmod_net_ops);
320 }
321
322 module_init(skbmod_init_module);
323 module_exit(skbmod_cleanup_module);
324
