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