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