1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2005 Nuno Antunes <nuno.antunes@gmail.com>
5 * Copyright (c) 2007 Alexander Motin <mav@freebsd.org>
6 * Copyright (c) 2019 Lutz Donnerhacke <lutz@donnerhacke.de>
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 *
18 * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28 * SUCH DAMAGE.
29 */
30
31 /*
32 * ng_car - An implementation of committed access rate for netgraph
33 *
34 * TODO:
35 * - Sanitize input config values (impose some limits)
36 * - Implement DSCP marking for IPv4
37 * - Decouple functionality into a simple classifier (g/y/r)
38 * and various action nodes (i.e. shape, dcsp, pcp)
39 */
40
41 #include <sys/param.h>
42 #include <sys/errno.h>
43 #include <sys/kernel.h>
44 #include <sys/malloc.h>
45 #include <sys/mbuf.h>
46
47 #include <netgraph/ng_message.h>
48 #include <netgraph/ng_parse.h>
49 #include <netgraph/netgraph.h>
50 #include <netgraph/ng_car.h>
51
52 #include "qos.h"
53
54 #ifdef NG_SEPARATE_MALLOC
55 static MALLOC_DEFINE(M_NETGRAPH_CAR, "netgraph_car", "netgraph car node");
56 #else
57 #define M_NETGRAPH_CAR M_NETGRAPH
58 #endif
59
60 #define NG_CAR_QUEUE_SIZE 100 /* Maximum queue size for SHAPE mode */
61 #define NG_CAR_QUEUE_MIN_TH 8 /* Minimum RED threshold for SHAPE mode */
62
63 /* Hook private info */
64 struct hookinfo {
65 hook_p hook; /* this (source) hook */
66 hook_p dest; /* destination hook */
67
68 int64_t tc; /* committed token bucket counter */
69 int64_t te; /* exceeded/peak token bucket counter */
70 struct bintime lastRefill; /* last token refill time */
71
72 struct ng_car_hookconf conf; /* hook configuration */
73 struct ng_car_hookstats stats; /* hook stats */
74
75 struct mbuf *q[NG_CAR_QUEUE_SIZE]; /* circular packet queue */
76 u_int q_first; /* first queue element */
77 u_int q_last; /* last queue element */
78 struct callout q_callout; /* periodic queue processing routine */
79 struct mtx q_mtx; /* queue mutex */
80 };
81
82 /* Private information for each node instance */
83 struct privdata {
84 node_p node; /* the node itself */
85 struct hookinfo upper; /* hook to upper layers */
86 struct hookinfo lower; /* hook to lower layers */
87 };
88 typedef struct privdata *priv_p;
89
90 static ng_constructor_t ng_car_constructor;
91 static ng_rcvmsg_t ng_car_rcvmsg;
92 static ng_shutdown_t ng_car_shutdown;
93 static ng_newhook_t ng_car_newhook;
94 static ng_rcvdata_t ng_car_rcvdata;
95 static ng_disconnect_t ng_car_disconnect;
96
97 static void ng_car_refillhook(struct hookinfo *h);
98 static void ng_car_schedule(struct hookinfo *h);
99 void ng_car_q_event(node_p node, hook_p hook, void *arg, int arg2);
100 static void ng_car_enqueue(struct hookinfo *h, item_p item);
101
102 /* Parse type for struct ng_car_hookstats */
103 static const struct ng_parse_struct_field ng_car_hookstats_type_fields[]
104 = NG_CAR_HOOKSTATS;
105 static const struct ng_parse_type ng_car_hookstats_type = {
106 &ng_parse_struct_type,
107 &ng_car_hookstats_type_fields
108 };
109
110 /* Parse type for struct ng_car_bulkstats */
111 static const struct ng_parse_struct_field ng_car_bulkstats_type_fields[]
112 = NG_CAR_BULKSTATS(&ng_car_hookstats_type);
113 static const struct ng_parse_type ng_car_bulkstats_type = {
114 &ng_parse_struct_type,
115 &ng_car_bulkstats_type_fields
116 };
117
118 /* Parse type for struct ng_car_hookconf */
119 static const struct ng_parse_struct_field ng_car_hookconf_type_fields[]
120 = NG_CAR_HOOKCONF;
121 static const struct ng_parse_type ng_car_hookconf_type = {
122 &ng_parse_struct_type,
123 &ng_car_hookconf_type_fields
124 };
125
126 /* Parse type for struct ng_car_bulkconf */
127 static const struct ng_parse_struct_field ng_car_bulkconf_type_fields[]
128 = NG_CAR_BULKCONF(&ng_car_hookconf_type);
129 static const struct ng_parse_type ng_car_bulkconf_type = {
130 &ng_parse_struct_type,
131 &ng_car_bulkconf_type_fields
132 };
133
134 /* Command list */
135 static struct ng_cmdlist ng_car_cmdlist[] = {
136 {
137 NGM_CAR_COOKIE,
138 NGM_CAR_GET_STATS,
139 "getstats",
140 NULL,
141 &ng_car_bulkstats_type,
142 },
143 {
144 NGM_CAR_COOKIE,
145 NGM_CAR_CLR_STATS,
146 "clrstats",
147 NULL,
148 NULL,
149 },
150 {
151 NGM_CAR_COOKIE,
152 NGM_CAR_GETCLR_STATS,
153 "getclrstats",
154 NULL,
155 &ng_car_bulkstats_type,
156 },
157
158 {
159 NGM_CAR_COOKIE,
160 NGM_CAR_GET_CONF,
161 "getconf",
162 NULL,
163 &ng_car_bulkconf_type,
164 },
165 {
166 NGM_CAR_COOKIE,
167 NGM_CAR_SET_CONF,
168 "setconf",
169 &ng_car_bulkconf_type,
170 NULL,
171 },
172 { 0 }
173 };
174
175 /* Netgraph node type descriptor */
176 static struct ng_type ng_car_typestruct = {
177 .version = NG_ABI_VERSION,
178 .name = NG_CAR_NODE_TYPE,
179 .constructor = ng_car_constructor,
180 .rcvmsg = ng_car_rcvmsg,
181 .shutdown = ng_car_shutdown,
182 .newhook = ng_car_newhook,
183 .rcvdata = ng_car_rcvdata,
184 .disconnect = ng_car_disconnect,
185 .cmdlist = ng_car_cmdlist,
186 };
187 NETGRAPH_INIT(car, &ng_car_typestruct);
188
189 /*
190 * Node constructor
191 */
192 static int
ng_car_constructor(node_p node)193 ng_car_constructor(node_p node)
194 {
195 priv_p priv;
196
197 /* Initialize private descriptor. */
198 priv = malloc(sizeof(*priv), M_NETGRAPH_CAR, M_WAITOK | M_ZERO);
199
200 NG_NODE_SET_PRIVATE(node, priv);
201 priv->node = node;
202
203 /*
204 * Arbitrary default values
205 */
206
207 priv->upper.hook = NULL;
208 priv->upper.dest = NULL;
209 priv->upper.tc = priv->upper.conf.cbs = NG_CAR_CBS_MIN;
210 priv->upper.te = priv->upper.conf.ebs = NG_CAR_EBS_MIN;
211 priv->upper.conf.cir = NG_CAR_CIR_DFLT;
212 priv->upper.conf.green_action = NG_CAR_ACTION_FORWARD;
213 priv->upper.conf.yellow_action = NG_CAR_ACTION_FORWARD;
214 priv->upper.conf.red_action = NG_CAR_ACTION_DROP;
215 priv->upper.conf.mode = 0;
216 getbinuptime(&priv->upper.lastRefill);
217 priv->upper.q_first = 0;
218 priv->upper.q_last = 0;
219 ng_callout_init(&priv->upper.q_callout);
220 mtx_init(&priv->upper.q_mtx, "ng_car_u", NULL, MTX_DEF);
221
222 priv->lower.hook = NULL;
223 priv->lower.dest = NULL;
224 priv->lower.tc = priv->lower.conf.cbs = NG_CAR_CBS_MIN;
225 priv->lower.te = priv->lower.conf.ebs = NG_CAR_EBS_MIN;
226 priv->lower.conf.cir = NG_CAR_CIR_DFLT;
227 priv->lower.conf.green_action = NG_CAR_ACTION_FORWARD;
228 priv->lower.conf.yellow_action = NG_CAR_ACTION_FORWARD;
229 priv->lower.conf.red_action = NG_CAR_ACTION_DROP;
230 priv->lower.conf.mode = 0;
231 priv->lower.lastRefill = priv->upper.lastRefill;
232 priv->lower.q_first = 0;
233 priv->lower.q_last = 0;
234 ng_callout_init(&priv->lower.q_callout);
235 mtx_init(&priv->lower.q_mtx, "ng_car_l", NULL, MTX_DEF);
236
237 return (0);
238 }
239
240 /*
241 * Add a hook.
242 */
243 static int
ng_car_newhook(node_p node,hook_p hook,const char * name)244 ng_car_newhook(node_p node, hook_p hook, const char *name)
245 {
246 const priv_p priv = NG_NODE_PRIVATE(node);
247
248 if (strcmp(name, NG_CAR_HOOK_LOWER) == 0) {
249 priv->lower.hook = hook;
250 priv->upper.dest = hook;
251 bzero(&priv->lower.stats, sizeof(priv->lower.stats));
252 NG_HOOK_SET_PRIVATE(hook, &priv->lower);
253 } else if (strcmp(name, NG_CAR_HOOK_UPPER) == 0) {
254 priv->upper.hook = hook;
255 priv->lower.dest = hook;
256 bzero(&priv->upper.stats, sizeof(priv->upper.stats));
257 NG_HOOK_SET_PRIVATE(hook, &priv->upper);
258 } else
259 return (EINVAL);
260 return(0);
261 }
262
263 /*
264 * Data has arrived.
265 */
266 static int
ng_car_rcvdata(hook_p hook,item_p item)267 ng_car_rcvdata(hook_p hook, item_p item )
268 {
269 struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
270 struct mbuf *m;
271 struct m_qos_color *colp;
272 enum qos_color col;
273 int error = 0;
274 u_int len;
275
276 /* If queue is not empty now then enqueue packet. */
277 if (hinfo->q_first != hinfo->q_last) {
278 ng_car_enqueue(hinfo, item);
279 return (0);
280 }
281
282 m = NGI_M(item);
283
284 #define NG_CAR_PERFORM_MATCH_ACTION(a,col) \
285 do { \
286 switch (a) { \
287 case NG_CAR_ACTION_FORWARD: \
288 /* Do nothing. */ \
289 break; \
290 case NG_CAR_ACTION_MARK: \
291 if (colp == NULL) { \
292 colp = (void *)m_tag_alloc( \
293 M_QOS_COOKIE, M_QOS_COLOR, \
294 MTAG_SIZE(m_qos_color), M_NOWAIT); \
295 if (colp != NULL) \
296 m_tag_prepend(m, &colp->tag); \
297 } \
298 if (colp != NULL) \
299 colp->color = col; \
300 break; \
301 case NG_CAR_ACTION_DROP: \
302 default: \
303 /* Drop packet and return. */ \
304 NG_FREE_ITEM(item); \
305 ++hinfo->stats.dropped_pkts; \
306 return (0); \
307 } \
308 } while (0)
309
310 /* Packet is counted as 128 tokens for better resolution */
311 if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
312 len = 128;
313 } else {
314 len = m->m_pkthdr.len;
315 }
316
317 /* Determine current color of the packet (default green) */
318 colp = (void *)m_tag_locate(m, M_QOS_COOKIE, M_QOS_COLOR, NULL);
319 if ((hinfo->conf.opt & NG_CAR_COLOR_AWARE) && (colp != NULL))
320 col = colp->color;
321 else
322 col = QOS_COLOR_GREEN;
323
324 /* Check committed token bucket. */
325 if (hinfo->tc - len >= 0 && col <= QOS_COLOR_GREEN) {
326 /* This packet is green. */
327 ++hinfo->stats.green_pkts;
328 hinfo->tc -= len;
329 NG_CAR_PERFORM_MATCH_ACTION(
330 hinfo->conf.green_action,
331 QOS_COLOR_GREEN);
332 } else {
333 /* Refill only if not green without it. */
334 ng_car_refillhook(hinfo);
335
336 /* Check committed token bucket again after refill. */
337 if (hinfo->tc - len >= 0 && col <= QOS_COLOR_GREEN) {
338 /* This packet is green */
339 ++hinfo->stats.green_pkts;
340 hinfo->tc -= len;
341 NG_CAR_PERFORM_MATCH_ACTION(
342 hinfo->conf.green_action,
343 QOS_COLOR_GREEN);
344
345 /* If not green and mode is SHAPE, enqueue packet. */
346 } else if (hinfo->conf.mode == NG_CAR_SHAPE) {
347 ng_car_enqueue(hinfo, item);
348 return (0);
349
350 /* If not green and mode is RED, calculate probability. */
351 } else if (hinfo->conf.mode == NG_CAR_RED) {
352 /* Is packet is bigger then extended burst? */
353 if (len - (hinfo->tc - len) > hinfo->conf.ebs ||
354 col >= QOS_COLOR_RED) {
355 /* This packet is definitely red. */
356 ++hinfo->stats.red_pkts;
357 hinfo->te = 0;
358 NG_CAR_PERFORM_MATCH_ACTION(
359 hinfo->conf.red_action,
360 QOS_COLOR_RED);
361
362 /* Use token bucket to simulate RED-like drop
363 probability. */
364 } else if (hinfo->te + (len - hinfo->tc) < hinfo->conf.ebs &&
365 col <= QOS_COLOR_YELLOW) {
366 /* This packet is yellow */
367 ++hinfo->stats.yellow_pkts;
368 hinfo->te += len - hinfo->tc;
369 /* Go to negative tokens. */
370 hinfo->tc -= len;
371 NG_CAR_PERFORM_MATCH_ACTION(
372 hinfo->conf.yellow_action,
373 QOS_COLOR_YELLOW);
374 } else {
375 /* This packet is probably red. */
376 ++hinfo->stats.red_pkts;
377 hinfo->te = 0;
378 NG_CAR_PERFORM_MATCH_ACTION(
379 hinfo->conf.red_action,
380 QOS_COLOR_RED);
381 }
382 /* If not green and mode is SINGLE/DOUBLE RATE. */
383 } else {
384 /* Check extended token bucket. */
385 if (hinfo->te - len >= 0 && col <= QOS_COLOR_YELLOW) {
386 /* This packet is yellow */
387 ++hinfo->stats.yellow_pkts;
388 hinfo->te -= len;
389 NG_CAR_PERFORM_MATCH_ACTION(
390 hinfo->conf.yellow_action,
391 QOS_COLOR_YELLOW);
392 } else {
393 /* This packet is red */
394 ++hinfo->stats.red_pkts;
395 NG_CAR_PERFORM_MATCH_ACTION(
396 hinfo->conf.red_action,
397 QOS_COLOR_RED);
398 }
399 }
400 }
401
402 #undef NG_CAR_PERFORM_MATCH_ACTION
403
404 NG_FWD_ITEM_HOOK(error, item, hinfo->dest);
405 if (error != 0)
406 ++hinfo->stats.errors;
407 ++hinfo->stats.passed_pkts;
408
409 return (error);
410 }
411
412 /*
413 * Receive a control message.
414 */
415 static int
ng_car_rcvmsg(node_p node,item_p item,hook_p lasthook)416 ng_car_rcvmsg(node_p node, item_p item, hook_p lasthook)
417 {
418 const priv_p priv = NG_NODE_PRIVATE(node);
419 struct ng_mesg *resp = NULL;
420 int error = 0;
421 struct ng_mesg *msg;
422
423 NGI_GET_MSG(item, msg);
424 switch (msg->header.typecookie) {
425 case NGM_CAR_COOKIE:
426 switch (msg->header.cmd) {
427 case NGM_CAR_GET_STATS:
428 case NGM_CAR_GETCLR_STATS:
429 {
430 struct ng_car_bulkstats *bstats;
431
432 NG_MKRESPONSE(resp, msg,
433 sizeof(*bstats), M_NOWAIT);
434 if (resp == NULL) {
435 error = ENOMEM;
436 break;
437 }
438 bstats = (struct ng_car_bulkstats *)resp->data;
439
440 bcopy(&priv->upper.stats, &bstats->downstream,
441 sizeof(bstats->downstream));
442 bcopy(&priv->lower.stats, &bstats->upstream,
443 sizeof(bstats->upstream));
444 }
445 if (msg->header.cmd == NGM_CAR_GET_STATS)
446 break;
447 case NGM_CAR_CLR_STATS:
448 bzero(&priv->upper.stats,
449 sizeof(priv->upper.stats));
450 bzero(&priv->lower.stats,
451 sizeof(priv->lower.stats));
452 break;
453 case NGM_CAR_GET_CONF:
454 {
455 struct ng_car_bulkconf *bconf;
456
457 NG_MKRESPONSE(resp, msg,
458 sizeof(*bconf), M_NOWAIT);
459 if (resp == NULL) {
460 error = ENOMEM;
461 break;
462 }
463 bconf = (struct ng_car_bulkconf *)resp->data;
464
465 bcopy(&priv->upper.conf, &bconf->downstream,
466 sizeof(bconf->downstream));
467 bcopy(&priv->lower.conf, &bconf->upstream,
468 sizeof(bconf->upstream));
469 /* Convert internal 1/(8*128) of pps into pps */
470 if (bconf->downstream.opt & NG_CAR_COUNT_PACKETS) {
471 bconf->downstream.cir /= 1024;
472 bconf->downstream.pir /= 1024;
473 bconf->downstream.cbs /= 128;
474 bconf->downstream.ebs /= 128;
475 }
476 if (bconf->upstream.opt & NG_CAR_COUNT_PACKETS) {
477 bconf->upstream.cir /= 1024;
478 bconf->upstream.pir /= 1024;
479 bconf->upstream.cbs /= 128;
480 bconf->upstream.ebs /= 128;
481 }
482 }
483 break;
484 case NGM_CAR_SET_CONF:
485 {
486 struct ng_car_bulkconf *const bconf =
487 (struct ng_car_bulkconf *)msg->data;
488
489 /* Check for invalid or illegal config. */
490 if (msg->header.arglen != sizeof(*bconf)) {
491 error = EINVAL;
492 break;
493 }
494 /* Convert pps into internal 1/(8*128) of pps */
495 if (bconf->downstream.opt & NG_CAR_COUNT_PACKETS) {
496 bconf->downstream.cir *= 1024;
497 bconf->downstream.pir *= 1024;
498 bconf->downstream.cbs *= 128;
499 bconf->downstream.ebs *= 128;
500 }
501 if (bconf->upstream.opt & NG_CAR_COUNT_PACKETS) {
502 bconf->upstream.cir *= 1024;
503 bconf->upstream.pir *= 1024;
504 bconf->upstream.cbs *= 128;
505 bconf->upstream.ebs *= 128;
506 }
507 if ((bconf->downstream.cir > 1000000000) ||
508 (bconf->downstream.pir > 1000000000) ||
509 (bconf->upstream.cir > 1000000000) ||
510 (bconf->upstream.pir > 1000000000) ||
511 (bconf->downstream.cbs == 0 &&
512 bconf->downstream.ebs == 0) ||
513 (bconf->upstream.cbs == 0 &&
514 bconf->upstream.ebs == 0))
515 {
516 error = EINVAL;
517 break;
518 }
519 if ((bconf->upstream.mode == NG_CAR_SHAPE) &&
520 (bconf->upstream.cir == 0)) {
521 error = EINVAL;
522 break;
523 }
524 if ((bconf->downstream.mode == NG_CAR_SHAPE) &&
525 (bconf->downstream.cir == 0)) {
526 error = EINVAL;
527 break;
528 }
529
530 /* Copy downstream config. */
531 bcopy(&bconf->downstream, &priv->upper.conf,
532 sizeof(priv->upper.conf));
533 priv->upper.tc = priv->upper.conf.cbs;
534 if (priv->upper.conf.mode == NG_CAR_RED ||
535 priv->upper.conf.mode == NG_CAR_SHAPE) {
536 priv->upper.te = 0;
537 } else {
538 priv->upper.te = priv->upper.conf.ebs;
539 }
540
541 /* Copy upstream config. */
542 bcopy(&bconf->upstream, &priv->lower.conf,
543 sizeof(priv->lower.conf));
544 priv->lower.tc = priv->lower.conf.cbs;
545 if (priv->lower.conf.mode == NG_CAR_RED ||
546 priv->lower.conf.mode == NG_CAR_SHAPE) {
547 priv->lower.te = 0;
548 } else {
549 priv->lower.te = priv->lower.conf.ebs;
550 }
551 }
552 break;
553 default:
554 error = EINVAL;
555 break;
556 }
557 break;
558 default:
559 error = EINVAL;
560 break;
561 }
562 NG_RESPOND_MSG(error, node, item, resp);
563 NG_FREE_MSG(msg);
564 return (error);
565 }
566
567 /*
568 * Do local shutdown processing.
569 */
570 static int
ng_car_shutdown(node_p node)571 ng_car_shutdown(node_p node)
572 {
573 const priv_p priv = NG_NODE_PRIVATE(node);
574
575 ng_uncallout(&priv->upper.q_callout, node);
576 ng_uncallout(&priv->lower.q_callout, node);
577 mtx_destroy(&priv->upper.q_mtx);
578 mtx_destroy(&priv->lower.q_mtx);
579 NG_NODE_UNREF(priv->node);
580 free(priv, M_NETGRAPH_CAR);
581 return (0);
582 }
583
584 /*
585 * Hook disconnection.
586 *
587 * For this type, removal of the last link destroys the node.
588 */
589 static int
ng_car_disconnect(hook_p hook)590 ng_car_disconnect(hook_p hook)
591 {
592 struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
593 const node_p node = NG_HOOK_NODE(hook);
594 const priv_p priv = NG_NODE_PRIVATE(node);
595
596 if (hinfo) {
597 /* Purge queue if not empty. */
598 while (hinfo->q_first != hinfo->q_last) {
599 NG_FREE_M(hinfo->q[hinfo->q_first]);
600 hinfo->q_first++;
601 if (hinfo->q_first >= NG_CAR_QUEUE_SIZE)
602 hinfo->q_first = 0;
603 }
604 /* Remove hook refs. */
605 if (hinfo->hook == priv->upper.hook)
606 priv->lower.dest = NULL;
607 else
608 priv->upper.dest = NULL;
609 hinfo->hook = NULL;
610 }
611 /* Already shutting down? */
612 if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0)
613 && (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
614 ng_rmnode_self(NG_HOOK_NODE(hook));
615 return (0);
616 }
617
618 /*
619 * Hook's token buckets refillment.
620 */
621 static void
ng_car_refillhook(struct hookinfo * h)622 ng_car_refillhook(struct hookinfo *h)
623 {
624 struct bintime newt, deltat;
625 unsigned int deltat_us;
626
627 /* Get current time. */
628 getbinuptime(&newt);
629
630 /* Get time delta since last refill. */
631 deltat = newt;
632 bintime_sub(&deltat, &h->lastRefill);
633
634 /* Time must go forward. */
635 if (deltat.sec < 0) {
636 h->lastRefill = newt;
637 return;
638 }
639
640 /* But not too far forward. */
641 if (deltat.sec >= 1000) {
642 deltat_us = (1000 << 20);
643 } else {
644 /* convert bintime to the 1/(2^20) of sec */
645 deltat_us = (deltat.sec << 20) + (deltat.frac >> 44);
646 }
647
648 if (h->conf.mode == NG_CAR_SINGLE_RATE) {
649 int64_t delta;
650 /* Refill committed token bucket. */
651 h->tc += (h->conf.cir * deltat_us) >> 23;
652 delta = h->tc - h->conf.cbs;
653 if (delta > 0) {
654 h->tc = h->conf.cbs;
655
656 /* Refill exceeded token bucket. */
657 h->te += delta;
658 if (h->te > ((int64_t)h->conf.ebs))
659 h->te = h->conf.ebs;
660 }
661
662 } else if (h->conf.mode == NG_CAR_DOUBLE_RATE) {
663 /* Refill committed token bucket. */
664 h->tc += (h->conf.cir * deltat_us) >> 23;
665 if (h->tc > ((int64_t)h->conf.cbs))
666 h->tc = h->conf.cbs;
667
668 /* Refill peak token bucket. */
669 h->te += (h->conf.pir * deltat_us) >> 23;
670 if (h->te > ((int64_t)h->conf.ebs))
671 h->te = h->conf.ebs;
672
673 } else { /* RED or SHAPE mode. */
674 /* Refill committed token bucket. */
675 h->tc += (h->conf.cir * deltat_us) >> 23;
676 if (h->tc > ((int64_t)h->conf.cbs))
677 h->tc = h->conf.cbs;
678 }
679
680 /* Remember this moment. */
681 h->lastRefill = newt;
682 }
683
684 /*
685 * Schedule callout when we will have required tokens.
686 */
687 static void
ng_car_schedule(struct hookinfo * hinfo)688 ng_car_schedule(struct hookinfo *hinfo)
689 {
690 int delay;
691
692 delay = (-(hinfo->tc)) * hz * 8 / hinfo->conf.cir + 1;
693
694 ng_callout(&hinfo->q_callout, NG_HOOK_NODE(hinfo->hook), hinfo->hook,
695 delay, &ng_car_q_event, NULL, 0);
696 }
697
698 /*
699 * Queue processing callout handler.
700 */
701 void
ng_car_q_event(node_p node,hook_p hook,void * arg,int arg2)702 ng_car_q_event(node_p node, hook_p hook, void *arg, int arg2)
703 {
704 struct hookinfo *hinfo = NG_HOOK_PRIVATE(hook);
705 struct mbuf *m;
706 int error;
707
708 /* Refill tokens for time we have slept. */
709 ng_car_refillhook(hinfo);
710
711 /* If we have some tokens */
712 while (hinfo->tc >= 0) {
713 /* Send packet. */
714 m = hinfo->q[hinfo->q_first];
715 NG_SEND_DATA_ONLY(error, hinfo->dest, m);
716 if (error != 0)
717 ++hinfo->stats.errors;
718 ++hinfo->stats.passed_pkts;
719
720 /* Get next one. */
721 hinfo->q_first++;
722 if (hinfo->q_first >= NG_CAR_QUEUE_SIZE)
723 hinfo->q_first = 0;
724
725 /* Stop if none left. */
726 if (hinfo->q_first == hinfo->q_last)
727 break;
728
729 /* If we have more packet, try it. */
730 m = hinfo->q[hinfo->q_first];
731 if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
732 hinfo->tc -= 128;
733 } else {
734 hinfo->tc -= m->m_pkthdr.len;
735 }
736 }
737
738 /* If something left */
739 if (hinfo->q_first != hinfo->q_last)
740 /* Schedule queue processing. */
741 ng_car_schedule(hinfo);
742 }
743
744 /*
745 * Enqueue packet.
746 */
747 static void
ng_car_enqueue(struct hookinfo * hinfo,item_p item)748 ng_car_enqueue(struct hookinfo *hinfo, item_p item)
749 {
750 struct mbuf *m;
751 int len;
752 struct m_qos_color *colp;
753 enum qos_color col;
754
755 NGI_GET_M(item, m);
756 NG_FREE_ITEM(item);
757
758 /* Determine current color of the packet (default green) */
759 colp = (void *)m_tag_locate(m, M_QOS_COOKIE, M_QOS_COLOR, NULL);
760 if ((hinfo->conf.opt & NG_CAR_COLOR_AWARE) && (colp != NULL))
761 col = colp->color;
762 else
763 col = QOS_COLOR_GREEN;
764
765 /* Lock queue mutex. */
766 mtx_lock(&hinfo->q_mtx);
767
768 /* Calculate used queue length. */
769 len = hinfo->q_last - hinfo->q_first;
770 if (len < 0)
771 len += NG_CAR_QUEUE_SIZE;
772
773 /* If queue is overflowed or we have no RED tokens. */
774 if ((len >= (NG_CAR_QUEUE_SIZE - 1)) ||
775 (hinfo->te + len >= NG_CAR_QUEUE_SIZE) ||
776 (col >= QOS_COLOR_RED)) {
777 /* Drop packet. */
778 ++hinfo->stats.red_pkts;
779 ++hinfo->stats.dropped_pkts;
780 NG_FREE_M(m);
781
782 hinfo->te = 0;
783 } else {
784 /* This packet is yellow. */
785 ++hinfo->stats.yellow_pkts;
786
787 /* Enqueue packet. */
788 hinfo->q[hinfo->q_last] = m;
789 hinfo->q_last++;
790 if (hinfo->q_last >= NG_CAR_QUEUE_SIZE)
791 hinfo->q_last = 0;
792
793 /* Use RED tokens. */
794 if (len > NG_CAR_QUEUE_MIN_TH)
795 hinfo->te += len - NG_CAR_QUEUE_MIN_TH;
796
797 /* If this is a first packet in the queue. */
798 if (len == 0) {
799 if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
800 hinfo->tc -= 128;
801 } else {
802 hinfo->tc -= m->m_pkthdr.len;
803 }
804
805 /* Schedule queue processing. */
806 ng_car_schedule(hinfo);
807 }
808 }
809
810 /* Unlock queue mutex. */
811 mtx_unlock(&hinfo->q_mtx);
812 }
813