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 int vlan_net_id;
 25
 26static int tcf_vlan(struct sk_buff *skb, const struct tc_action *a,
 27		    struct tcf_result *res)
 28{
 29	struct tcf_vlan *v = a->priv;
 30	int action;
 31	int err;
 32
 33	spin_lock(&v->tcf_lock);
 34	v->tcf_tm.lastuse = jiffies;
 35	bstats_update(&v->tcf_bstats, skb);
 36	action = v->tcf_action;
 37
 38	switch (v->tcfv_action) {
 39	case TCA_VLAN_ACT_POP:
 40		err = skb_vlan_pop(skb);
 41		if (err)
 42			goto drop;
 43		break;
 44	case TCA_VLAN_ACT_PUSH:
 45		err = skb_vlan_push(skb, v->tcfv_push_proto, v->tcfv_push_vid);
 46		if (err)
 47			goto drop;
 48		break;
 49	default:
 50		BUG();
 51	}
 52
 53	goto unlock;
 54
 55drop:
 56	action = TC_ACT_SHOT;
 57	v->tcf_qstats.drops++;
 58unlock:
 59	spin_unlock(&v->tcf_lock);
 60	return action;
 61}
 62
 63static const struct nla_policy vlan_policy[TCA_VLAN_MAX + 1] = {
 64	[TCA_VLAN_PARMS]		= { .len = sizeof(struct tc_vlan) },
 65	[TCA_VLAN_PUSH_VLAN_ID]		= { .type = NLA_U16 },
 66	[TCA_VLAN_PUSH_VLAN_PROTOCOL]	= { .type = NLA_U16 },
 67};
 68
 69static int tcf_vlan_init(struct net *net, struct nlattr *nla,
 70			 struct nlattr *est, struct tc_action *a,
 71			 int ovr, int bind)
 72{
 73	struct tc_action_net *tn = net_generic(net, vlan_net_id);
 74	struct nlattr *tb[TCA_VLAN_MAX + 1];
 75	struct tc_vlan *parm;
 76	struct tcf_vlan *v;
 77	int action;
 78	__be16 push_vid = 0;
 79	__be16 push_proto = 0;
 80	int ret = 0, exists = 0;
 81	int err;
 82
 83	if (!nla)
 84		return -EINVAL;
 85
 86	err = nla_parse_nested(tb, TCA_VLAN_MAX, nla, vlan_policy);
 87	if (err < 0)
 88		return err;
 89
 90	if (!tb[TCA_VLAN_PARMS])
 91		return -EINVAL;
 92	parm = nla_data(tb[TCA_VLAN_PARMS]);
 93	exists = tcf_hash_check(tn, parm->index, a, bind);
 94	if (exists && bind)
 95		return 0;
 96
 97	switch (parm->v_action) {
 98	case TCA_VLAN_ACT_POP:
 99		break;
100	case TCA_VLAN_ACT_PUSH:
101		if (!tb[TCA_VLAN_PUSH_VLAN_ID]) {
102			if (exists)
103				tcf_hash_release(a, bind);
104			return -EINVAL;
105		}
106		push_vid = nla_get_u16(tb[TCA_VLAN_PUSH_VLAN_ID]);
107		if (push_vid >= VLAN_VID_MASK) {
108			if (exists)
109				tcf_hash_release(a, bind);
110			return -ERANGE;
111		}
112
113		if (tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]) {
114			push_proto = nla_get_be16(tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]);
115			switch (push_proto) {
116			case htons(ETH_P_8021Q):
117			case htons(ETH_P_8021AD):
118				break;
119			default:
120				return -EPROTONOSUPPORT;
121			}
122		} else {
123			push_proto = htons(ETH_P_8021Q);
124		}
125		break;
126	default:
127		if (exists)
128			tcf_hash_release(a, bind);
129		return -EINVAL;
130	}
131	action = parm->v_action;
132
133	if (!exists) {
134		ret = tcf_hash_create(tn, parm->index, est, a,
135				      sizeof(*v), bind, false);
136		if (ret)
137			return ret;
138
139		ret = ACT_P_CREATED;
140	} else {
141		tcf_hash_release(a, bind);
142		if (!ovr)
143			return -EEXIST;
144	}
145
146	v = to_vlan(a);
147
148	spin_lock_bh(&v->tcf_lock);
149
150	v->tcfv_action = action;
151	v->tcfv_push_vid = push_vid;
152	v->tcfv_push_proto = push_proto;
153
154	v->tcf_action = parm->action;
155
156	spin_unlock_bh(&v->tcf_lock);
157
158	if (ret == ACT_P_CREATED)
159		tcf_hash_insert(tn, a);
160	return ret;
161}
162
163static int tcf_vlan_dump(struct sk_buff *skb, struct tc_action *a,
164			 int bind, int ref)
165{
166	unsigned char *b = skb_tail_pointer(skb);
167	struct tcf_vlan *v = a->priv;
168	struct tc_vlan opt = {
169		.index    = v->tcf_index,
170		.refcnt   = v->tcf_refcnt - ref,
171		.bindcnt  = v->tcf_bindcnt - bind,
172		.action   = v->tcf_action,
173		.v_action = v->tcfv_action,
174	};
175	struct tcf_t t;
176
177	if (nla_put(skb, TCA_VLAN_PARMS, sizeof(opt), &opt))
178		goto nla_put_failure;
179
180	if (v->tcfv_action == TCA_VLAN_ACT_PUSH &&
181	    (nla_put_u16(skb, TCA_VLAN_PUSH_VLAN_ID, v->tcfv_push_vid) ||
182	     nla_put_be16(skb, TCA_VLAN_PUSH_VLAN_PROTOCOL, v->tcfv_push_proto)))
183		goto nla_put_failure;
184
185	t.install = jiffies_to_clock_t(jiffies - v->tcf_tm.install);
186	t.lastuse = jiffies_to_clock_t(jiffies - v->tcf_tm.lastuse);
187	t.expires = jiffies_to_clock_t(v->tcf_tm.expires);
188	if (nla_put(skb, TCA_VLAN_TM, sizeof(t), &t))
189		goto nla_put_failure;
190	return skb->len;
191
192nla_put_failure:
193	nlmsg_trim(skb, b);
194	return -1;
195}
196
197static int tcf_vlan_walker(struct net *net, struct sk_buff *skb,
198			   struct netlink_callback *cb, int type,
199			   struct tc_action *a)
200{
201	struct tc_action_net *tn = net_generic(net, vlan_net_id);
202
203	return tcf_generic_walker(tn, skb, cb, type, a);
204}
205
206static int tcf_vlan_search(struct net *net, struct tc_action *a, u32 index)
207{
208	struct tc_action_net *tn = net_generic(net, vlan_net_id);
209
210	return tcf_hash_search(tn, a, index);
211}
212
213static struct tc_action_ops act_vlan_ops = {
214	.kind		=	"vlan",
215	.type		=	TCA_ACT_VLAN,
216	.owner		=	THIS_MODULE,
217	.act		=	tcf_vlan,
218	.dump		=	tcf_vlan_dump,
219	.init		=	tcf_vlan_init,
220	.walk		=	tcf_vlan_walker,
221	.lookup		=	tcf_vlan_search,
222};
223
224static __net_init int vlan_init_net(struct net *net)
225{
226	struct tc_action_net *tn = net_generic(net, vlan_net_id);
227
228	return tc_action_net_init(tn, &act_vlan_ops, VLAN_TAB_MASK);
229}
230
231static void __net_exit vlan_exit_net(struct net *net)
232{
233	struct tc_action_net *tn = net_generic(net, vlan_net_id);
234
235	tc_action_net_exit(tn);
236}
237
238static struct pernet_operations vlan_net_ops = {
239	.init = vlan_init_net,
240	.exit = vlan_exit_net,
241	.id   = &vlan_net_id,
242	.size = sizeof(struct tc_action_net),
243};
244
245static int __init vlan_init_module(void)
246{
247	return tcf_register_action(&act_vlan_ops, &vlan_net_ops);
248}
249
250static void __exit vlan_cleanup_module(void)
251{
252	tcf_unregister_action(&act_vlan_ops, &vlan_net_ops);
253}
254
255module_init(vlan_init_module);
256module_exit(vlan_cleanup_module);
257
258MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
259MODULE_DESCRIPTION("vlan manipulation actions");
260MODULE_LICENSE("GPL v2");