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 24 static 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 53 drop: 54 action = TC_ACT_SHOT; 55 v->tcf_qstats.drops++; 56 unlock: 57 spin_unlock(&v->tcf_lock); 58 return action; 59 } 60 61 static 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 67 static 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 149 static 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 178 nla_put_failure: 179 nlmsg_trim(skb, b); 180 return -1; 181 } 182 183 static 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 192 static int __init vlan_init_module(void) 193 { 194 return tcf_register_action(&act_vlan_ops, VLAN_TAB_MASK); 195 } 196 197 static void __exit vlan_cleanup_module(void) 198 { 199 tcf_unregister_action(&act_vlan_ops); 200 } 201 202 module_init(vlan_init_module); 203 module_exit(vlan_cleanup_module); 204 205 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>"); 206 MODULE_DESCRIPTION("vlan manipulation actions"); 207 MODULE_LICENSE("GPL v2"); 208