1 /*- 2 * Copyright (c) 2003 IPNET Internet Communication Company 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24 * SUCH DAMAGE. 25 * 26 * Author: Ruslan Ermilov <ru@FreeBSD.org> 27 * 28 * $FreeBSD$ 29 */ 30 31 #include <sys/param.h> 32 #include <sys/errno.h> 33 #include <sys/kernel.h> 34 #include <sys/malloc.h> 35 #include <sys/mbuf.h> 36 #include <sys/queue.h> 37 #include <sys/socket.h> 38 #include <sys/systm.h> 39 40 #include <net/ethernet.h> 41 #include <net/if.h> 42 #include <net/if_vlan_var.h> 43 44 #include <netgraph/ng_message.h> 45 #include <netgraph/ng_parse.h> 46 #include <netgraph/ng_vlan.h> 47 #include <netgraph/netgraph.h> 48 49 static ng_constructor_t ng_vlan_constructor; 50 static ng_rcvmsg_t ng_vlan_rcvmsg; 51 static ng_shutdown_t ng_vlan_shutdown; 52 static ng_newhook_t ng_vlan_newhook; 53 static ng_rcvdata_t ng_vlan_rcvdata; 54 static ng_disconnect_t ng_vlan_disconnect; 55 56 /* Parse type for struct ng_vlan_filter. */ 57 static const struct ng_parse_struct_field ng_vlan_filter_fields[] = 58 NG_VLAN_FILTER_FIELDS; 59 static const struct ng_parse_type ng_vlan_filter_type = { 60 &ng_parse_struct_type, 61 &ng_vlan_filter_fields 62 }; 63 64 static int 65 ng_vlan_getTableLength(const struct ng_parse_type *type, 66 const u_char *start, const u_char *buf) 67 { 68 const struct ng_vlan_table *const table = 69 (const struct ng_vlan_table *)(buf - sizeof(u_int32_t)); 70 71 return table->n; 72 } 73 74 /* Parse type for struct ng_vlan_table. */ 75 static const struct ng_parse_array_info ng_vlan_table_array_info = { 76 &ng_vlan_filter_type, 77 ng_vlan_getTableLength 78 }; 79 static const struct ng_parse_type ng_vlan_table_array_type = { 80 &ng_parse_array_type, 81 &ng_vlan_table_array_info 82 }; 83 static const struct ng_parse_struct_field ng_vlan_table_fields[] = 84 NG_VLAN_TABLE_FIELDS; 85 static const struct ng_parse_type ng_vlan_table_type = { 86 &ng_parse_struct_type, 87 &ng_vlan_table_fields 88 }; 89 90 /* List of commands and how to convert arguments to/from ASCII. */ 91 static const struct ng_cmdlist ng_vlan_cmdlist[] = { 92 { 93 NGM_VLAN_COOKIE, 94 NGM_VLAN_ADD_FILTER, 95 "addfilter", 96 &ng_vlan_filter_type, 97 NULL 98 }, 99 { 100 NGM_VLAN_COOKIE, 101 NGM_VLAN_DEL_FILTER, 102 "delfilter", 103 &ng_parse_hookbuf_type, 104 NULL 105 }, 106 { 107 NGM_VLAN_COOKIE, 108 NGM_VLAN_GET_TABLE, 109 "gettable", 110 NULL, 111 &ng_vlan_table_type 112 }, 113 { 0 } 114 }; 115 116 static struct ng_type ng_vlan_typestruct = { 117 NG_ABI_VERSION, 118 NG_VLAN_NODE_TYPE, 119 NULL, 120 ng_vlan_constructor, 121 ng_vlan_rcvmsg, 122 ng_vlan_shutdown, 123 ng_vlan_newhook, 124 NULL, 125 NULL, 126 ng_vlan_rcvdata, 127 ng_vlan_disconnect, 128 ng_vlan_cmdlist 129 }; 130 NETGRAPH_INIT(vlan, &ng_vlan_typestruct); 131 132 struct filter { 133 LIST_ENTRY(filter) next; 134 u_int16_t vlan; 135 hook_p hook; 136 }; 137 138 #define HASHSIZE 16 139 #define HASH(id) ((((id) >> 8) ^ ((id) >> 4) ^ (id)) & 0x0f) 140 LIST_HEAD(filterhead, filter); 141 142 typedef struct { 143 hook_p downstream_hook; 144 hook_p nomatch_hook; 145 struct filterhead hashtable[HASHSIZE]; 146 u_int32_t nent; 147 } *priv_p; 148 149 static struct filter * 150 ng_vlan_findentry(priv_p priv, u_int16_t vlan) 151 { 152 struct filterhead *chain = &priv->hashtable[HASH(vlan)]; 153 struct filter *f; 154 155 LIST_FOREACH(f, chain, next) 156 if (f->vlan == vlan) 157 return (f); 158 return (NULL); 159 } 160 161 static int 162 ng_vlan_constructor(node_p node) 163 { 164 priv_p priv; 165 int i; 166 167 MALLOC(priv, priv_p, sizeof(*priv), M_NETGRAPH, M_NOWAIT | M_ZERO); 168 if (priv == NULL) 169 return (ENOMEM); 170 for (i = 0; i < HASHSIZE; i++) 171 LIST_INIT(&priv->hashtable[i]); 172 NG_NODE_SET_PRIVATE(node, priv); 173 return (0); 174 } 175 176 static int 177 ng_vlan_newhook(node_p node, hook_p hook, const char *name) 178 { 179 const priv_p priv = NG_NODE_PRIVATE(node); 180 181 if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0) 182 priv->downstream_hook = hook; 183 else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0) 184 priv->nomatch_hook = hook; 185 else { 186 /* 187 * Any other hook name is valid and can 188 * later be associated with a filter rule. 189 */ 190 } 191 NG_HOOK_SET_PRIVATE(hook, NULL); 192 return (0); 193 } 194 195 static int 196 ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook) 197 { 198 const priv_p priv = NG_NODE_PRIVATE(node); 199 int error = 0; 200 struct ng_mesg *msg, *resp = NULL; 201 struct ng_vlan_filter *vf; 202 struct filter *f; 203 hook_p hook; 204 struct ng_vlan_table *t; 205 int i; 206 207 NGI_GET_MSG(item, msg); 208 /* Deal with message according to cookie and command. */ 209 switch (msg->header.typecookie) { 210 case NGM_VLAN_COOKIE: 211 switch (msg->header.cmd) { 212 case NGM_VLAN_ADD_FILTER: 213 /* Check that message is long enough. */ 214 if (msg->header.arglen != sizeof(*vf)) { 215 error = EINVAL; 216 break; 217 } 218 vf = (struct ng_vlan_filter *)msg->data; 219 /* Sanity check the VLAN ID value. */ 220 if (vf->vlan & ~EVL_VLID_MASK) { 221 error = EINVAL; 222 break; 223 } 224 /* Check that a referenced hook exists. */ 225 hook = ng_findhook(node, vf->hook); 226 if (hook == NULL) { 227 error = ENOENT; 228 break; 229 } 230 /* And is not one of the special hooks. */ 231 if (hook == priv->downstream_hook || 232 hook == priv->nomatch_hook) { 233 error = EINVAL; 234 break; 235 } 236 /* And is not already in service. */ 237 if (NG_HOOK_PRIVATE(hook) != NULL) { 238 error = EEXIST; 239 break; 240 } 241 /* Check we don't already trap this VLAN. */ 242 if (ng_vlan_findentry(priv, vf->vlan)) { 243 error = EEXIST; 244 break; 245 } 246 /* Create filter. */ 247 MALLOC(f, struct filter *, sizeof(*f), 248 M_NETGRAPH, M_NOWAIT | M_ZERO); 249 if (f == NULL) { 250 error = ENOMEM; 251 break; 252 } 253 /* Link filter and hook together. */ 254 f->hook = hook; 255 f->vlan = vf->vlan; 256 NG_HOOK_SET_PRIVATE(hook, f); 257 /* Register filter in a hash table. */ 258 LIST_INSERT_HEAD( 259 &priv->hashtable[HASH(f->vlan)], f, next); 260 priv->nent++; 261 break; 262 case NGM_VLAN_DEL_FILTER: 263 /* Check that message is long enough. */ 264 if (msg->header.arglen != NG_HOOKSIZ) { 265 error = EINVAL; 266 break; 267 } 268 /* Check that hook exists and is active. */ 269 hook = ng_findhook(node, (char *)msg->data); 270 if (hook == NULL || 271 (f = NG_HOOK_PRIVATE(hook)) == NULL) { 272 error = ENOENT; 273 break; 274 } 275 /* Purge a rule that refers to this hook. */ 276 NG_HOOK_SET_PRIVATE(hook, NULL); 277 LIST_REMOVE(f, next); 278 priv->nent--; 279 FREE(f, M_NETGRAPH); 280 break; 281 case NGM_VLAN_GET_TABLE: 282 NG_MKRESPONSE(resp, msg, sizeof(*t) + 283 priv->nent * sizeof(*t->filter), M_NOWAIT); 284 if (resp == NULL) { 285 error = ENOMEM; 286 break; 287 } 288 t = (struct ng_vlan_table *)resp->data; 289 t->n = priv->nent; 290 vf = &t->filter[0]; 291 for (i = 0; i < HASHSIZE; i++) { 292 LIST_FOREACH(f, &priv->hashtable[i], next) { 293 vf->vlan = f->vlan; 294 strncpy(vf->hook, NG_HOOK_NAME(f->hook), 295 NG_HOOKSIZ); 296 vf++; 297 } 298 } 299 break; 300 default: /* Unknown command. */ 301 error = EINVAL; 302 break; 303 } 304 break; 305 default: /* Unknown type cookie. */ 306 error = EINVAL; 307 break; 308 } 309 NG_RESPOND_MSG(error, node, item, resp); 310 NG_FREE_MSG(msg); 311 return (error); 312 } 313 314 static int 315 ng_vlan_rcvdata(hook_p hook, item_p item) 316 { 317 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 318 struct ether_header *eh; 319 struct ether_vlan_header *evl; 320 int error; 321 u_int16_t vlan; 322 struct mbuf *m; 323 struct m_tag *mtag; 324 struct filter *f; 325 326 /* Make sure we have an entire header. */ 327 NGI_GET_M(item, m); 328 if (m->m_len < sizeof(*eh) && 329 (m = m_pullup(m, sizeof(*eh))) == NULL) { 330 NG_FREE_ITEM(item); 331 return (EINVAL); 332 } 333 eh = mtod(m, struct ether_header *); 334 if (hook == priv->downstream_hook) { 335 /* 336 * If from downstream, select between a match hook 337 * or the nomatch hook. 338 */ 339 mtag = m_tag_locate(m, MTAG_VLAN, MTAG_VLAN_TAG, NULL); 340 if (mtag != NULL || eh->ether_type == htons(ETHERTYPE_VLAN)) { 341 if (mtag != NULL) { 342 /* 343 * Packet is tagged, m contains a normal 344 * Ethernet frame; tag is stored out-of-band. 345 */ 346 vlan = EVL_VLANOFTAG(VLAN_TAG_VALUE(mtag)); 347 (void)&evl; /* XXX silence GCC */ 348 } else { 349 if (m->m_len < sizeof(*evl) && 350 (m = m_pullup(m, sizeof(*evl))) == NULL) { 351 NG_FREE_ITEM(item); 352 return (EINVAL); 353 } 354 evl = mtod(m, struct ether_vlan_header *); 355 vlan = EVL_VLANOFTAG(ntohs(evl->evl_tag)); 356 } 357 if ((f = ng_vlan_findentry(priv, vlan)) != NULL) { 358 if (mtag != NULL) 359 m_tag_delete(m, mtag); 360 else { 361 evl->evl_encap_proto = evl->evl_proto; 362 bcopy(mtod(m, caddr_t), 363 mtod(m, caddr_t) + 364 ETHER_VLAN_ENCAP_LEN, 365 ETHER_HDR_LEN); 366 m_adj(m, ETHER_VLAN_ENCAP_LEN); 367 } 368 } 369 } else 370 f = NULL; 371 if (f != NULL) 372 NG_FWD_NEW_DATA(error, item, f->hook, m); 373 else 374 NG_FWD_NEW_DATA(error, item, priv->nomatch_hook, m); 375 } else { 376 /* 377 * It is heading towards the downstream. 378 * If from nomatch, pass it unmodified. 379 * Otherwise, do the VLAN encapsulation. 380 */ 381 if (hook != priv->nomatch_hook) { 382 if ((f = NG_HOOK_PRIVATE(hook)) == NULL) { 383 NG_FREE_ITEM(item); 384 NG_FREE_M(m); 385 return (EOPNOTSUPP); 386 } 387 M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_DONTWAIT); 388 /* M_PREPEND takes care of m_len and m_pkthdr.len. */ 389 if (m == NULL || (m->m_len < sizeof(*evl) && 390 (m = m_pullup(m, sizeof(*evl))) == NULL)) { 391 NG_FREE_ITEM(item); 392 return (ENOMEM); 393 } 394 /* 395 * Transform the Ethernet header into an Ethernet header 396 * with 802.1Q encapsulation. 397 */ 398 bcopy(mtod(m, char *) + ETHER_VLAN_ENCAP_LEN, 399 mtod(m, char *), ETHER_HDR_LEN); 400 evl = mtod(m, struct ether_vlan_header *); 401 evl->evl_proto = evl->evl_encap_proto; 402 evl->evl_encap_proto = htons(ETHERTYPE_VLAN); 403 evl->evl_tag = htons(f->vlan); 404 } 405 NG_FWD_NEW_DATA(error, item, priv->downstream_hook, m); 406 } 407 return (error); 408 } 409 410 static int 411 ng_vlan_shutdown(node_p node) 412 { 413 const priv_p priv = NG_NODE_PRIVATE(node); 414 415 NG_NODE_SET_PRIVATE(node, NULL); 416 NG_NODE_UNREF(node); 417 FREE(priv, M_NETGRAPH); 418 return (0); 419 } 420 421 static int 422 ng_vlan_disconnect(hook_p hook) 423 { 424 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); 425 struct filter *f; 426 427 if (hook == priv->downstream_hook) 428 priv->downstream_hook = NULL; 429 else if (hook == priv->nomatch_hook) 430 priv->nomatch_hook = NULL; 431 else { 432 /* Purge a rule that refers to this hook. */ 433 if ((f = NG_HOOK_PRIVATE(hook)) != NULL) { 434 LIST_REMOVE(f, next); 435 priv->nent--; 436 FREE(f, M_NETGRAPH); 437 } 438 } 439 NG_HOOK_SET_PRIVATE(hook, NULL); 440 if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) && 441 (NG_NODE_IS_VALID(NG_HOOK_NODE(hook)))) 442 ng_rmnode_self(NG_HOOK_NODE(hook)); 443 return (0); 444 } 445