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