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
69 #define ERROUT(x) { error = (x); goto done; }
70
71 static int
72 ng_patch_config_getlen(const struct ng_parse_type *type,
73 const u_char *start, const u_char *buf)
74 {
75 const struct ng_patch_config *conf;
76
77 conf = (const struct ng_patch_config *)(buf -
78 offsetof(struct ng_patch_config, ops));
79
80 return (conf->count);
81 }
82
83 static const struct ng_parse_struct_field ng_patch_op_type_fields[]
84 = NG_PATCH_OP_TYPE;
85 static const struct ng_parse_type ng_patch_op_type = {
86 &ng_parse_struct_type,
87 &ng_patch_op_type_fields
88 };
89
90 static const struct ng_parse_array_info ng_patch_ops_array_info = {
91 &ng_patch_op_type,
92 &ng_patch_config_getlen
93 };
94 static const struct ng_parse_type ng_patch_ops_array_type = {
95 &ng_parse_array_type,
96 &ng_patch_ops_array_info
97 };
98
99 static const struct ng_parse_struct_field ng_patch_config_type_fields[]
100 = NG_PATCH_CONFIG_TYPE;
101 static const struct ng_parse_type ng_patch_config_type = {
102 &ng_parse_struct_type,
103 &ng_patch_config_type_fields
104 };
105
106 static const struct ng_parse_struct_field ng_patch_stats_fields[]
107 = NG_PATCH_STATS_TYPE;
108 static const struct ng_parse_type ng_patch_stats_type = {
109 &ng_parse_struct_type,
110 &ng_patch_stats_fields
111 };
112
113 static const struct ng_cmdlist ng_patch_cmdlist[] = {
114 {
115 NGM_PATCH_COOKIE,
116 NGM_PATCH_GETDLT,
117 "getdlt",
118 NULL,
119 &ng_parse_uint8_type
120 },
121 {
122 NGM_PATCH_COOKIE,
123 NGM_PATCH_SETDLT,
124 "setdlt",
125 &ng_parse_uint8_type,
126 NULL
127 },
128 {
129 NGM_PATCH_COOKIE,
130 NGM_PATCH_GETCONFIG,
131 "getconfig",
132 NULL,
133 &ng_patch_config_type
134 },
135 {
136 NGM_PATCH_COOKIE,
137 NGM_PATCH_SETCONFIG,
138 "setconfig",
139 &ng_patch_config_type,
140 NULL
141 },
142 {
143 NGM_PATCH_COOKIE,
144 NGM_PATCH_GET_STATS,
145 "getstats",
146 NULL,
147 &ng_patch_stats_type
148 },
149 {
150 NGM_PATCH_COOKIE,
151 NGM_PATCH_CLR_STATS,
152 "clrstats",
153 NULL,
154 NULL
155 },
156 {
157 NGM_PATCH_COOKIE,
158 NGM_PATCH_GETCLR_STATS,
159 "getclrstats",
160 NULL,
161 &ng_patch_stats_type
162 },
163 { 0 }
164 };
165
166 static struct ng_type typestruct = {
167 .version = NG_ABI_VERSION,
168 .name = NG_PATCH_NODE_TYPE,
169 .constructor = ng_patch_constructor,
170 .rcvmsg = ng_patch_rcvmsg,
171 .shutdown = ng_patch_shutdown,
172 .newhook = ng_patch_newhook,
173 .rcvdata = ng_patch_rcvdata,
174 .disconnect = ng_patch_disconnect,
175 .cmdlist = ng_patch_cmdlist,
176 };
177
178 NETGRAPH_INIT(patch, &typestruct);
179
180 static int
181 ng_patch_constructor(node_p node)
182 {
183 priv_p privdata;
184
185 privdata = malloc(sizeof(*privdata), M_NETGRAPH, M_WAITOK | M_ZERO);
186 privdata->dlt = DLT_RAW;
187
188 NG_NODE_SET_PRIVATE(node, privdata);
189
190 return (0);
191 }
192
193 static int
194 ng_patch_newhook(node_p node, hook_p hook, const char *name)
195 {
196 const priv_p privp = NG_NODE_PRIVATE(node);
197
198 if (strncmp(name, NG_PATCH_HOOK_IN, strlen(NG_PATCH_HOOK_IN)) == 0) {
199 privp->in = hook;
200 } else if (strncmp(name, NG_PATCH_HOOK_OUT,
201 strlen(NG_PATCH_HOOK_OUT)) == 0) {
202 privp->out = hook;
203 } else
204 return (EINVAL);
205
206 return (0);
207 }
208
209 static int
210 ng_patch_rcvmsg(node_p node, item_p item, hook_p lasthook)
211 {
212 const priv_p privp = NG_NODE_PRIVATE(node);
213 struct ng_patch_config *conf, *newconf;
214 struct ng_mesg *msg;
215 struct ng_mesg *resp = NULL;
216 int i, error = 0;
217
218 NGI_GET_MSG(item, msg);
219
220 if (msg->header.typecookie != NGM_PATCH_COOKIE)
221 ERROUT(EINVAL);
222
223 switch (msg->header.cmd)
224 {
225 case NGM_PATCH_GETCONFIG:
226 if (privp->conf == NULL)
227 ERROUT(0);
228
229 NG_MKRESPONSE(resp, msg,
230 NG_PATCH_CONF_SIZE(privp->conf->count), M_WAITOK);
231
232 if (resp == NULL)
233 ERROUT(ENOMEM);
234
235 bcopy(privp->conf, resp->data,
236 NG_PATCH_CONF_SIZE(privp->conf->count));
237
238 conf = (struct ng_patch_config *) resp->data;
239
240 for (i = 0; i < conf->count; i++) {
241 switch (conf->ops[i].length)
242 {
243 case 1:
244 conf->ops[i].val.v8 = conf->ops[i].val.v1;
245 break;
246 case 2:
247 conf->ops[i].val.v8 = conf->ops[i].val.v2;
248 break;
249 case 4:
250 conf->ops[i].val.v8 = conf->ops[i].val.v4;
251 break;
252 case 8:
253 break;
254 }
255 }
256
257 break;
258
259 case NGM_PATCH_SETCONFIG:
260 conf = (struct ng_patch_config *) msg->data;
261
262 if (msg->header.arglen < sizeof(struct ng_patch_config) ||
263 msg->header.arglen < NG_PATCH_CONF_SIZE(conf->count))
264 ERROUT(EINVAL);
265
266 for (i = 0; i < conf->count; i++) {
267 switch (conf->ops[i].length)
268 {
269 case 1:
270 conf->ops[i].val.v1 = (uint8_t) conf->ops[i].val.v8;
271 break;
272 case 2:
273 conf->ops[i].val.v2 = (uint16_t) conf->ops[i].val.v8;
274 break;
275 case 4:
276 conf->ops[i].val.v4 = (uint32_t) conf->ops[i].val.v8;
277 break;
278 case 8:
279 break;
280 default:
281 ERROUT(EINVAL);
282 }
283 }
284
285 conf->csum_flags &= NG_PATCH_CSUM_IPV4|NG_PATCH_CSUM_IPV6;
286 conf->relative_offset = !!conf->relative_offset;
287
288 newconf = malloc(NG_PATCH_CONF_SIZE(conf->count), M_NETGRAPH, M_WAITOK | M_ZERO);
289
290 bcopy(conf, newconf, NG_PATCH_CONF_SIZE(conf->count));
291
292 if (privp->conf)
293 free(privp->conf, M_NETGRAPH);
294
295 privp->conf = newconf;
296
297 break;
298
299 case NGM_PATCH_GET_STATS:
300 case NGM_PATCH_CLR_STATS:
301 case NGM_PATCH_GETCLR_STATS:
302 if (msg->header.cmd != NGM_PATCH_CLR_STATS) {
303 NG_MKRESPONSE(resp, msg, sizeof(struct ng_patch_stats), M_WAITOK);
304
305 if (resp == NULL)
306 ERROUT(ENOMEM);
307
308 bcopy(&(privp->stats), resp->data, sizeof(struct ng_patch_stats));
309 }
310
311 if (msg->header.cmd != NGM_PATCH_GET_STATS)
312 bzero(&(privp->stats), sizeof(struct ng_patch_stats));
313
314 break;
315
316 case NGM_PATCH_GETDLT:
317 NG_MKRESPONSE(resp, msg, sizeof(uint8_t), M_WAITOK);
318
319 if (resp == NULL)
320 ERROUT(ENOMEM);
321
322 *((uint8_t *) resp->data) = privp->dlt;
323
324 break;
325
326 case NGM_PATCH_SETDLT:
327 if (msg->header.arglen != sizeof(uint8_t))
328 ERROUT(EINVAL);
329
330 switch (*(uint8_t *) msg->data)
331 {
332 case DLT_EN10MB:
333 case DLT_RAW:
334 privp->dlt = *(uint8_t *) msg->data;
335 break;
336
337 default:
338 ERROUT(EINVAL);
339 }
340
341 break;
342
343 default:
344 ERROUT(EINVAL);
345 }
346
347 done:
348 NG_RESPOND_MSG(error, node, item, resp);
349 NG_FREE_MSG(msg);
350
351 return (error);
352 }
353
354 static void
355 do_patch(priv_p privp, struct mbuf *m, int global_offset)
356 {
357 int i, offset, patched = 0;
358 union ng_patch_op_val val;
359
360 for (i = 0; i < privp->conf->count; i++) {
361 offset = global_offset + privp->conf->ops[i].offset;
362
363 if (offset + privp->conf->ops[i].length > m->m_pkthdr.len)
364 continue;
365
366 /* for "=" operation we don't need to copy data from mbuf */
367 if (privp->conf->ops[i].mode != NG_PATCH_MODE_SET)
368 m_copydata(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
369
370 switch (privp->conf->ops[i].length)
371 {
372 case 1:
373 switch (privp->conf->ops[i].mode)
374 {
375 case NG_PATCH_MODE_SET:
376 val.v1 = privp->conf->ops[i].val.v1;
377 break;
378 case NG_PATCH_MODE_ADD:
379 val.v1 += privp->conf->ops[i].val.v1;
380 break;
381 case NG_PATCH_MODE_SUB:
382 val.v1 -= privp->conf->ops[i].val.v1;
383 break;
384 case NG_PATCH_MODE_MUL:
385 val.v1 *= privp->conf->ops[i].val.v1;
386 break;
387 case NG_PATCH_MODE_DIV:
388 val.v1 /= privp->conf->ops[i].val.v1;
389 break;
390 case NG_PATCH_MODE_NEG:
391 *((int8_t *) &val) = - *((int8_t *) &val);
392 break;
393 case NG_PATCH_MODE_AND:
394 val.v1 &= privp->conf->ops[i].val.v1;
395 break;
396 case NG_PATCH_MODE_OR:
397 val.v1 |= privp->conf->ops[i].val.v1;
398 break;
399 case NG_PATCH_MODE_XOR:
400 val.v1 ^= privp->conf->ops[i].val.v1;
401 break;
402 case NG_PATCH_MODE_SHL:
403 val.v1 <<= privp->conf->ops[i].val.v1;
404 break;
405 case NG_PATCH_MODE_SHR:
406 val.v1 >>= privp->conf->ops[i].val.v1;
407 break;
408 }
409 break;
410
411 case 2:
412 val.v2 = ntohs(val.v2);
413
414 switch (privp->conf->ops[i].mode)
415 {
416 case NG_PATCH_MODE_SET:
417 val.v2 = privp->conf->ops[i].val.v2;
418 break;
419 case NG_PATCH_MODE_ADD:
420 val.v2 += privp->conf->ops[i].val.v2;
421 break;
422 case NG_PATCH_MODE_SUB:
423 val.v2 -= privp->conf->ops[i].val.v2;
424 break;
425 case NG_PATCH_MODE_MUL:
426 val.v2 *= privp->conf->ops[i].val.v2;
427 break;
428 case NG_PATCH_MODE_DIV:
429 val.v2 /= privp->conf->ops[i].val.v2;
430 break;
431 case NG_PATCH_MODE_NEG:
432 *((int16_t *) &val) = - *((int16_t *) &val);
433 break;
434 case NG_PATCH_MODE_AND:
435 val.v2 &= privp->conf->ops[i].val.v2;
436 break;
437 case NG_PATCH_MODE_OR:
438 val.v2 |= privp->conf->ops[i].val.v2;
439 break;
440 case NG_PATCH_MODE_XOR:
441 val.v2 ^= privp->conf->ops[i].val.v2;
442 break;
443 case NG_PATCH_MODE_SHL:
444 val.v2 <<= privp->conf->ops[i].val.v2;
445 break;
446 case NG_PATCH_MODE_SHR:
447 val.v2 >>= privp->conf->ops[i].val.v2;
448 break;
449 }
450
451 val.v2 = htons(val.v2);
452
453 break;
454
455 case 4:
456 val.v4 = ntohl(val.v4);
457
458 switch (privp->conf->ops[i].mode)
459 {
460 case NG_PATCH_MODE_SET:
461 val.v4 = privp->conf->ops[i].val.v4;
462 break;
463 case NG_PATCH_MODE_ADD:
464 val.v4 += privp->conf->ops[i].val.v4;
465 break;
466 case NG_PATCH_MODE_SUB:
467 val.v4 -= privp->conf->ops[i].val.v4;
468 break;
469 case NG_PATCH_MODE_MUL:
470 val.v4 *= privp->conf->ops[i].val.v4;
471 break;
472 case NG_PATCH_MODE_DIV:
473 val.v4 /= privp->conf->ops[i].val.v4;
474 break;
475 case NG_PATCH_MODE_NEG:
476 *((int32_t *) &val) = - *((int32_t *) &val);
477 break;
478 case NG_PATCH_MODE_AND:
479 val.v4 &= privp->conf->ops[i].val.v4;
480 break;
481 case NG_PATCH_MODE_OR:
482 val.v4 |= privp->conf->ops[i].val.v4;
483 break;
484 case NG_PATCH_MODE_XOR:
485 val.v4 ^= privp->conf->ops[i].val.v4;
486 break;
487 case NG_PATCH_MODE_SHL:
488 val.v4 <<= privp->conf->ops[i].val.v4;
489 break;
490 case NG_PATCH_MODE_SHR:
491 val.v4 >>= privp->conf->ops[i].val.v4;
492 break;
493 }
494
495 val.v4 = htonl(val.v4);
496
497 break;
498
499 case 8:
500 val.v8 = be64toh(val.v8);
501
502 switch (privp->conf->ops[i].mode)
503 {
504 case NG_PATCH_MODE_SET:
505 val.v8 = privp->conf->ops[i].val.v8;
506 break;
507 case NG_PATCH_MODE_ADD:
508 val.v8 += privp->conf->ops[i].val.v8;
509 break;
510 case NG_PATCH_MODE_SUB:
511 val.v8 -= privp->conf->ops[i].val.v8;
512 break;
513 case NG_PATCH_MODE_MUL:
514 val.v8 *= privp->conf->ops[i].val.v8;
515 break;
516 case NG_PATCH_MODE_DIV:
517 val.v8 /= privp->conf->ops[i].val.v8;
518 break;
519 case NG_PATCH_MODE_NEG:
520 *((int64_t *) &val) = - *((int64_t *) &val);
521 break;
522 case NG_PATCH_MODE_AND:
523 val.v8 &= privp->conf->ops[i].val.v8;
524 break;
525 case NG_PATCH_MODE_OR:
526 val.v8 |= privp->conf->ops[i].val.v8;
527 break;
528 case NG_PATCH_MODE_XOR:
529 val.v8 ^= privp->conf->ops[i].val.v8;
530 break;
531 case NG_PATCH_MODE_SHL:
532 val.v8 <<= privp->conf->ops[i].val.v8;
533 break;
534 case NG_PATCH_MODE_SHR:
535 val.v8 >>= privp->conf->ops[i].val.v8;
536 break;
537 }
538
539 val.v8 = htobe64(val.v8);
540
541 break;
542 }
543
544 m_copyback(m, offset, privp->conf->ops[i].length, (caddr_t) &val);
545 patched = 1;
546 }
547
548 if (patched)
549 privp->stats.patched++;
550 }
551
552 static int
553 ng_patch_rcvdata(hook_p hook, item_p item)
554 {
555 const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
556 struct mbuf *m;
557 hook_p out;
558 int pullup_len = 0;
559 int error = 0;
560
561 priv->stats.received++;
562
563 NGI_GET_M(item, m);
564
565 #define PULLUP_CHECK(mbuf, length) do { \
566 pullup_len += length; \
567 if (((mbuf)->m_pkthdr.len < pullup_len) || \
568 (pullup_len > MHLEN)) { \
569 error = EINVAL; \
570 goto bypass; \
571 } \
572 if ((mbuf)->m_len < pullup_len && \
573 (((mbuf) = m_pullup((mbuf), pullup_len)) == NULL)) { \
574 error = ENOBUFS; \
575 goto drop; \
576 } \
577 } while (0)
578
579 if (priv->conf && hook == priv->in &&
580 m && (m->m_flags & M_PKTHDR)) {
581
582 m = m_unshare(m, M_NOWAIT);
583
584 if (m == NULL)
585 ERROUT(ENOMEM);
586
587 if (priv->conf->relative_offset) {
588 struct ether_header *eh;
589 struct ng_patch_vlan_header *vh;
590 uint16_t etype;
591
592 switch (priv->dlt)
593 {
594 case DLT_EN10MB:
595 PULLUP_CHECK(m, sizeof(struct ether_header));
596 eh = mtod(m, struct ether_header *);
597 etype = ntohs(eh->ether_type);
598
599 for (;;) { /* QinQ support */
600 switch (etype)
601 {
602 case 0x8100:
603 case 0x88A8:
604 case 0x9100:
605 PULLUP_CHECK(m, sizeof(struct ng_patch_vlan_header));
606 vh = (struct ng_patch_vlan_header *) mtodo(m,
607 pullup_len - sizeof(struct ng_patch_vlan_header));
608 etype = ntohs(vh->etype);
609 break;
610
611 default:
612 goto loopend;
613 }
614 }
615 loopend:
616 break;
617
618 case DLT_RAW:
619 break;
620
621 default:
622 ERROUT(EINVAL);
623 }
624 }
625
626 do_patch(priv, m, pullup_len);
627
628 m->m_pkthdr.csum_flags |= priv->conf->csum_flags;
629 }
630
631 #undef PULLUP_CHECK
632
633 bypass:
634 out = NULL;
635
636 if (hook == priv->in) {
637 /* return frames on 'in' hook if 'out' not connected */
638 out = priv->out ? priv->out : priv->in;
639 } else if (hook == priv->out && priv->in) {
640 /* pass frames on 'out' hook if 'in' connected */
641 out = priv->in;
642 }
643
644 if (out == NULL)
645 ERROUT(0);
646
647 NG_FWD_NEW_DATA(error, item, out, m);
648
649 return (error);
650
651 done:
652 drop:
653 NG_FREE_ITEM(item);
654 NG_FREE_M(m);
655
656 priv->stats.dropped++;
657
658 return (error);
659 }
660
661 static int
662 ng_patch_shutdown(node_p node)
663 {
664 const priv_p privdata = NG_NODE_PRIVATE(node);
665
666 NG_NODE_SET_PRIVATE(node, NULL);
667 NG_NODE_UNREF(node);
668
669 if (privdata->conf != NULL)
670 free(privdata->conf, M_NETGRAPH);
671
672 free(privdata, M_NETGRAPH);
673
674 return (0);
675 }
676
677 static int
678 ng_patch_disconnect(hook_p hook)
679 {
680 priv_p priv;
681
682 priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
683
684 if (hook == priv->in) {
685 priv->in = NULL;
686 }
687
688 if (hook == priv->out) {
689 priv->out = NULL;
690 }
691
692 if (NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0 &&
693 NG_NODE_IS_VALID(NG_HOOK_NODE(hook))) /* already shutting down? */
694 ng_rmnode_self(NG_HOOK_NODE(hook));
695
696 return (0);
697 }
Cache object: fd7d26deb4c7dd3e8adabc275939d6f7
|