1 /*-
2 * Copyright (c) 2010 Maxim Ignatenko <gelraen.ua@gmail.com>
3 * Copyright (c) 2015 Dmitry Vagin <daemon.hammer@ya.ru>
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 *
27 */
28
29 #include <sys/cdefs.h>
30 __FBSDID("$FreeBSD$");
31
32 #include <sys/param.h>
33 #include <sys/systm.h>
34 #include <sys/kernel.h>
35 #include <sys/endian.h>
36 #include <sys/malloc.h>
37 #include <sys/mbuf.h>
38
39 #include <net/bpf.h>
40 #include <net/ethernet.h>
41
42 #include <netgraph/ng_message.h>
43 #include <netgraph/ng_parse.h>
44 #include <netgraph/netgraph.h>
45
46 #include <netgraph/ng_patch.h>
47
48 /* private data */
49 struct ng_patch_priv {
50 hook_p in;
51 hook_p out;
52 uint8_t dlt; /* DLT_XXX from bpf.h */
53 struct ng_patch_stats stats;
54 struct ng_patch_config *conf;
55 };
56
57 typedef struct ng_patch_priv *priv_p;
58
59 /* Netgraph methods */
60 static ng_constructor_t ng_patch_constructor;
61 static ng_rcvmsg_t ng_patch_rcvmsg;
62 static ng_shutdown_t ng_patch_shutdown;
63 static ng_newhook_t ng_patch_newhook;
64 static ng_rcvdata_t ng_patch_rcvdata;
65 static ng_disconnect_t ng_patch_disconnect;
66
67 #define ERROUT(x) { error = (x); goto done; }
68
69 static int
70 ng_patch_config_getlen(const struct ng_parse_type *type,
71 const u_char *start, const u_char *buf)
72 {
73 const struct ng_patch_config *conf;
74
75 conf = (const struct ng_patch_config *)(buf -
76 offsetof(struct ng_patch_config, ops));
77
78 return (conf->count);
79 }
80
81 static const struct ng_parse_struct_field ng_patch_op_type_fields[]
82 = NG_PATCH_OP_TYPE;
83 static const struct ng_parse_type ng_patch_op_type = {
84 &ng_parse_struct_type,
85 &ng_patch_op_type_fields
86 };
87
88 static const struct ng_parse_array_info ng_patch_ops_array_info = {
89 &ng_patch_op_type,
90 &ng_patch_config_getlen
91 };
92 static const struct ng_parse_type ng_patch_ops_array_type = {
93 &ng_parse_array_type,
94 &ng_patch_ops_array_info
95 };
96
97 static const struct ng_parse_struct_field ng_patch_config_type_fields[]
98 = NG_PATCH_CONFIG_TYPE;
99 static const struct ng_parse_type ng_patch_config_type = {
100 &ng_parse_struct_type,
101 &ng_patch_config_type_fields
102 };
103
104 static const struct ng_parse_struct_field ng_patch_stats_fields[]
105 = NG_PATCH_STATS_TYPE;
106 static const struct ng_parse_type ng_patch_stats_type = {
107 &ng_parse_struct_type,
108 &ng_patch_stats_fields
109 };
110
111 static const struct ng_cmdlist ng_patch_cmdlist[] = {
112 {
113 NGM_PATCH_COOKIE,
114 NGM_PATCH_GETDLT,
115 "getdlt",
116 NULL,
117 &ng_parse_uint8_type
118 },
119 {
120 NGM_PATCH_COOKIE,
121 NGM_PATCH_SETDLT,
122 "setdlt",
123 &ng_parse_uint8_type,
124 NULL
125 },
126 {
127 NGM_PATCH_COOKIE,
128 NGM_PATCH_GETCONFIG,
129 "getconfig",
130 NULL,
131 &ng_patch_config_type
132 },
133 {
134 NGM_PATCH_COOKIE,
135 NGM_PATCH_SETCONFIG,
136 "setconfig",
137 &ng_patch_config_type,
138 NULL
139 },
140 {
141 NGM_PATCH_COOKIE,
142 NGM_PATCH_GET_STATS,
143 "getstats",
144 NULL,
145 &ng_patch_stats_type
146 },
147 {
148 NGM_PATCH_COOKIE,
149 NGM_PATCH_CLR_STATS,
150 "clrstats",
151 NULL,
152 NULL
153 },
154 {
155 NGM_PATCH_COOKIE,
156 NGM_PATCH_GETCLR_STATS,
157 "getclrstats",
158 NULL,
159 &ng_patch_stats_type
160 },
161 { 0 }
162 };
163
164 static struct ng_type typestruct = {
165 .version = NG_ABI_VERSION,
166 .name = NG_PATCH_NODE_TYPE,
167 .constructor = ng_patch_constructor,
168 .rcvmsg = ng_patch_rcvmsg,
169 .shutdown = ng_patch_shutdown,
170 .newhook = ng_patch_newhook,
171 .rcvdata = ng_patch_rcvdata,
172 .disconnect = ng_patch_disconnect,
173 .cmdlist = ng_patch_cmdlist,
174 };
175
176 NETGRAPH_INIT(patch, &typestruct);
177
178 static int
179 ng_patch_constructor(node_p node)
180 {
181 priv_p privdata;
182
183 privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAITOK | M_ZERO);
184 privdata->dlt = DLT_RAW;
185
186 NG_NODE_SET_PRIVATE(node, privdata);
187
188 return (0);
189 }
190
191 static int
192 ng_patch_newhook(node_p node, hook_p hook, const char *name)
193 {
194 const priv_p privp = NG_NODE_PRIVATE(node);
195
196 if (strncmp(name, NG_PATCH_HOOK_IN, strlen(NG_PATCH_HOOK_IN)) == 0) {
197 privp->in = hook;
198 } else if (strncmp(name, NG_PATCH_HOOK_OUT,
199 strlen(NG_PATCH_HOOK_OUT)) == 0) {
200 privp->out = hook;
201 } else
202 return (EINVAL);
203
204 return (0);
205 }
206
207 static int
208 ng_patch_rcvmsg(node_p node, item_p item, hook_p lasthook)
209 {
210 const priv_p privp = NG_NODE_PRIVATE(node);
211 struct ng_patch_config *conf, *newconf;
212 struct ng_mesg *msg;
213 struct ng_mesg *resp = NULL;
214 int i, error = 0;
215
216 NGI_GET_MSG(item, msg);
217
218 if (msg->header.typecookie != NGM_PATCH_COOKIE)
219 ERROUT(EINVAL);
220
221 switch (msg->header.cmd)
222 {
223 case NGM_PATCH_GETCONFIG:
224 if (privp->conf == NULL)
225 ERROUT(0);
226
227 NG_MKRESPONSE(resp, msg,
228 NG_PATCH_CONF_SIZE(privp->conf->count), M_WAITOK);
229
230 if (resp == NULL)
231 ERROUT(ENOMEM);
232
233 bcopy(privp->conf, resp->data,
234 NG_PATCH_CONF_SIZE(privp->conf->count));
235
236 conf = (struct ng_patch_config *) resp->data;
237
238 for (i = 0; i < conf->count; i++) {
239 switch (conf->ops[i].length)
240 {
241 case 1:
242 conf->ops[i].val.v8 = conf->ops[i].val.v1;
243 break;
244 case 2:
245 conf->ops[i].val.v8 = conf->ops[i].val.v2;
246 break;
247 case 4:
248 conf->ops[i].val.v8 = conf->ops[i].val.v4;
249 break;
250 case 8:
251 break;
252 }
253 }
254
255 break;
256
257 case NGM_PATCH_SETCONFIG:
258 conf = (struct ng_patch_config *) msg->data;
259
260 if (msg->header.arglen < sizeof(struct ng_patch_config) ||
261 msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count))
262 ERROUT(EINVAL);
263
264 for (i = 0; i < conf->count; i++) {
265 switch (conf->ops[i].length)
266 {
267 case 1:
268 conf->ops[i].val.v1 = (uint8_t) conf->ops[i].val.v8;
269 break;
270 case 2:
271 conf->ops[i].val.v2 = (uint16_t) conf->ops[i].val.v8;
272 break;
273 case 4:
274 conf->ops[i].val.v4 = (uint32_t) conf->ops[i].val.v8;
275 break;
276 case 8:
277 break;
278 default:
279 ERROUT(EINVAL);
280 }
281 }
282
283 conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6;
284 conf->relative_offset = !!conf->relative_offset;
285
286 newconf = malloc(NG_PATCH_CONF_SIZE(conf->count), M_NETGRAPH, M_WAITOK | M_ZERO);
287
288 bcopy(conf, newconf, NG_PATCH_CONF_SIZE(conf->count));
289
290 if (privp->conf)
291 free(privp->conf, M_NETGRAPH);
292
293 privp->conf = newconf;
294
295 break;
296
297 case NGM_PATCH_GET_STATS:
298 case NGM_PATCH_CLR_STATS:
299 case NGM_PATCH_GETCLR_STATS:
300 if (msg->header.cmd != NGM_PATCH_CLR_STATS) {
301 NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK);
302
303 if (resp == NULL)
304 ERROUT(ENOMEM);
305
306 bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats));
307 }
308
309 if (msg->header.cmd != NGM_PATCH_GET_STATS)
310 bzero(&(privp->stats), sizeof(struct ng_patch_stats));
311
312 break;
313
314 case NGM_PATCH_GETDLT:
315 NG_MKRESPONSE(resp, msg, sizeof(uint8_t), M_WAITOK);
316
317 if (resp == NULL)
318 ERROUT(ENOMEM);
319
320 *((uint8_t *) resp->data) = privp->dlt;
321
322 break;
323
324 case NGM_PATCH_SETDLT:
325 if (msg->header.arglen != sizeof(uint8_t))
326 ERROUT(EINVAL);
327
328 switch (*(uint8_t *) msg->data)
329 {
330 case DLT_EN10MB:
331 case DLT_RAW:
332 privp->dlt = *(uint8_t *) msg->data;
333 break;
334
335 default:
336 ERROUT(EINVAL);
337 }
338
339 break;
340
341 default:
342 ERROUT(EINVAL);
343 }
344
345 done:
346 NG_RESPOND_MSG(error, node, item, resp);
347 NG_FREE_MSG(msg);
348
349 return (error);
350 }
351
352 static void
353 do_patch(priv_p privp, struct mbuf *m, int global_offset)
354 {
355 int i, offset, patched = 0;
356 union ng_patch_op_val val;
357
358 for (i = 0; i < privp->conf->count; i++) {
359 offset = global_offset + privp->conf->ops[i].offset;
360
361 if (offset + privp->conf->ops[i].length > m->m_pkthdr.len)
362 continue;
363
364 /* for "=" operation we don't need to copy data from mbuf */
365 if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET)
366 m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
367
368 switch (privp->conf->ops[i].length)
369 {
370 case 1:
371 switch (privp->conf->ops[i].mode)
372 {
373 case NG_PATCH_MODE_SET:
374 val.v1 = privp->conf->ops[i].val.v1;
375 break;
376 case NG_PATCH_MODE_ADD:
377 val.v1 += privp->conf->ops[i].val.v1;
378 break;
379 case NG_PATCH_MODE_SUB:
380 val.v1 -= privp->conf->ops[i].val.v1;
381 break;
382 case NG_PATCH_MODE_MUL:
383 val.v1 *= privp->conf->ops[i].val.v1;
384 break;
385 case NG_PATCH_MODE_DIV:
386 val.v1 /= privp->conf->ops[i].val.v1;
387 break;
388 case NG_PATCH_MODE_NEG:
389 *((int8_t *) &val) = - *((int8_t *) &val);
390 break;
391 case NG_PATCH_MODE_AND:
392 val.v1 &= privp->conf->ops[i].val.v1;
393 break;
394 case NG_PATCH_MODE_OR:
395 val.v1 |= privp->conf->ops[i].val.v1;
396 break;
397 case NG_PATCH_MODE_XOR:
398 val.v1 ^= privp->conf->ops[i].val.v1;
399 break;
400 case NG_PATCH_MODE_SHL:
401 val.v1 <<= privp->conf->ops[i].val.v1;
402 break;
403 case NG_PATCH_MODE_SHR:
404 val.v1 >>= privp->conf->ops[i].val.v1;
405 break;
406 }
407 break;
408
409 case 2:
410 val.v2 = ntohs(val.v2);
411
412 switch (privp->conf->ops[i].mode)
413 {
414 case NG_PATCH_MODE_SET:
415 val.v2 = privp->conf->ops[i].val.v2;
416 break;
417 case NG_PATCH_MODE_ADD:
418 val.v2 += privp->conf->ops[i].val.v2;
419 break;
420 case NG_PATCH_MODE_SUB:
421 val.v2 -= privp->conf->ops[i].val.v2;
422 break;
423 case NG_PATCH_MODE_MUL:
424 val.v2 *= privp->conf->ops[i].val.v2;
425 break;
426 case NG_PATCH_MODE_DIV:
427 val.v2 /= privp->conf->ops[i].val.v2;
428 break;
429 case NG_PATCH_MODE_NEG:
430 *((int16_t *) &val) = - *((int16_t *) &val);
431 break;
432 case NG_PATCH_MODE_AND:
433 val.v2 &= privp->conf->ops[i].val.v2;
434 break;
435 case NG_PATCH_MODE_OR:
436 val.v2 |= privp->conf->ops[i].val.v2;
437 break;
438 case NG_PATCH_MODE_XOR:
439 val.v2 ^= privp->conf->ops[i].val.v2;
440 break;
441 case NG_PATCH_MODE_SHL:
442 val.v2 <<= privp->conf->ops[i].val.v2;
443 break;
444 case NG_PATCH_MODE_SHR:
445 val.v2 >>= privp->conf->ops[i].val.v2;
446 break;
447 }
448
449 val.v2 = htons(val.v2);
450
451 break;
452
453 case 4:
454 val.v4 = ntohl(val.v4);
455
456 switch (privp->conf->ops[i].mode)
457 {
458 case NG_PATCH_MODE_SET:
459 val.v4 = privp->conf->ops[i].val.v4;
460 break;
461 case NG_PATCH_MODE_ADD:
462 val.v4 += privp->conf->ops[i].val.v4;
463 break;
464 case NG_PATCH_MODE_SUB:
465 val.v4 -= privp->conf->ops[i].val.v4;
466 break;
467 case NG_PATCH_MODE_MUL:
468 val.v4 *= privp->conf->ops[i].val.v4;
469 break;
470 case NG_PATCH_MODE_DIV:
471 val.v4 /= privp->conf->ops[i].val.v4;
472 break;
473 case NG_PATCH_MODE_NEG:
474 *((int32_t *) &val) = - *((int32_t *) &val);
475 break;
476 case NG_PATCH_MODE_AND:
477 val.v4 &= privp->conf->ops[i].val.v4;
478 break;
479 case NG_PATCH_MODE_OR:
480 val.v4 |= privp->conf->ops[i].val.v4;
481 break;
482 case NG_PATCH_MODE_XOR:
483 val.v4 ^= privp->conf->ops[i].val.v4;
484 break;
485 case NG_PATCH_MODE_SHL:
486 val.v4 <<= privp->conf->ops[i].val.v4;
487 break;
488 case NG_PATCH_MODE_SHR:
489 val.v4 >>= privp->conf->ops[i].val.v4;
490 break;
491 }
492
493 val.v4 = htonl(val.v4);
494
495 break;
496
497 case 8:
498 val.v8 = be64toh(val.v8);
499
500 switch (privp->conf->ops[i].mode)
501 {
502 case NG_PATCH_MODE_SET:
503 val.v8 = privp->conf->ops[i].val.v8;
504 break;
505 case NG_PATCH_MODE_ADD:
506 val.v8 += privp->conf->ops[i].val.v8;
507 break;
508 case NG_PATCH_MODE_SUB:
509 val.v8 -= privp->conf->ops[i].val.v8;
510 break;
511 case NG_PATCH_MODE_MUL:
512 val.v8 *= privp->conf->ops[i].val.v8;
513 break;
514 case NG_PATCH_MODE_DIV:
515 val.v8 /= privp->conf->ops[i].val.v8;
516 break;
517 case NG_PATCH_MODE_NEG:
518 *((int64_t *) &val) = - *((int64_t *) &val);
519 break;
520 case NG_PATCH_MODE_AND:
521 val.v8 &= privp->conf->ops[i].val.v8;
522 break;
523 case NG_PATCH_MODE_OR:
524 val.v8 |= privp->conf->ops[i].val.v8;
525 break;
526 case NG_PATCH_MODE_XOR:
527 val.v8 ^= privp->conf->ops[i].val.v8;
528 break;
529 case NG_PATCH_MODE_SHL:
530 val.v8 <<= privp->conf->ops[i].val.v8;
531 break;
532 case NG_PATCH_MODE_SHR:
533 val.v8 >>= privp->conf->ops[i].val.v8;
534 break;
535 }
536
537 val.v8 = htobe64(val.v8);
538
539 break;
540 }
541
542 m_copyback(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
543 patched = 1;
544 }
545
546 if (patched)
547 privp->stats.patched++;
548 }
549
550 static int
551 ng_patch_rcvdata(hook_p hook, item_p item)
552 {
553 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
554 struct mbuf *m;
555 hook_p out;
556 int pullup_len = 0;
557 int error = 0;
558
559 priv->stats.received++;
560
561 NGI_GET_M(item, m);
562
563 #define PULLUP_CHECK(mbuf, length) do { \
564 pullup_len += length; \
565 if (((mbuf)->m_pkthdr.len < pullup_len) || \
566 (pullup_len > MHLEN)) { \
567 error = EINVAL; \
568 goto bypass; \
569 } \
570 if ((mbuf)->m_len < pullup_len && \
571 (((mbuf) = m_pullup((mbuf), pullup_len)) == NULL)) { \
572 error = ENOBUFS; \
573 goto drop; \
574 } \
575 } while (0)
576
577 if (priv->conf && hook == priv->in &&
578 m && (m->m_flags & M_PKTHDR)) {
579
580 m = m_unshare(m, M_NOWAIT);
581
582 if (m == NULL)
583 ERROUT(ENOMEM);
584
585 if (priv->conf->relative_offset) {
586 struct ether_header *eh;
587 struct ng_patch_vlan_header *vh;
588 uint16_t etype;
589
590 switch (priv->dlt)
591 {
592 case DLT_EN10MB:
593 PULLUP_CHECK(m, sizeof(struct ether_header));
594 eh = mtod(m, struct ether_header *);
595 etype = ntohs(eh->ether_type);
596
597 for (;;) { /* QinQ support */
598 switch (etype)
599 {
600 case 0x8100:
601 case 0x88A8:
602 case 0x9100:
603 PULLUP_CHECK(m, sizeof(struct ng_patch_vlan_header));
604 vh = (struct ng_patch_vlan_header *) mtodo(m,
605 pullup_len - sizeof(struct ng_patch_vlan_header));
606 etype = ntohs(vh->etype);
607 break;
608
609 default:
610 goto loopend;
611 }
612 }
613 loopend:
614 break;
615
616 case DLT_RAW:
617 break;
618
619 default:
620 ERROUT(EINVAL);
621 }
622 }
623
624 do_patch(priv, m, pullup_len);
625
626 m->m_pkthdr.csum_flags |= priv->conf->csum_flags;
627 }
628
629 #undef PULLUP_CHECK
630
631 bypass:
632 out = NULL;
633
634 if (hook == priv->in) {
635 /* return frames on 'in' hook if 'out' not connected */
636 out = priv->out ? priv->out : priv->in;
637 } else if (hook == priv->out && priv->in) {
638 /* pass frames on 'out' hook if 'in' connected */
639 out = priv->in;
640 }
641
642 if (out == NULL)
643 ERROUT(0);
644
645 NG_FWD_NEW_DATA(error, item, out, m);
646
647 return (error);
648
649 done:
650 drop:
651 NG_FREE_ITEM(item);
652 NG_FREE_M(m);
653
654 priv->stats.dropped++;
655
656 return (error);
657 }
658
659 static int
660 ng_patch_shutdown(node_p node)
661 {
662 const priv_p privdata = NG_NODE_PRIVATE(node);
663
664 NG_NODE_SET_PRIVATE(node, NULL);
665 NG_NODE_UNREF(node);
666
667 if (privdata->conf != NULL)
668 free(privdata->conf, M_NETGRAPH);
669
670 free(privdata, M_NETGRAPH);
671
672 return (0);
673 }
674
675 static int
676 ng_patch_disconnect(hook_p hook)
677 {
678 priv_p priv;
679
680 priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
681
682 if (hook == priv->in) {
683 priv->in = NULL;
684 }
685
686 if (hook == priv->out) {
687 priv->out = NULL;
688 }
689
690 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 &&
691 NG_NODE_IS_VALID(NG_HOOK_NODE(hook))) /* already shutting down? */
692 ng_rmnode_self(NG_HOOK_NODE(hook));
693
694 return (0);
695 }
Cache object: f0b45dcc93551c7ee00d095ad24349b7
|