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