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