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 #include <net/pkt_cls.h> 19 20 #include <linux/tc_act/tc_vlan.h> 21 #include <net/tc_act/tc_vlan.h> 22 23 static unsigned int vlan_net_id; 24 static struct tc_action_ops act_vlan_ops; 25 26 static int tcf_vlan_act(struct sk_buff *skb, const struct tc_action *a, 27 struct tcf_result *res) 28 { 29 struct tcf_vlan *v = to_vlan(a); 30 struct tcf_vlan_params *p; 31 int action; 32 int err; 33 u16 tci; 34 35 tcf_lastuse_update(&v->tcf_tm); 36 bstats_cpu_update(this_cpu_ptr(v->common.cpu_bstats), skb); 37 38 /* Ensure 'data' points at mac_header prior calling vlan manipulating 39 * functions. 40 */ 41 if (skb_at_tc_ingress(skb)) 42 skb_push_rcsum(skb, skb->mac_len); 43 44 action = READ_ONCE(v->tcf_action); 45 46 p = rcu_dereference_bh(v->vlan_p); 47 48 switch (p->tcfv_action) { 49 case TCA_VLAN_ACT_POP: 50 err = skb_vlan_pop(skb); 51 if (err) 52 goto drop; 53 break; 54 case TCA_VLAN_ACT_PUSH: 55 err = skb_vlan_push(skb, p->tcfv_push_proto, p->tcfv_push_vid | 56 (p->tcfv_push_prio << VLAN_PRIO_SHIFT)); 57 if (err) 58 goto drop; 59 break; 60 case TCA_VLAN_ACT_MODIFY: 61 /* No-op if no vlan tag (either hw-accel or in-payload) */ 62 if (!skb_vlan_tagged(skb)) 63 goto out; 64 /* extract existing tag (and guarantee no hw-accel tag) */ 65 if (skb_vlan_tag_present(skb)) { 66 tci = skb_vlan_tag_get(skb); 67 __vlan_hwaccel_clear_tag(skb); 68 } else { 69 /* in-payload vlan tag, pop it */ 70 err = __skb_vlan_pop(skb, &tci); 71 if (err) 72 goto drop; 73 } 74 /* replace the vid */ 75 tci = (tci & ~VLAN_VID_MASK) | p->tcfv_push_vid; 76 /* replace prio bits, if tcfv_push_prio specified */ 77 if (p->tcfv_push_prio) { 78 tci &= ~VLAN_PRIO_MASK; 79 tci |= p->tcfv_push_prio << VLAN_PRIO_SHIFT; 80 } 81 /* put updated tci as hwaccel tag */ 82 __vlan_hwaccel_put_tag(skb, p->tcfv_push_proto, tci); 83 break; 84 default: 85 BUG(); 86 } 87 88 out: 89 if (skb_at_tc_ingress(skb)) 90 skb_pull_rcsum(skb, skb->mac_len); 91 92 return action; 93 94 drop: 95 qstats_drop_inc(this_cpu_ptr(v->common.cpu_qstats)); 96 return TC_ACT_SHOT; 97 } 98 99 static const struct nla_policy vlan_policy[TCA_VLAN_MAX + 1] = { 100 [TCA_VLAN_PARMS] = { .len = sizeof(struct tc_vlan) }, 101 [TCA_VLAN_PUSH_VLAN_ID] = { .type = NLA_U16 }, 102 [TCA_VLAN_PUSH_VLAN_PROTOCOL] = { .type = NLA_U16 }, 103 [TCA_VLAN_PUSH_VLAN_PRIORITY] = { .type = NLA_U8 }, 104 }; 105 106 static int tcf_vlan_init(struct net *net, struct nlattr *nla, 107 struct nlattr *est, struct tc_action **a, 108 int ovr, int bind, bool rtnl_held, 109 struct tcf_proto *tp, struct netlink_ext_ack *extack) 110 { 111 struct tc_action_net *tn = net_generic(net, vlan_net_id); 112 struct nlattr *tb[TCA_VLAN_MAX + 1]; 113 struct tcf_chain *goto_ch = NULL; 114 struct tcf_vlan_params *p; 115 struct tc_vlan *parm; 116 struct tcf_vlan *v; 117 int action; 118 u16 push_vid = 0; 119 __be16 push_proto = 0; 120 u8 push_prio = 0; 121 bool exists = false; 122 int ret = 0, err; 123 124 if (!nla) 125 return -EINVAL; 126 127 err = nla_parse_nested_deprecated(tb, TCA_VLAN_MAX, nla, vlan_policy, 128 NULL); 129 if (err < 0) 130 return err; 131 132 if (!tb[TCA_VLAN_PARMS]) 133 return -EINVAL; 134 parm = nla_data(tb[TCA_VLAN_PARMS]); 135 err = tcf_idr_check_alloc(tn, &parm->index, a, bind); 136 if (err < 0) 137 return err; 138 exists = err; 139 if (exists && bind) 140 return 0; 141 142 switch (parm->v_action) { 143 case TCA_VLAN_ACT_POP: 144 break; 145 case TCA_VLAN_ACT_PUSH: 146 case TCA_VLAN_ACT_MODIFY: 147 if (!tb[TCA_VLAN_PUSH_VLAN_ID]) { 148 if (exists) 149 tcf_idr_release(*a, bind); 150 else 151 tcf_idr_cleanup(tn, parm->index); 152 return -EINVAL; 153 } 154 push_vid = nla_get_u16(tb[TCA_VLAN_PUSH_VLAN_ID]); 155 if (push_vid >= VLAN_VID_MASK) { 156 if (exists) 157 tcf_idr_release(*a, bind); 158 else 159 tcf_idr_cleanup(tn, parm->index); 160 return -ERANGE; 161 } 162 163 if (tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]) { 164 push_proto = nla_get_be16(tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]); 165 switch (push_proto) { 166 case htons(ETH_P_8021Q): 167 case htons(ETH_P_8021AD): 168 break; 169 default: 170 if (exists) 171 tcf_idr_release(*a, bind); 172 else 173 tcf_idr_cleanup(tn, parm->index); 174 return -EPROTONOSUPPORT; 175 } 176 } else { 177 push_proto = htons(ETH_P_8021Q); 178 } 179 180 if (tb[TCA_VLAN_PUSH_VLAN_PRIORITY]) 181 push_prio = nla_get_u8(tb[TCA_VLAN_PUSH_VLAN_PRIORITY]); 182 break; 183 default: 184 if (exists) 185 tcf_idr_release(*a, bind); 186 else 187 tcf_idr_cleanup(tn, parm->index); 188 return -EINVAL; 189 } 190 action = parm->v_action; 191 192 if (!exists) { 193 ret = tcf_idr_create(tn, parm->index, est, a, 194 &act_vlan_ops, bind, true); 195 if (ret) { 196 tcf_idr_cleanup(tn, parm->index); 197 return ret; 198 } 199 200 ret = ACT_P_CREATED; 201 } else if (!ovr) { 202 tcf_idr_release(*a, bind); 203 return -EEXIST; 204 } 205 206 err = tcf_action_check_ctrlact(parm->action, tp, &goto_ch, extack); 207 if (err < 0) 208 goto release_idr; 209 210 v = to_vlan(*a); 211 212 p = kzalloc(sizeof(*p), GFP_KERNEL); 213 if (!p) { 214 err = -ENOMEM; 215 goto put_chain; 216 } 217 218 p->tcfv_action = action; 219 p->tcfv_push_vid = push_vid; 220 p->tcfv_push_prio = push_prio; 221 p->tcfv_push_proto = push_proto; 222 223 spin_lock_bh(&v->tcf_lock); 224 goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch); 225 rcu_swap_protected(v->vlan_p, p, lockdep_is_held(&v->tcf_lock)); 226 spin_unlock_bh(&v->tcf_lock); 227 228 if (goto_ch) 229 tcf_chain_put_by_act(goto_ch); 230 if (p) 231 kfree_rcu(p, rcu); 232 233 if (ret == ACT_P_CREATED) 234 tcf_idr_insert(tn, *a); 235 return ret; 236 put_chain: 237 if (goto_ch) 238 tcf_chain_put_by_act(goto_ch); 239 release_idr: 240 tcf_idr_release(*a, bind); 241 return err; 242 } 243 244 static void tcf_vlan_cleanup(struct tc_action *a) 245 { 246 struct tcf_vlan *v = to_vlan(a); 247 struct tcf_vlan_params *p; 248 249 p = rcu_dereference_protected(v->vlan_p, 1); 250 if (p) 251 kfree_rcu(p, rcu); 252 } 253 254 static int tcf_vlan_dump(struct sk_buff *skb, struct tc_action *a, 255 int bind, int ref) 256 { 257 unsigned char *b = skb_tail_pointer(skb); 258 struct tcf_vlan *v = to_vlan(a); 259 struct tcf_vlan_params *p; 260 struct tc_vlan opt = { 261 .index = v->tcf_index, 262 .refcnt = refcount_read(&v->tcf_refcnt) - ref, 263 .bindcnt = atomic_read(&v->tcf_bindcnt) - bind, 264 }; 265 struct tcf_t t; 266 267 spin_lock_bh(&v->tcf_lock); 268 opt.action = v->tcf_action; 269 p = rcu_dereference_protected(v->vlan_p, lockdep_is_held(&v->tcf_lock)); 270 opt.v_action = p->tcfv_action; 271 if (nla_put(skb, TCA_VLAN_PARMS, sizeof(opt), &opt)) 272 goto nla_put_failure; 273 274 if ((p->tcfv_action == TCA_VLAN_ACT_PUSH || 275 p->tcfv_action == TCA_VLAN_ACT_MODIFY) && 276 (nla_put_u16(skb, TCA_VLAN_PUSH_VLAN_ID, p->tcfv_push_vid) || 277 nla_put_be16(skb, TCA_VLAN_PUSH_VLAN_PROTOCOL, 278 p->tcfv_push_proto) || 279 (nla_put_u8(skb, TCA_VLAN_PUSH_VLAN_PRIORITY, 280 p->tcfv_push_prio)))) 281 goto nla_put_failure; 282 283 tcf_tm_dump(&t, &v->tcf_tm); 284 if (nla_put_64bit(skb, TCA_VLAN_TM, sizeof(t), &t, TCA_VLAN_PAD)) 285 goto nla_put_failure; 286 spin_unlock_bh(&v->tcf_lock); 287 288 return skb->len; 289 290 nla_put_failure: 291 spin_unlock_bh(&v->tcf_lock); 292 nlmsg_trim(skb, b); 293 return -1; 294 } 295 296 static int tcf_vlan_walker(struct net *net, struct sk_buff *skb, 297 struct netlink_callback *cb, int type, 298 const struct tc_action_ops *ops, 299 struct netlink_ext_ack *extack) 300 { 301 struct tc_action_net *tn = net_generic(net, vlan_net_id); 302 303 return tcf_generic_walker(tn, skb, cb, type, ops, extack); 304 } 305 306 static int tcf_vlan_search(struct net *net, struct tc_action **a, u32 index) 307 { 308 struct tc_action_net *tn = net_generic(net, vlan_net_id); 309 310 return tcf_idr_search(tn, a, index); 311 } 312 313 static struct tc_action_ops act_vlan_ops = { 314 .kind = "vlan", 315 .id = TCA_ID_VLAN, 316 .owner = THIS_MODULE, 317 .act = tcf_vlan_act, 318 .dump = tcf_vlan_dump, 319 .init = tcf_vlan_init, 320 .cleanup = tcf_vlan_cleanup, 321 .walk = tcf_vlan_walker, 322 .lookup = tcf_vlan_search, 323 .size = sizeof(struct tcf_vlan), 324 }; 325 326 static __net_init int vlan_init_net(struct net *net) 327 { 328 struct tc_action_net *tn = net_generic(net, vlan_net_id); 329 330 return tc_action_net_init(tn, &act_vlan_ops); 331 } 332 333 static void __net_exit vlan_exit_net(struct list_head *net_list) 334 { 335 tc_action_net_exit(net_list, vlan_net_id); 336 } 337 338 static struct pernet_operations vlan_net_ops = { 339 .init = vlan_init_net, 340 .exit_batch = vlan_exit_net, 341 .id = &vlan_net_id, 342 .size = sizeof(struct tc_action_net), 343 }; 344 345 static int __init vlan_init_module(void) 346 { 347 return tcf_register_action(&act_vlan_ops, &vlan_net_ops); 348 } 349 350 static void __exit vlan_cleanup_module(void) 351 { 352 tcf_unregister_action(&act_vlan_ops, &vlan_net_ops); 353 } 354 355 module_init(vlan_init_module); 356 module_exit(vlan_cleanup_module); 357 358 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>"); 359 MODULE_DESCRIPTION("vlan manipulation actions"); 360 MODULE_LICENSE("GPL v2"); 361