1
2
3
4
5
6
7
8
9
10#include <linux/module.h>
11#include <linux/init.h>
12#include <linux/kernel.h>
13#include <linux/skbuff.h>
14#include <linux/rtnetlink.h>
15#include <linux/if_vlan.h>
16#include <net/netlink.h>
17#include <net/pkt_sched.h>
18
19#include <linux/tc_act/tc_vlan.h>
20#include <net/tc_act/tc_vlan.h>
21
22#define VLAN_TAB_MASK 15
23
24static int vlan_net_id;
25static struct tc_action_ops act_vlan_ops;
26
27static int tcf_vlan(struct sk_buff *skb, const struct tc_action *a,
28 struct tcf_result *res)
29{
30 struct tcf_vlan *v = to_vlan(a);
31 int action;
32 int err;
33 u16 tci;
34
35 spin_lock(&v->tcf_lock);
36 tcf_lastuse_update(&v->tcf_tm);
37 bstats_update(&v->tcf_bstats, skb);
38 action = v->tcf_action;
39
40
41
42
43 if (skb_at_tc_ingress(skb))
44 skb_push_rcsum(skb, skb->mac_len);
45
46 switch (v->tcfv_action) {
47 case TCA_VLAN_ACT_POP:
48 err = skb_vlan_pop(skb);
49 if (err)
50 goto drop;
51 break;
52 case TCA_VLAN_ACT_PUSH:
53 err = skb_vlan_push(skb, v->tcfv_push_proto, v->tcfv_push_vid |
54 (v->tcfv_push_prio << VLAN_PRIO_SHIFT));
55 if (err)
56 goto drop;
57 break;
58 case TCA_VLAN_ACT_MODIFY:
59
60 if (!skb_vlan_tagged(skb))
61 goto unlock;
62
63 if (skb_vlan_tag_present(skb)) {
64 tci = skb_vlan_tag_get(skb);
65 skb->vlan_tci = 0;
66 } else {
67
68 err = __skb_vlan_pop(skb, &tci);
69 if (err)
70 goto drop;
71 }
72
73 tci = (tci & ~VLAN_VID_MASK) | v->tcfv_push_vid;
74
75 if (v->tcfv_push_prio) {
76 tci &= ~VLAN_PRIO_MASK;
77 tci |= v->tcfv_push_prio << VLAN_PRIO_SHIFT;
78 }
79
80 __vlan_hwaccel_put_tag(skb, v->tcfv_push_proto, tci);
81 break;
82 default:
83 BUG();
84 }
85
86 goto unlock;
87
88drop:
89 action = TC_ACT_SHOT;
90 v->tcf_qstats.drops++;
91unlock:
92 if (skb_at_tc_ingress(skb))
93 skb_pull_rcsum(skb, skb->mac_len);
94
95 spin_unlock(&v->tcf_lock);
96 return action;
97}
98
99static const struct nla_policy vlan_policy[TCA_VLAN_MAX + 1] = {
100 [TCA_VLAN_PARMS] = { .len = sizeof(struct tc_vlan) },
101 [TCA_VLAN_PUSH_VLAN_ID] = { .type = NLA_U16 },
102 [TCA_VLAN_PUSH_VLAN_PROTOCOL] = { .type = NLA_U16 },
103 [TCA_VLAN_PUSH_VLAN_PRIORITY] = { .type = NLA_U8 },
104};
105
106static int tcf_vlan_init(struct net *net, struct nlattr *nla,
107 struct nlattr *est, struct tc_action **a,
108 int ovr, int bind)
109{
110 struct tc_action_net *tn = net_generic(net, vlan_net_id);
111 struct nlattr *tb[TCA_VLAN_MAX + 1];
112 struct tc_vlan *parm;
113 struct tcf_vlan *v;
114 int action;
115 __be16 push_vid = 0;
116 __be16 push_proto = 0;
117 u8 push_prio = 0;
118 bool exists = false;
119 int ret = 0, err;
120
121 if (!nla)
122 return -EINVAL;
123
124 err = nla_parse_nested(tb, TCA_VLAN_MAX, nla, vlan_policy);
125 if (err < 0)
126 return err;
127
128 if (!tb[TCA_VLAN_PARMS])
129 return -EINVAL;
130 parm = nla_data(tb[TCA_VLAN_PARMS]);
131 exists = tcf_hash_check(tn, parm->index, a, bind);
132 if (exists && bind)
133 return 0;
134
135 switch (parm->v_action) {
136 case TCA_VLAN_ACT_POP:
137 break;
138 case TCA_VLAN_ACT_PUSH:
139 case TCA_VLAN_ACT_MODIFY:
140 if (!tb[TCA_VLAN_PUSH_VLAN_ID]) {
141 if (exists)
142 tcf_hash_release(*a, bind);
143 return -EINVAL;
144 }
145 push_vid = nla_get_u16(tb[TCA_VLAN_PUSH_VLAN_ID]);
146 if (push_vid >= VLAN_VID_MASK) {
147 if (exists)
148 tcf_hash_release(*a, bind);
149 return -ERANGE;
150 }
151
152 if (tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]) {
153 push_proto = nla_get_be16(tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]);
154 switch (push_proto) {
155 case htons(ETH_P_8021Q):
156 case htons(ETH_P_8021AD):
157 break;
158 default:
159 return -EPROTONOSUPPORT;
160 }
161 } else {
162 push_proto = htons(ETH_P_8021Q);
163 }
164
165 if (tb[TCA_VLAN_PUSH_VLAN_PRIORITY])
166 push_prio = nla_get_u8(tb[TCA_VLAN_PUSH_VLAN_PRIORITY]);
167 break;
168 default:
169 if (exists)
170 tcf_hash_release(*a, bind);
171 return -EINVAL;
172 }
173 action = parm->v_action;
174
175 if (!exists) {
176 ret = tcf_hash_create(tn, parm->index, est, a,
177 &act_vlan_ops, bind, false);
178 if (ret)
179 return ret;
180
181 ret = ACT_P_CREATED;
182 } else {
183 tcf_hash_release(*a, bind);
184 if (!ovr)
185 return -EEXIST;
186 }
187
188 v = to_vlan(*a);
189
190 spin_lock_bh(&v->tcf_lock);
191
192 v->tcfv_action = action;
193 v->tcfv_push_vid = push_vid;
194 v->tcfv_push_prio = push_prio;
195 v->tcfv_push_proto = push_proto;
196
197 v->tcf_action = parm->action;
198
199 spin_unlock_bh(&v->tcf_lock);
200
201 if (ret == ACT_P_CREATED)
202 tcf_hash_insert(tn, *a);
203 return ret;
204}
205
206static int tcf_vlan_dump(struct sk_buff *skb, struct tc_action *a,
207 int bind, int ref)
208{
209 unsigned char *b = skb_tail_pointer(skb);
210 struct tcf_vlan *v = to_vlan(a);
211 struct tc_vlan opt = {
212 .index = v->tcf_index,
213 .refcnt = v->tcf_refcnt - ref,
214 .bindcnt = v->tcf_bindcnt - bind,
215 .action = v->tcf_action,
216 .v_action = v->tcfv_action,
217 };
218 struct tcf_t t;
219
220 if (nla_put(skb, TCA_VLAN_PARMS, sizeof(opt), &opt))
221 goto nla_put_failure;
222
223 if ((v->tcfv_action == TCA_VLAN_ACT_PUSH ||
224 v->tcfv_action == TCA_VLAN_ACT_MODIFY) &&
225 (nla_put_u16(skb, TCA_VLAN_PUSH_VLAN_ID, v->tcfv_push_vid) ||
226 nla_put_be16(skb, TCA_VLAN_PUSH_VLAN_PROTOCOL,
227 v->tcfv_push_proto) ||
228 (nla_put_u8(skb, TCA_VLAN_PUSH_VLAN_PRIORITY,
229 v->tcfv_push_prio))))
230 goto nla_put_failure;
231
232 tcf_tm_dump(&t, &v->tcf_tm);
233 if (nla_put_64bit(skb, TCA_VLAN_TM, sizeof(t), &t, TCA_VLAN_PAD))
234 goto nla_put_failure;
235 return skb->len;
236
237nla_put_failure:
238 nlmsg_trim(skb, b);
239 return -1;
240}
241
242static int tcf_vlan_walker(struct net *net, struct sk_buff *skb,
243 struct netlink_callback *cb, int type,
244 const struct tc_action_ops *ops)
245{
246 struct tc_action_net *tn = net_generic(net, vlan_net_id);
247
248 return tcf_generic_walker(tn, skb, cb, type, ops);
249}
250
251static int tcf_vlan_search(struct net *net, struct tc_action **a, u32 index)
252{
253 struct tc_action_net *tn = net_generic(net, vlan_net_id);
254
255 return tcf_hash_search(tn, a, index);
256}
257
258static struct tc_action_ops act_vlan_ops = {
259 .kind = "vlan",
260 .type = TCA_ACT_VLAN,
261 .owner = THIS_MODULE,
262 .act = tcf_vlan,
263 .dump = tcf_vlan_dump,
264 .init = tcf_vlan_init,
265 .walk = tcf_vlan_walker,
266 .lookup = tcf_vlan_search,
267 .size = sizeof(struct tcf_vlan),
268};
269
270static __net_init int vlan_init_net(struct net *net)
271{
272 struct tc_action_net *tn = net_generic(net, vlan_net_id);
273
274 return tc_action_net_init(tn, &act_vlan_ops, VLAN_TAB_MASK);
275}
276
277static void __net_exit vlan_exit_net(struct net *net)
278{
279 struct tc_action_net *tn = net_generic(net, vlan_net_id);
280
281 tc_action_net_exit(tn);
282}
283
284static struct pernet_operations vlan_net_ops = {
285 .init = vlan_init_net,
286 .exit = vlan_exit_net,
287 .id = &vlan_net_id,
288 .size = sizeof(struct tc_action_net),
289};
290
291static int __init vlan_init_module(void)
292{
293 return tcf_register_action(&act_vlan_ops, &vlan_net_ops);
294}
295
296static void __exit vlan_cleanup_module(void)
297{
298 tcf_unregister_action(&act_vlan_ops, &vlan_net_ops);
299}
300
301module_init(vlan_init_module);
302module_exit(vlan_cleanup_module);
303
304MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
305MODULE_DESCRIPTION("vlan manipulation actions");
306MODULE_LICENSE("GPL v2");
307
