1/*
  2 * Copyright (c) 2014 Jiri Pirko <jiri@resnulli.us>
  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 as published by
  6 * the Free Software Foundation; either version 2 of the License, or
  7 * (at your option) any later version.
  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 unsigned 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	/* Ensure 'data' points at mac_header prior calling vlan manipulating
 41	 * functions.
 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		/* No-op if no vlan tag (either hw-accel or in-payload) */
 60		if (!skb_vlan_tagged(skb))
 61			goto unlock;
 62		/* extract existing tag (and guarantee no hw-accel tag) */
 63		if (skb_vlan_tag_present(skb)) {
 64			tci = skb_vlan_tag_get(skb);
 65			skb->vlan_tci = 0;
 66		} else {
 67			/* in-payload vlan tag, pop it */
 68			err = __skb_vlan_pop(skb, &tci);
 69			if (err)
 70				goto drop;
 71		}
 72		/* replace the vid */
 73		tci = (tci & ~VLAN_VID_MASK) | v->tcfv_push_vid;
 74		/* replace prio bits, if tcfv_push_prio specified */
 75		if (v->tcfv_push_prio) {
 76			tci &= ~VLAN_PRIO_MASK;
 77			tci |= v->tcfv_push_prio << VLAN_PRIO_SHIFT;
 78		}
 79		/* put updated tci as hwaccel tag */
 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");