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), 120 bind, false); 121 if (ret) 122 return ret; 123 124 ret = ACT_P_CREATED; 125 } else { 126 if (bind) 127 return 0; 128 tcf_hash_release(a, bind); 129 if (!ovr) 130 return -EEXIST; 131 } 132 133 v = to_vlan(a); 134 135 spin_lock_bh(&v->tcf_lock); 136 137 v->tcfv_action = action; 138 v->tcfv_push_vid = push_vid; 139 v->tcfv_push_proto = push_proto; 140 141 v->tcf_action = parm->action; 142 143 spin_unlock_bh(&v->tcf_lock); 144 145 if (ret == ACT_P_CREATED) 146 tcf_hash_insert(a); 147 return ret; 148 } 149 150 static int tcf_vlan_dump(struct sk_buff *skb, struct tc_action *a, 151 int bind, int ref) 152 { 153 unsigned char *b = skb_tail_pointer(skb); 154 struct tcf_vlan *v = a->priv; 155 struct tc_vlan opt = { 156 .index = v->tcf_index, 157 .refcnt = v->tcf_refcnt - ref, 158 .bindcnt = v->tcf_bindcnt - bind, 159 .action = v->tcf_action, 160 .v_action = v->tcfv_action, 161 }; 162 struct tcf_t t; 163 164 if (nla_put(skb, TCA_VLAN_PARMS, sizeof(opt), &opt)) 165 goto nla_put_failure; 166 167 if (v->tcfv_action == TCA_VLAN_ACT_PUSH && 168 (nla_put_u16(skb, TCA_VLAN_PUSH_VLAN_ID, v->tcfv_push_vid) || 169 nla_put_be16(skb, TCA_VLAN_PUSH_VLAN_PROTOCOL, v->tcfv_push_proto))) 170 goto nla_put_failure; 171 172 t.install = jiffies_to_clock_t(jiffies - v->tcf_tm.install); 173 t.lastuse = jiffies_to_clock_t(jiffies - v->tcf_tm.lastuse); 174 t.expires = jiffies_to_clock_t(v->tcf_tm.expires); 175 if (nla_put(skb, TCA_VLAN_TM, sizeof(t), &t)) 176 goto nla_put_failure; 177 return skb->len; 178 179 nla_put_failure: 180 nlmsg_trim(skb, b); 181 return -1; 182 } 183 184 static struct tc_action_ops act_vlan_ops = { 185 .kind = "vlan", 186 .type = TCA_ACT_VLAN, 187 .owner = THIS_MODULE, 188 .act = tcf_vlan, 189 .dump = tcf_vlan_dump, 190 .init = tcf_vlan_init, 191 }; 192 193 static int __init vlan_init_module(void) 194 { 195 return tcf_register_action(&act_vlan_ops, VLAN_TAB_MASK); 196 } 197 198 static void __exit vlan_cleanup_module(void) 199 { 200 tcf_unregister_action(&act_vlan_ops); 201 } 202 203 module_init(vlan_init_module); 204 module_exit(vlan_cleanup_module); 205 206 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>"); 207 MODULE_DESCRIPTION("vlan manipulation actions"); 208 MODULE_LICENSE("GPL v2"); 209