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