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