Line 0
Link Here
|
|
|
1 |
/* |
2 |
* SPDX-License-Identifier: BSD-2-Clause-FreeBSD |
3 |
* |
4 |
* Copyright (c) 2019 IKS Service GmbH |
5 |
* All rights reserved. |
6 |
* |
7 |
* Redistribution and use in source and binary forms, with or without |
8 |
* modification, are permitted provided that the following conditions |
9 |
* are met: |
10 |
* 1. Redistributions of source code must retain the above copyright |
11 |
* notice, this list of conditions and the following disclaimer. |
12 |
* 2. Redistributions in binary form must reproduce the above copyright |
13 |
* notice, this list of conditions and the following disclaimer in the |
14 |
* documentation and/or other materials provided with the distribution. |
15 |
* |
16 |
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND |
17 |
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
18 |
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
19 |
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE |
20 |
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
21 |
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
22 |
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
23 |
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
24 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
25 |
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
26 |
* SUCH DAMAGE. |
27 |
* |
28 |
* Author: Lutz Donnerhacke <lutz@donnerhacke.de> |
29 |
* |
30 |
* $FreeBSD$ |
31 |
*/ |
32 |
|
33 |
#include <sys/param.h> |
34 |
#include <sys/systm.h> |
35 |
#include <sys/kernel.h> |
36 |
#include <sys/mbuf.h> |
37 |
#include <sys/malloc.h> |
38 |
#include <sys/ctype.h> |
39 |
#include <sys/errno.h> |
40 |
#include <sys/syslog.h> |
41 |
|
42 |
#include <net/ethernet.h> |
43 |
|
44 |
#include <netgraph/ng_message.h> |
45 |
#include <netgraph/ng_parse.h> |
46 |
#include <netgraph/ng_vlan_rotate.h> |
47 |
#include <netgraph/netgraph.h> |
48 |
|
49 |
/* |
50 |
* This section contains the netgraph method declarations for the |
51 |
* sample node. These methods define the netgraph 'type'. |
52 |
*/ |
53 |
|
54 |
static ng_constructor_t ng_vlanrotate_constructor; |
55 |
static ng_rcvmsg_t ng_vlanrotate_rcvmsg; |
56 |
static ng_shutdown_t ng_vlanrotate_shutdown; |
57 |
static ng_newhook_t ng_vlanrotate_newhook; |
58 |
static ng_rcvdata_t ng_vlanrotate_rcvdata; |
59 |
static ng_disconnect_t ng_vlanrotate_disconnect; |
60 |
|
61 |
/* Parse type for struct ng_vlanrotate_conf. */ |
62 |
static const struct ng_parse_struct_field ng_vlanrotate_conf_fields[] = { |
63 |
{ "rot", &ng_parse_int8_type }, |
64 |
{ "min", &ng_parse_uint8_type }, |
65 |
{ "max", &ng_parse_uint8_type }, |
66 |
{ NULL } |
67 |
}; |
68 |
static const struct ng_parse_type ng_vlanrotate_conf_type = { |
69 |
&ng_parse_struct_type, |
70 |
&ng_vlanrotate_conf_fields |
71 |
}; |
72 |
|
73 |
/* Parse type for struct ng_vlanrotate_stat. */ |
74 |
static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = { |
75 |
&ng_parse_uint64_type, |
76 |
NG_VLANROTATE_MAX_VLANS |
77 |
}; |
78 |
static struct ng_parse_type ng_vlanrotate_stat_hist = { |
79 |
&ng_parse_fixedarray_type, |
80 |
&ng_vlanrotate_stat_hist_info |
81 |
}; |
82 |
static const struct ng_parse_struct_field ng_vlanrotate_stat_fields[] = { |
83 |
{ "drops", &ng_parse_uint64_type }, |
84 |
{ "excessive", &ng_parse_uint64_type }, |
85 |
{ "incomplete", &ng_parse_uint64_type }, |
86 |
{ "histogram", &ng_vlanrotate_stat_hist }, |
87 |
{ NULL } |
88 |
}; |
89 |
static struct ng_parse_type ng_vlanrotate_stat_type = { |
90 |
&ng_parse_struct_type, |
91 |
&ng_vlanrotate_stat_fields |
92 |
}; |
93 |
|
94 |
|
95 |
/* List of commands and how to convert arguments to/from ASCII */ |
96 |
static const struct ng_cmdlist ng_vlanrotate_cmdlist[] = { |
97 |
{ |
98 |
NGM_VLANROTATE_COOKIE, |
99 |
NGM_VLANROTATE_GET_CONF, |
100 |
"getconf", |
101 |
NULL, |
102 |
&ng_vlanrotate_conf_type, |
103 |
}, |
104 |
{ |
105 |
NGM_VLANROTATE_COOKIE, |
106 |
NGM_VLANROTATE_SET_CONF, |
107 |
"setconf", |
108 |
&ng_vlanrotate_conf_type, |
109 |
NULL |
110 |
}, |
111 |
{ |
112 |
NGM_VLANROTATE_COOKIE, |
113 |
NGM_VLANROTATE_GET_STAT, |
114 |
"getstat", |
115 |
NULL, |
116 |
&ng_vlanrotate_stat_type |
117 |
}, |
118 |
{ |
119 |
NGM_VLANROTATE_COOKIE, |
120 |
NGM_VLANROTATE_CLR_STAT, |
121 |
"clrstat", |
122 |
NULL, |
123 |
&ng_vlanrotate_stat_type |
124 |
}, |
125 |
{ |
126 |
NGM_VLANROTATE_COOKIE, |
127 |
NGM_VLANROTATE_GETCLR_STAT, |
128 |
"getclrstat", |
129 |
NULL, |
130 |
&ng_vlanrotate_stat_type |
131 |
}, |
132 |
{ 0 } |
133 |
}; |
134 |
|
135 |
/* Netgraph node type descriptor */ |
136 |
static struct ng_type typestruct = { |
137 |
.version = NG_ABI_VERSION, |
138 |
.name = NG_VLANROTATE_NODE_TYPE, |
139 |
.constructor = ng_vlanrotate_constructor, |
140 |
.rcvmsg = ng_vlanrotate_rcvmsg, |
141 |
.shutdown = ng_vlanrotate_shutdown, |
142 |
.newhook = ng_vlanrotate_newhook, |
143 |
.rcvdata = ng_vlanrotate_rcvdata, |
144 |
.disconnect = ng_vlanrotate_disconnect, |
145 |
.cmdlist = ng_vlanrotate_cmdlist, |
146 |
}; |
147 |
NETGRAPH_INIT(vlanrotate, &typestruct); |
148 |
|
149 |
/* Information we store for each node */ |
150 |
struct vlanrotate { |
151 |
hook_p original_hook; |
152 |
hook_p ordered_hook; |
153 |
hook_p excessive_hook; |
154 |
hook_p incomplete_hook; |
155 |
struct ng_vlanrotate_conf conf; |
156 |
struct ng_vlanrotate_stat stat; |
157 |
}; |
158 |
typedef struct vlanrotate *vlanrotate_p; |
159 |
|
160 |
/* |
161 |
* Set up the private data structure. |
162 |
*/ |
163 |
static int |
164 |
ng_vlanrotate_constructor(node_p node) |
165 |
{ |
166 |
vlanrotate_p vrp = malloc(sizeof(*vrp), M_NETGRAPH, M_WAITOK | M_ZERO); |
167 |
|
168 |
vrp->conf.max = NG_VLANROTATE_MAX_VLANS; |
169 |
|
170 |
NG_NODE_SET_PRIVATE(node, vrp); |
171 |
return (0); |
172 |
} |
173 |
|
174 |
/* |
175 |
* Give our ok for a hook to be added. |
176 |
*/ |
177 |
static int |
178 |
ng_vlanrotate_newhook(node_p node, hook_p hook, const char *name) |
179 |
{ |
180 |
const vlanrotate_p vrp = NG_NODE_PRIVATE(node); |
181 |
hook_p *dst = NULL; |
182 |
|
183 |
if (strcmp(name, NG_VLANROTATE_HOOK_ORDERED) == 0) { |
184 |
dst = &vrp->ordered_hook; |
185 |
} else if (strcmp(name, NG_VLANROTATE_HOOK_ORIGINAL) == 0) { |
186 |
dst = &vrp->original_hook; |
187 |
} else if (strcmp(name, NG_VLANROTATE_HOOK_EXCESSIVE) == 0) { |
188 |
dst = &vrp->excessive_hook; |
189 |
} else if (strcmp(name, NG_VLANROTATE_HOOK_INCOMPLETE) == 0) { |
190 |
dst = &vrp->incomplete_hook; |
191 |
} |
192 |
|
193 |
if(dst == NULL) |
194 |
return (EINVAL); /* not a hook we know about */ |
195 |
|
196 |
if(*dst != NULL) |
197 |
return (EADDRINUSE); /* don't override */ |
198 |
|
199 |
*dst = hook; |
200 |
return (0); |
201 |
} |
202 |
|
203 |
/* |
204 |
* Get a netgraph control message. |
205 |
* We actually receive a queue item that has a pointer to the message. |
206 |
* If we free the item, the message will be freed too, unless we remove |
207 |
* it from the item using NGI_GET_MSG(); |
208 |
* The return address is also stored in the item, as an ng_ID_t, |
209 |
* accessible as NGI_RETADDR(item); |
210 |
* Check it is one we understand. If needed, send a response. |
211 |
* We could save the address for an async action later, but don't here. |
212 |
* Always free the message. |
213 |
* The response should be in a malloc'd region that the caller can 'free'. |
214 |
* A response is not required. |
215 |
*/ |
216 |
static int |
217 |
ng_vlanrotate_rcvmsg(node_p node, item_p item, hook_p lasthook) |
218 |
{ |
219 |
const vlanrotate_p vrp = NG_NODE_PRIVATE(node); |
220 |
struct ng_mesg *resp = NULL; |
221 |
int error = 0; |
222 |
struct ng_mesg *msg; |
223 |
struct ng_vlanrotate_conf * pcf; |
224 |
|
225 |
NGI_GET_MSG(item, msg); |
226 |
/* Deal with message according to cookie and command */ |
227 |
switch (msg->header.typecookie) { |
228 |
case NGM_VLANROTATE_COOKIE: |
229 |
switch (msg->header.cmd) { |
230 |
case NGM_VLANROTATE_GET_CONF: |
231 |
NG_MKRESPONSE(resp, msg, sizeof(vrp->conf), M_NOWAIT); |
232 |
if (!resp) { |
233 |
error = ENOMEM; |
234 |
break; |
235 |
} |
236 |
*((struct ng_vlanrotate_conf *) resp->data) = vrp->conf; |
237 |
break; |
238 |
case NGM_VLANROTATE_SET_CONF: |
239 |
if (msg->header.arglen != sizeof(*pcf)) { |
240 |
error = EINVAL; |
241 |
break; |
242 |
} |
243 |
|
244 |
pcf = (struct ng_vlanrotate_conf *) msg->data; |
245 |
|
246 |
if(pcf->max == 0) pcf->max = vrp->conf.max; /* keep current value */ |
247 |
|
248 |
if(pcf->max > NG_VLANROTATE_MAX_VLANS) error = EINVAL; |
249 |
if(pcf->min > pcf->max) error = EINVAL; |
250 |
if(abs(pcf->rot) >= pcf->max) error = EINVAL; |
251 |
|
252 |
if(error == 0) /* okay */ |
253 |
vrp->conf = *pcf; |
254 |
|
255 |
break; |
256 |
case NGM_VLANROTATE_GET_STAT: |
257 |
case NGM_VLANROTATE_GETCLR_STAT: |
258 |
NG_MKRESPONSE(resp, msg, sizeof(vrp->stat), M_NOWAIT); |
259 |
if (!resp) { |
260 |
error = ENOMEM; |
261 |
break; |
262 |
} |
263 |
*(struct ng_vlanrotate_stat *)resp->data = vrp->stat; |
264 |
if(msg->header.cmd != NGM_VLANROTATE_GETCLR_STAT) |
265 |
break; |
266 |
case NGM_VLANROTATE_CLR_STAT: |
267 |
bzero(&(vrp->stat), sizeof(vrp->stat)); |
268 |
break; |
269 |
default: |
270 |
error = EINVAL; /* unknown command */ |
271 |
break; |
272 |
} |
273 |
break; |
274 |
default: |
275 |
error = EINVAL; /* unknown cookie type */ |
276 |
break; |
277 |
} |
278 |
|
279 |
/* Take care of synchronous response, if any */ |
280 |
NG_RESPOND_MSG(error, node, item, resp); |
281 |
/* Free the message and return */ |
282 |
NG_FREE_MSG(msg); |
283 |
return(error); |
284 |
} |
285 |
|
286 |
/* |
287 |
* Receive data, and do something with it. |
288 |
* Actually we receive a queue item which holds the data. |
289 |
* If we free the item it will also free the data unless we have |
290 |
* previously disassociated it using the NGI_GET_M() macro. |
291 |
* Possibly send it out on another link after processing. |
292 |
* Possibly do something different if it comes from different |
293 |
* hooks. The caller will never free m, so if we use up this data or |
294 |
* abort we must free it. |
295 |
* |
296 |
* If we want, we may decide to force this data to be queued and reprocessed |
297 |
* at the netgraph NETISR time. |
298 |
* We would do that by setting the HK_QUEUE flag on our hook. We would do that |
299 |
* in the connect() method. |
300 |
*/ |
301 |
|
302 |
struct ether_vlan_stack_entry { |
303 |
uint16_t proto; |
304 |
uint16_t tag; |
305 |
} __packed; |
306 |
|
307 |
struct ether_vlan_stack_header { |
308 |
uint8_t dst[ETHER_ADDR_LEN]; |
309 |
uint8_t src[ETHER_ADDR_LEN]; |
310 |
struct ether_vlan_stack_entry vlan_stack[1]; |
311 |
} __packed; |
312 |
|
313 |
static int |
314 |
ng_vlanrotate_gcd(int a, int b) |
315 |
{ |
316 |
if (b == 0) |
317 |
return a; |
318 |
else |
319 |
return ng_vlanrotate_gcd(b, a % b); |
320 |
} |
321 |
|
322 |
#define COPY_VLAN_KEEP_QOS(dst,src) { \ |
323 |
(dst).proto = (src).proto; \ |
324 |
(dst).tag &= ~htons(EVL_VLID_MASK); \ |
325 |
(dst).tag |= (src).tag & htons(EVL_VLID_MASK); \ |
326 |
} while(0) |
327 |
|
328 |
static void |
329 |
ng_vlanrotate_rotate(struct ether_vlan_stack_entry arr[], int d, int n) |
330 |
{ |
331 |
int i, j, k; |
332 |
struct ether_vlan_stack_entry temp; |
333 |
|
334 |
/* for each comensurable slice */ |
335 |
for (i = ng_vlanrotate_gcd(d, n); i-- > 0;) { |
336 |
/* rotate left aka downwards */ |
337 |
COPY_VLAN_KEEP_QOS(temp, arr[i]); |
338 |
j = i; |
339 |
|
340 |
while (1) { |
341 |
k = j + d; |
342 |
if (k >= n) |
343 |
k = k - n; |
344 |
if (k == i) |
345 |
break; |
346 |
COPY_VLAN_KEEP_QOS(arr[j], arr[k]); |
347 |
j = k; |
348 |
} |
349 |
|
350 |
COPY_VLAN_KEEP_QOS(arr[j], temp); |
351 |
} |
352 |
} |
353 |
|
354 |
#undef COPY_VLAN_KEEP_QOS |
355 |
|
356 |
static int |
357 |
ng_vlanrotate_rcvdata(hook_p hook, item_p item) |
358 |
{ |
359 |
const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); |
360 |
struct mbuf *m = NULL; |
361 |
hook_p dst_hook; |
362 |
int8_t rotate; |
363 |
int8_t vlans = 0; |
364 |
int error = ENOSYS; |
365 |
struct ether_vlan_stack_header *evsh; |
366 |
|
367 |
NGI_GET_M(item, m); |
368 |
|
369 |
if(hook == vrp->ordered_hook) { |
370 |
rotate = + vrp->conf.rot; |
371 |
dst_hook = vrp->original_hook; |
372 |
} else if(hook == vrp->original_hook) { |
373 |
rotate = - vrp->conf.rot; |
374 |
dst_hook = vrp->ordered_hook; |
375 |
} else { |
376 |
dst_hook = vrp->original_hook; |
377 |
goto send; /* everything else goes out unmodified */ |
378 |
} |
379 |
|
380 |
if(dst_hook == NULL) { |
381 |
error = ENETDOWN; |
382 |
goto fail; |
383 |
} |
384 |
|
385 |
/* count the vlans */ |
386 |
for(vlans = 0; vlans <= NG_VLANROTATE_MAX_VLANS; vlans++) { |
387 |
size_t expected_len = sizeof(struct ether_vlan_stack_header) + vlans * sizeof(struct ether_vlan_stack_entry); |
388 |
|
389 |
if (m->m_len < expected_len) { |
390 |
m = m_pullup(m, expected_len); |
391 |
if (m == NULL) { |
392 |
error = EINVAL; |
393 |
goto fail; |
394 |
} |
395 |
} |
396 |
|
397 |
evsh = mtod(m, struct ether_vlan_stack_header *); |
398 |
switch(ntohs(evsh->vlan_stack[vlans].proto)) { |
399 |
case ETHERTYPE_VLAN: |
400 |
case ETHERTYPE_QINQ: |
401 |
case 0x9100: |
402 |
break; |
403 |
default: |
404 |
goto out; |
405 |
} |
406 |
} |
407 |
out: |
408 |
if(vlans > vrp->conf.max || vlans >= NG_VLANROTATE_MAX_VLANS) { |
409 |
vrp->stat.excessive++; |
410 |
dst_hook = vrp->excessive_hook; |
411 |
goto send; |
412 |
} |
413 |
|
414 |
if((vlans < vrp->conf.min) || (vlans <= abs(rotate))) { |
415 |
vrp->stat.incomplete++; |
416 |
dst_hook = vrp->incomplete_hook; |
417 |
goto send; |
418 |
} |
419 |
vrp->stat.histogram[vlans]++; |
420 |
|
421 |
/* 01234 5 vlans |
422 |
* ----- |
423 |
* 34012 +2 rotate |
424 |
* 12340 +4 rotate |
425 |
* 12340 -1 rotate |
426 |
*/ |
427 |
if(rotate == 0) { |
428 |
/* do nothing */ |
429 |
} else if(rotate > 0) { |
430 |
ng_vlanrotate_rotate(evsh->vlan_stack, rotate, vlans); |
431 |
} else { |
432 |
ng_vlanrotate_rotate(evsh->vlan_stack, vlans + rotate, vlans); |
433 |
} |
434 |
|
435 |
send: |
436 |
if(dst_hook == NULL) goto fail; |
437 |
NG_FWD_NEW_DATA(error, item, dst_hook, m); |
438 |
return 0; |
439 |
|
440 |
fail: |
441 |
vrp->stat.drops ++; |
442 |
if(m != NULL) m_freem(m); |
443 |
NG_FREE_ITEM(item); |
444 |
return (error); |
445 |
} |
446 |
|
447 |
/* |
448 |
* Do local shutdown processing.. |
449 |
* All our links and the name have already been removed. |
450 |
*/ |
451 |
static int |
452 |
ng_vlanrotate_shutdown(node_p node) |
453 |
{ |
454 |
const vlanrotate_p vrp = NG_NODE_PRIVATE(node); |
455 |
|
456 |
NG_NODE_SET_PRIVATE(node, NULL); |
457 |
NG_NODE_UNREF(node); |
458 |
free(vrp, M_NETGRAPH); |
459 |
|
460 |
return (0); |
461 |
} |
462 |
|
463 |
/* |
464 |
* Hook disconnection |
465 |
* |
466 |
* For this type, removal of the last link destroys the node |
467 |
*/ |
468 |
static int |
469 |
ng_vlanrotate_disconnect(hook_p hook) |
470 |
{ |
471 |
const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); |
472 |
|
473 |
if(vrp->original_hook == hook) vrp->original_hook = NULL; |
474 |
if(vrp->ordered_hook == hook) vrp->ordered_hook = NULL; |
475 |
if(vrp->excessive_hook == hook) vrp->excessive_hook = NULL; |
476 |
if(vrp->incomplete_hook == hook) vrp->incomplete_hook = NULL; |
477 |
|
478 |
if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) |
479 |
&& (NG_NODE_IS_VALID(NG_HOOK_NODE(hook)))) /* already shutting down? */ |
480 |
ng_rmnode_self(NG_HOOK_NODE(hook)); |
481 |
return (0); |
482 |
} |
483 |
|