1 /* $NetBSD: altq_cdnr.c,v 1.4 2001/11/12 23:14:21 lukem Exp $ */ 2 /* $KAME: altq_cdnr.c,v 1.8 2000/12/14 08:12:45 thorpej Exp $ */ 3 4 /* 5 * Copyright (C) 1999-2000 6 * Sony Computer Science Laboratories Inc. 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 SONY CSL 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 SONY CSL 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 #include <sys/cdefs.h> 31 __KERNEL_RCSID(0, "$NetBSD: altq_cdnr.c,v 1.4 2001/11/12 23:14:21 lukem Exp $"); 32 33 #if defined(__FreeBSD__) || defined(__NetBSD__) 34 #include "opt_altq.h" 35 #if (__FreeBSD__ != 2) 36 #include "opt_inet.h" 37 #ifdef __FreeBSD__ 38 #include "opt_inet6.h" 39 #endif 40 #endif 41 #endif /* __FreeBSD__ || __NetBSD__ */ 42 43 #include <sys/param.h> 44 #include <sys/malloc.h> 45 #include <sys/mbuf.h> 46 #include <sys/socket.h> 47 #include <sys/sockio.h> 48 #include <sys/systm.h> 49 #include <sys/proc.h> 50 #include <sys/errno.h> 51 #include <sys/kernel.h> 52 #include <sys/queue.h> 53 54 #include <net/if.h> 55 #include <net/if_types.h> 56 #include <netinet/in.h> 57 #include <netinet/in_systm.h> 58 #include <netinet/ip.h> 59 #ifdef INET6 60 #include <netinet/ip6.h> 61 #endif 62 63 #include <altq/altq.h> 64 #include <altq/altq_conf.h> 65 #include <altq/altq_cdnr.h> 66 67 /* 68 * diffserv traffic conditioning module 69 */ 70 71 int altq_cdnr_enabled = 0; 72 73 /* traffic conditioner is enabled by ALTQ_CDNR option in opt_altq.h */ 74 #ifdef ALTQ_CDNR 75 76 /* cdnr_list keeps all cdnr's allocated. */ 77 static LIST_HEAD(, top_cdnr) tcb_list; 78 79 int cdnropen __P((dev_t, int, int, struct proc *)); 80 int cdnrclose __P((dev_t, int, int, struct proc *)); 81 int cdnrioctl __P((dev_t, ioctlcmd_t, caddr_t, int, struct proc *)); 82 83 static int altq_cdnr_input __P((struct mbuf *, int)); 84 static struct top_cdnr *tcb_lookup __P((char *ifname)); 85 static struct cdnr_block *cdnr_handle2cb __P((u_long)); 86 static u_long cdnr_cb2handle __P((struct cdnr_block *)); 87 static void *cdnr_cballoc __P((struct top_cdnr *, int, 88 struct tc_action *(*)(struct cdnr_block *, struct cdnr_pktinfo *))); 89 static void cdnr_cbdestroy __P((void *)); 90 static int tca_verify_action __P((struct tc_action *)); 91 static void tca_import_action __P((struct tc_action *, struct tc_action *)); 92 static void tca_invalidate_action __P((struct tc_action *)); 93 94 static int generic_element_destroy __P((struct cdnr_block *)); 95 static struct top_cdnr *top_create __P((struct ifaltq *)); 96 static int top_destroy __P((struct top_cdnr *)); 97 static struct cdnr_block *element_create __P((struct top_cdnr *, 98 struct tc_action *)); 99 static int element_destroy __P((struct cdnr_block *)); 100 static void tb_import_profile __P((struct tbe *, struct tb_profile *)); 101 static struct tbmeter *tbm_create __P((struct top_cdnr *, struct tb_profile *, 102 struct tc_action *, struct tc_action *)); 103 static int tbm_destroy __P((struct tbmeter *)); 104 static struct tc_action *tbm_input __P((struct cdnr_block *, 105 struct cdnr_pktinfo *)); 106 static struct trtcm *trtcm_create __P((struct top_cdnr *, 107 struct tb_profile *, struct tb_profile *, 108 struct tc_action *, struct tc_action *, struct tc_action *, 109 int)); 110 static int trtcm_destroy __P((struct trtcm *)); 111 static struct tc_action *trtcm_input __P((struct cdnr_block *, 112 struct cdnr_pktinfo *)); 113 static struct tswtcm *tswtcm_create __P((struct top_cdnr *, 114 u_int32_t, u_int32_t, u_int32_t, 115 struct tc_action *, struct tc_action *, struct tc_action *)); 116 static int tswtcm_destroy __P((struct tswtcm *)); 117 static struct tc_action *tswtcm_input __P((struct cdnr_block *, 118 struct cdnr_pktinfo *)); 119 120 static int cdnrcmd_if_attach __P((char *)); 121 static int cdnrcmd_if_detach __P((char *)); 122 static int cdnrcmd_add_element __P((struct cdnr_add_element *)); 123 static int cdnrcmd_delete_element __P((struct cdnr_delete_element *)); 124 static int cdnrcmd_add_filter __P((struct cdnr_add_filter *)); 125 static int cdnrcmd_delete_filter __P((struct cdnr_delete_filter *)); 126 static int cdnrcmd_add_tbm __P((struct cdnr_add_tbmeter *)); 127 static int cdnrcmd_modify_tbm __P((struct cdnr_modify_tbmeter *)); 128 static int cdnrcmd_tbm_stats __P((struct cdnr_tbmeter_stats *)); 129 static int cdnrcmd_add_trtcm __P((struct cdnr_add_trtcm *)); 130 static int cdnrcmd_modify_trtcm __P((struct cdnr_modify_trtcm *)); 131 static int cdnrcmd_tcm_stats __P((struct cdnr_tcm_stats *)); 132 static int cdnrcmd_add_tswtcm __P((struct cdnr_add_tswtcm *)); 133 static int cdnrcmd_modify_tswtcm __P((struct cdnr_modify_tswtcm *)); 134 static int cdnrcmd_get_stats __P((struct cdnr_get_stats *)); 135 136 /* 137 * top level input function called from ip_input. 138 * should be called before converting header fields to host-byte-order. 139 */ 140 int 141 altq_cdnr_input(m, af) 142 struct mbuf *m; 143 int af; /* address family */ 144 { 145 struct ifnet *ifp; 146 struct ip *ip; 147 struct top_cdnr *top; 148 struct tc_action *tca; 149 struct cdnr_block *cb; 150 struct cdnr_pktinfo pktinfo; 151 152 ifp = m->m_pkthdr.rcvif; 153 if (!ALTQ_IS_CNDTNING(&ifp->if_snd)) 154 /* traffic conditioner is not enabled on this interface */ 155 return (1); 156 157 top = ifp->if_snd.altq_cdnr; 158 159 ip = mtod(m, struct ip *); 160 #ifdef INET6 161 if (af == AF_INET6) { 162 u_int32_t flowlabel; 163 164 flowlabel = ((struct ip6_hdr *)ip)->ip6_flow; 165 pktinfo.pkt_dscp = (ntohl(flowlabel) >> 20) & DSCP_MASK; 166 } else 167 #endif 168 pktinfo.pkt_dscp = ip->ip_tos & DSCP_MASK; 169 pktinfo.pkt_len = m_pktlen(m); 170 171 tca = NULL; 172 173 cb = acc_classify(&top->tc_classifier, m, af); 174 if (cb != NULL) 175 tca = &cb->cb_action; 176 177 if (tca == NULL) 178 tca = &top->tc_block.cb_action; 179 180 while (1) { 181 PKTCNTR_ADD(&top->tc_cnts[tca->tca_code], pktinfo.pkt_len); 182 183 switch (tca->tca_code) { 184 case TCACODE_PASS: 185 return (1); 186 case TCACODE_DROP: 187 m_freem(m); 188 return (0); 189 case TCACODE_RETURN: 190 return (0); 191 case TCACODE_MARK: 192 #ifdef INET6 193 if (af == AF_INET6) { 194 struct ip6_hdr *ip6 = (struct ip6_hdr *)ip; 195 u_int32_t flowlabel; 196 197 flowlabel = ntohl(ip6->ip6_flow); 198 flowlabel = (tca->tca_dscp << 20) | 199 (flowlabel & ~(DSCP_MASK << 20)); 200 ip6->ip6_flow = htonl(flowlabel); 201 } else 202 #endif 203 ip->ip_tos = tca->tca_dscp | 204 (ip->ip_tos & DSCP_CUMASK); 205 return (1); 206 case TCACODE_NEXT: 207 cb = tca->tca_next; 208 tca = (*cb->cb_input)(cb, &pktinfo); 209 break; 210 case TCACODE_NONE: 211 default: 212 return (1); 213 } 214 } 215 } 216 217 static struct top_cdnr * 218 tcb_lookup(ifname) 219 char *ifname; 220 { 221 struct top_cdnr *top; 222 struct ifnet *ifp; 223 224 if ((ifp = ifunit(ifname)) != NULL) 225 LIST_FOREACH(top, &tcb_list, tc_next) 226 if (top->tc_ifq->altq_ifp == ifp) 227 return (top); 228 return (NULL); 229 } 230 231 static struct cdnr_block * 232 cdnr_handle2cb(handle) 233 u_long handle; 234 { 235 struct cdnr_block *cb; 236 237 cb = (struct cdnr_block *)handle; 238 if (handle != ALIGN(cb)) 239 return (NULL); 240 241 if (cb == NULL || cb->cb_handle != handle) 242 return (NULL); 243 return (cb); 244 } 245 246 static u_long 247 cdnr_cb2handle(cb) 248 struct cdnr_block *cb; 249 { 250 return (cb->cb_handle); 251 } 252 253 static void * 254 cdnr_cballoc(top, type, input_func) 255 struct top_cdnr *top; 256 int type; 257 struct tc_action *(*input_func)(struct cdnr_block *, 258 struct cdnr_pktinfo *); 259 { 260 struct cdnr_block *cb; 261 int size; 262 263 switch (type) { 264 case TCETYPE_TOP: 265 size = sizeof(struct top_cdnr); 266 break; 267 case TCETYPE_ELEMENT: 268 size = sizeof(struct cdnr_block); 269 break; 270 case TCETYPE_TBMETER: 271 size = sizeof(struct tbmeter); 272 break; 273 case TCETYPE_TRTCM: 274 size = sizeof(struct trtcm); 275 break; 276 case TCETYPE_TSWTCM: 277 size = sizeof(struct tswtcm); 278 break; 279 default: 280 return (NULL); 281 } 282 283 MALLOC(cb, struct cdnr_block *, size, M_DEVBUF, M_WAITOK); 284 if (cb == NULL) 285 return (NULL); 286 bzero(cb, size); 287 288 cb->cb_len = size; 289 cb->cb_type = type; 290 cb->cb_ref = 0; 291 cb->cb_handle = (u_long)cb; 292 if (top == NULL) 293 cb->cb_top = (struct top_cdnr *)cb; 294 else 295 cb->cb_top = top; 296 297 if (input_func != NULL) { 298 /* 299 * if this cdnr has an action function, 300 * make tc_action to call itself. 301 */ 302 cb->cb_action.tca_code = TCACODE_NEXT; 303 cb->cb_action.tca_next = cb; 304 cb->cb_input = input_func; 305 } else 306 cb->cb_action.tca_code = TCACODE_NONE; 307 308 /* if this isn't top, register the element to the top level cdnr */ 309 if (top != NULL) 310 LIST_INSERT_HEAD(&top->tc_elements, cb, cb_next); 311 312 return ((void *)cb); 313 } 314 315 static void 316 cdnr_cbdestroy(cblock) 317 void *cblock; 318 { 319 struct cdnr_block *cb = cblock; 320 321 /* delete filters belonging to this cdnr */ 322 acc_discard_filters(&cb->cb_top->tc_classifier, cb, 0); 323 324 /* remove from the top level cdnr */ 325 if (cb->cb_top != cblock) 326 LIST_REMOVE(cb, cb_next); 327 328 FREE(cb, M_DEVBUF); 329 } 330 331 /* 332 * conditioner common destroy routine 333 */ 334 static int 335 generic_element_destroy(cb) 336 struct cdnr_block *cb; 337 { 338 int error = 0; 339 340 switch (cb->cb_type) { 341 case TCETYPE_TOP: 342 error = top_destroy((struct top_cdnr *)cb); 343 break; 344 case TCETYPE_ELEMENT: 345 error = element_destroy(cb); 346 break; 347 case TCETYPE_TBMETER: 348 error = tbm_destroy((struct tbmeter *)cb); 349 break; 350 case TCETYPE_TRTCM: 351 error = trtcm_destroy((struct trtcm *)cb); 352 break; 353 case TCETYPE_TSWTCM: 354 error = tswtcm_destroy((struct tswtcm *)cb); 355 break; 356 default: 357 error = EINVAL; 358 } 359 return (error); 360 } 361 362 static int 363 tca_verify_action(utca) 364 struct tc_action *utca; 365 { 366 switch (utca->tca_code) { 367 case TCACODE_PASS: 368 case TCACODE_DROP: 369 case TCACODE_MARK: 370 /* these are ok */ 371 break; 372 373 case TCACODE_HANDLE: 374 /* verify handle value */ 375 if (cdnr_handle2cb(utca->tca_handle) == NULL) 376 return (-1); 377 break; 378 379 case TCACODE_NONE: 380 case TCACODE_RETURN: 381 case TCACODE_NEXT: 382 default: 383 /* should not be passed from a user */ 384 return (-1); 385 } 386 return (0); 387 } 388 389 static void 390 tca_import_action(ktca, utca) 391 struct tc_action *ktca, *utca; 392 { 393 struct cdnr_block *cb; 394 395 *ktca = *utca; 396 if (ktca->tca_code == TCACODE_HANDLE) { 397 cb = cdnr_handle2cb(ktca->tca_handle); 398 if (cb == NULL) { 399 ktca->tca_code = TCACODE_NONE; 400 return; 401 } 402 ktca->tca_code = TCACODE_NEXT; 403 ktca->tca_next = cb; 404 cb->cb_ref++; 405 } else if (ktca->tca_code == TCACODE_MARK) { 406 ktca->tca_dscp &= DSCP_MASK; 407 } 408 return; 409 } 410 411 static void 412 tca_invalidate_action(tca) 413 struct tc_action *tca; 414 { 415 struct cdnr_block *cb; 416 417 if (tca->tca_code == TCACODE_NEXT) { 418 cb = tca->tca_next; 419 if (cb == NULL) 420 return; 421 cb->cb_ref--; 422 } 423 tca->tca_code = TCACODE_NONE; 424 } 425 426 /* 427 * top level traffic conditioner 428 */ 429 static struct top_cdnr * 430 top_create(ifq) 431 struct ifaltq *ifq; 432 { 433 struct top_cdnr *top; 434 435 if ((top = cdnr_cballoc(NULL, TCETYPE_TOP, NULL)) == NULL) 436 return (NULL); 437 438 top->tc_ifq = ifq; 439 /* set default action for the top level conditioner */ 440 top->tc_block.cb_action.tca_code = TCACODE_PASS; 441 442 LIST_INSERT_HEAD(&tcb_list, top, tc_next); 443 444 ifq->altq_cdnr = top; 445 446 return (top); 447 } 448 449 static int 450 top_destroy(top) 451 struct top_cdnr *top; 452 { 453 struct cdnr_block *cb; 454 455 if (ALTQ_IS_CNDTNING(top->tc_ifq)) 456 ALTQ_CLEAR_CNDTNING(top->tc_ifq); 457 top->tc_ifq->altq_cdnr = NULL; 458 459 /* 460 * destroy all the conditioner elements belonging to this interface 461 */ 462 while ((cb = LIST_FIRST(&top->tc_elements)) != NULL) { 463 while (cb != NULL && cb->cb_ref > 0) 464 cb = LIST_NEXT(cb, cb_next); 465 if (cb != NULL) 466 generic_element_destroy(cb); 467 } 468 469 LIST_REMOVE(top, tc_next); 470 471 cdnr_cbdestroy(top); 472 473 /* if there is no active conditioner, remove the input hook */ 474 if (altq_input != NULL) { 475 LIST_FOREACH(top, &tcb_list, tc_next) 476 if (ALTQ_IS_CNDTNING(top->tc_ifq)) 477 break; 478 if (top == NULL) 479 altq_input = NULL; 480 } 481 482 return (0); 483 } 484 485 /* 486 * simple tc elements without input function (e.g., dropper and makers). 487 */ 488 static struct cdnr_block * 489 element_create(top, action) 490 struct top_cdnr *top; 491 struct tc_action *action; 492 { 493 struct cdnr_block *cb; 494 495 if (tca_verify_action(action) < 0) 496 return (NULL); 497 498 if ((cb = cdnr_cballoc(top, TCETYPE_ELEMENT, NULL)) == NULL) 499 return (NULL); 500 501 tca_import_action(&cb->cb_action, action); 502 503 return (cb); 504 } 505 506 static int 507 element_destroy(cb) 508 struct cdnr_block *cb; 509 { 510 if (cb->cb_ref > 0) 511 return (EBUSY); 512 513 tca_invalidate_action(&cb->cb_action); 514 515 cdnr_cbdestroy(cb); 516 return (0); 517 } 518 519 /* 520 * internal representation of token bucket parameters 521 * rate: byte_per_unittime << 32 522 * (((bits_per_sec) / 8) << 32) / machclk_freq 523 * depth: byte << 32 524 * 525 */ 526 #define TB_SHIFT 32 527 #define TB_SCALE(x) ((u_int64_t)(x) << TB_SHIFT) 528 #define TB_UNSCALE(x) ((x) >> TB_SHIFT) 529 530 static void 531 tb_import_profile(tb, profile) 532 struct tbe *tb; 533 struct tb_profile *profile; 534 { 535 tb->rate = TB_SCALE(profile->rate / 8) / machclk_freq; 536 tb->depth = TB_SCALE(profile->depth); 537 if (tb->rate > 0) 538 tb->filluptime = tb->depth / tb->rate; 539 else 540 tb->filluptime = 0xffffffffffffffffLL; 541 tb->token = tb->depth; 542 tb->last = read_machclk(); 543 } 544 545 /* 546 * simple token bucket meter 547 */ 548 static struct tbmeter * 549 tbm_create(top, profile, in_action, out_action) 550 struct top_cdnr *top; 551 struct tb_profile *profile; 552 struct tc_action *in_action, *out_action; 553 { 554 struct tbmeter *tbm = NULL; 555 556 if (tca_verify_action(in_action) < 0 557 || tca_verify_action(out_action) < 0) 558 return (NULL); 559 560 if ((tbm = cdnr_cballoc(top, TCETYPE_TBMETER, 561 tbm_input)) == NULL) 562 return (NULL); 563 564 tb_import_profile(&tbm->tb, profile); 565 566 tca_import_action(&tbm->in_action, in_action); 567 tca_import_action(&tbm->out_action, out_action); 568 569 return (tbm); 570 } 571 572 static int 573 tbm_destroy(tbm) 574 struct tbmeter *tbm; 575 { 576 if (tbm->cdnrblk.cb_ref > 0) 577 return (EBUSY); 578 579 tca_invalidate_action(&tbm->in_action); 580 tca_invalidate_action(&tbm->out_action); 581 582 cdnr_cbdestroy(tbm); 583 return (0); 584 } 585 586 static struct tc_action * 587 tbm_input(cb, pktinfo) 588 struct cdnr_block *cb; 589 struct cdnr_pktinfo *pktinfo; 590 { 591 struct tbmeter *tbm = (struct tbmeter *)cb; 592 u_int64_t len; 593 u_int64_t interval, now; 594 595 len = TB_SCALE(pktinfo->pkt_len); 596 597 if (tbm->tb.token < len) { 598 now = read_machclk(); 599 interval = now - tbm->tb.last; 600 if (interval >= tbm->tb.filluptime) 601 tbm->tb.token = tbm->tb.depth; 602 else { 603 tbm->tb.token += interval * tbm->tb.rate; 604 if (tbm->tb.token > tbm->tb.depth) 605 tbm->tb.token = tbm->tb.depth; 606 } 607 tbm->tb.last = now; 608 } 609 610 if (tbm->tb.token < len) { 611 PKTCNTR_ADD(&tbm->out_cnt, pktinfo->pkt_len); 612 return (&tbm->out_action); 613 } 614 615 tbm->tb.token -= len; 616 PKTCNTR_ADD(&tbm->in_cnt, pktinfo->pkt_len); 617 return (&tbm->in_action); 618 } 619 620 /* 621 * two rate three color marker 622 * as described in draft-heinanen-diffserv-trtcm-01.txt 623 */ 624 static struct trtcm * 625 trtcm_create(top, cmtd_profile, peak_profile, 626 green_action, yellow_action, red_action, coloraware) 627 struct top_cdnr *top; 628 struct tb_profile *cmtd_profile, *peak_profile; 629 struct tc_action *green_action, *yellow_action, *red_action; 630 int coloraware; 631 { 632 struct trtcm *tcm = NULL; 633 634 if (tca_verify_action(green_action) < 0 635 || tca_verify_action(yellow_action) < 0 636 || tca_verify_action(red_action) < 0) 637 return (NULL); 638 639 if ((tcm = cdnr_cballoc(top, TCETYPE_TRTCM, 640 trtcm_input)) == NULL) 641 return (NULL); 642 643 tb_import_profile(&tcm->cmtd_tb, cmtd_profile); 644 tb_import_profile(&tcm->peak_tb, peak_profile); 645 646 tca_import_action(&tcm->green_action, green_action); 647 tca_import_action(&tcm->yellow_action, yellow_action); 648 tca_import_action(&tcm->red_action, red_action); 649 650 /* set dscps to use */ 651 if (tcm->green_action.tca_code == TCACODE_MARK) 652 tcm->green_dscp = tcm->green_action.tca_dscp & DSCP_MASK; 653 else 654 tcm->green_dscp = DSCP_AF11; 655 if (tcm->yellow_action.tca_code == TCACODE_MARK) 656 tcm->yellow_dscp = tcm->yellow_action.tca_dscp & DSCP_MASK; 657 else 658 tcm->yellow_dscp = DSCP_AF12; 659 if (tcm->red_action.tca_code == TCACODE_MARK) 660 tcm->red_dscp = tcm->red_action.tca_dscp & DSCP_MASK; 661 else 662 tcm->red_dscp = DSCP_AF13; 663 664 tcm->coloraware = coloraware; 665 666 return (tcm); 667 } 668 669 static int 670 trtcm_destroy(tcm) 671 struct trtcm *tcm; 672 { 673 if (tcm->cdnrblk.cb_ref > 0) 674 return (EBUSY); 675 676 tca_invalidate_action(&tcm->green_action); 677 tca_invalidate_action(&tcm->yellow_action); 678 tca_invalidate_action(&tcm->red_action); 679 680 cdnr_cbdestroy(tcm); 681 return (0); 682 } 683 684 static struct tc_action * 685 trtcm_input(cb, pktinfo) 686 struct cdnr_block *cb; 687 struct cdnr_pktinfo *pktinfo; 688 { 689 struct trtcm *tcm = (struct trtcm *)cb; 690 u_int64_t len; 691 u_int64_t interval, now; 692 u_int8_t color; 693 694 len = TB_SCALE(pktinfo->pkt_len); 695 if (tcm->coloraware) { 696 color = pktinfo->pkt_dscp; 697 if (color != tcm->yellow_dscp && color != tcm->red_dscp) 698 color = tcm->green_dscp; 699 } else { 700 /* if color-blind, precolor it as green */ 701 color = tcm->green_dscp; 702 } 703 704 now = read_machclk(); 705 if (tcm->cmtd_tb.token < len) { 706 interval = now - tcm->cmtd_tb.last; 707 if (interval >= tcm->cmtd_tb.filluptime) 708 tcm->cmtd_tb.token = tcm->cmtd_tb.depth; 709 else { 710 tcm->cmtd_tb.token += interval * tcm->cmtd_tb.rate; 711 if (tcm->cmtd_tb.token > tcm->cmtd_tb.depth) 712 tcm->cmtd_tb.token = tcm->cmtd_tb.depth; 713 } 714 tcm->cmtd_tb.last = now; 715 } 716 if (tcm->peak_tb.token < len) { 717 interval = now - tcm->peak_tb.last; 718 if (interval >= tcm->peak_tb.filluptime) 719 tcm->peak_tb.token = tcm->peak_tb.depth; 720 else { 721 tcm->peak_tb.token += interval * tcm->peak_tb.rate; 722 if (tcm->peak_tb.token > tcm->peak_tb.depth) 723 tcm->peak_tb.token = tcm->peak_tb.depth; 724 } 725 tcm->peak_tb.last = now; 726 } 727 728 if (color == tcm->red_dscp || tcm->peak_tb.token < len) { 729 pktinfo->pkt_dscp = tcm->red_dscp; 730 PKTCNTR_ADD(&tcm->red_cnt, pktinfo->pkt_len); 731 return (&tcm->red_action); 732 } 733 734 if (color == tcm->yellow_dscp || tcm->cmtd_tb.token < len) { 735 pktinfo->pkt_dscp = tcm->yellow_dscp; 736 tcm->peak_tb.token -= len; 737 PKTCNTR_ADD(&tcm->yellow_cnt, pktinfo->pkt_len); 738 return (&tcm->yellow_action); 739 } 740 741 pktinfo->pkt_dscp = tcm->green_dscp; 742 tcm->cmtd_tb.token -= len; 743 tcm->peak_tb.token -= len; 744 PKTCNTR_ADD(&tcm->green_cnt, pktinfo->pkt_len); 745 return (&tcm->green_action); 746 } 747 748 /* 749 * time sliding window three color marker 750 * as described in draft-fang-diffserv-tc-tswtcm-00.txt 751 */ 752 static struct tswtcm * 753 tswtcm_create(top, cmtd_rate, peak_rate, avg_interval, 754 green_action, yellow_action, red_action) 755 struct top_cdnr *top; 756 u_int32_t cmtd_rate, peak_rate, avg_interval; 757 struct tc_action *green_action, *yellow_action, *red_action; 758 { 759 struct tswtcm *tsw; 760 761 if (tca_verify_action(green_action) < 0 762 || tca_verify_action(yellow_action) < 0 763 || tca_verify_action(red_action) < 0) 764 return (NULL); 765 766 if ((tsw = cdnr_cballoc(top, TCETYPE_TSWTCM, 767 tswtcm_input)) == NULL) 768 return (NULL); 769 770 tca_import_action(&tsw->green_action, green_action); 771 tca_import_action(&tsw->yellow_action, yellow_action); 772 tca_import_action(&tsw->red_action, red_action); 773 774 /* set dscps to use */ 775 if (tsw->green_action.tca_code == TCACODE_MARK) 776 tsw->green_dscp = tsw->green_action.tca_dscp & DSCP_MASK; 777 else 778 tsw->green_dscp = DSCP_AF11; 779 if (tsw->yellow_action.tca_code == TCACODE_MARK) 780 tsw->yellow_dscp = tsw->yellow_action.tca_dscp & DSCP_MASK; 781 else 782 tsw->yellow_dscp = DSCP_AF12; 783 if (tsw->red_action.tca_code == TCACODE_MARK) 784 tsw->red_dscp = tsw->red_action.tca_dscp & DSCP_MASK; 785 else 786 tsw->red_dscp = DSCP_AF13; 787 788 /* convert rates from bits/sec to bytes/sec */ 789 tsw->cmtd_rate = cmtd_rate / 8; 790 tsw->peak_rate = peak_rate / 8; 791 tsw->avg_rate = 0; 792 793 /* timewin is converted from msec to machine clock unit */ 794 tsw->timewin = (u_int64_t)machclk_freq * avg_interval / 1000; 795 796 return (tsw); 797 } 798 799 static int 800 tswtcm_destroy(tsw) 801 struct tswtcm *tsw; 802 { 803 if (tsw->cdnrblk.cb_ref > 0) 804 return (EBUSY); 805 806 tca_invalidate_action(&tsw->green_action); 807 tca_invalidate_action(&tsw->yellow_action); 808 tca_invalidate_action(&tsw->red_action); 809 810 cdnr_cbdestroy(tsw); 811 return (0); 812 } 813 814 static struct tc_action * 815 tswtcm_input(cb, pktinfo) 816 struct cdnr_block *cb; 817 struct cdnr_pktinfo *pktinfo; 818 { 819 struct tswtcm *tsw = (struct tswtcm *)cb; 820 int len; 821 u_int32_t avg_rate; 822 u_int64_t interval, now, tmp; 823 824 /* 825 * rate estimator 826 */ 827 len = pktinfo->pkt_len; 828 now = read_machclk(); 829 830 interval = now - tsw->t_front; 831 /* 832 * calculate average rate: 833 * avg = (avg * timewin + pkt_len)/(timewin + interval) 834 * pkt_len needs to be multiplied by machclk_freq in order to 835 * get (bytes/sec). 836 * note: when avg_rate (bytes/sec) and timewin (machclk unit) are 837 * less than 32 bits, the following 64-bit operation has enough 838 * precision. 839 */ 840 tmp = ((u_int64_t)tsw->avg_rate * tsw->timewin 841 + (u_int64_t)len * machclk_freq) / (tsw->timewin + interval); 842 tsw->avg_rate = avg_rate = (u_int32_t)tmp; 843 tsw->t_front = now; 844 845 /* 846 * marker 847 */ 848 if (avg_rate > tsw->cmtd_rate) { 849 u_int32_t randval = random() % avg_rate; 850 851 if (avg_rate > tsw->peak_rate) { 852 if (randval < avg_rate - tsw->peak_rate) { 853 /* mark red */ 854 pktinfo->pkt_dscp = tsw->red_dscp; 855 PKTCNTR_ADD(&tsw->red_cnt, len); 856 return (&tsw->red_action); 857 } else if (randval < avg_rate - tsw->cmtd_rate) 858 goto mark_yellow; 859 } else { 860 /* peak_rate >= avg_rate > cmtd_rate */ 861 if (randval < avg_rate - tsw->cmtd_rate) { 862 mark_yellow: 863 pktinfo->pkt_dscp = tsw->yellow_dscp; 864 PKTCNTR_ADD(&tsw->yellow_cnt, len); 865 return (&tsw->yellow_action); 866 } 867 } 868 } 869 870 /* mark green */ 871 pktinfo->pkt_dscp = tsw->green_dscp; 872 PKTCNTR_ADD(&tsw->green_cnt, len); 873 return (&tsw->green_action); 874 } 875 876 /* 877 * ioctl requests 878 */ 879 static int 880 cdnrcmd_if_attach(ifname) 881 char *ifname; 882 { 883 struct ifnet *ifp; 884 struct top_cdnr *top; 885 886 if ((ifp = ifunit(ifname)) == NULL) 887 return (EBADF); 888 889 if (ifp->if_snd.altq_cdnr != NULL) 890 return (EBUSY); 891 892 if ((top = top_create(&ifp->if_snd)) == NULL) 893 return (ENOMEM); 894 return (0); 895 } 896 897 static int 898 cdnrcmd_if_detach(ifname) 899 char *ifname; 900 { 901 struct top_cdnr *top; 902 903 if ((top = tcb_lookup(ifname)) == NULL) 904 return (EBADF); 905 906 return top_destroy(top); 907 } 908 909 static int 910 cdnrcmd_add_element(ap) 911 struct cdnr_add_element *ap; 912 { 913 struct top_cdnr *top; 914 struct cdnr_block *cb; 915 916 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 917 return (EBADF); 918 919 cb = element_create(top, &ap->action); 920 if (cb == NULL) 921 return (EINVAL); 922 /* return a class handle to the user */ 923 ap->cdnr_handle = cdnr_cb2handle(cb); 924 return (0); 925 } 926 927 static int 928 cdnrcmd_delete_element(ap) 929 struct cdnr_delete_element *ap; 930 { 931 struct top_cdnr *top; 932 struct cdnr_block *cb; 933 934 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 935 return (EBADF); 936 937 if ((cb = cdnr_handle2cb(ap->cdnr_handle)) == NULL) 938 return (EINVAL); 939 940 if (cb->cb_type != TCETYPE_ELEMENT) 941 return generic_element_destroy(cb); 942 943 return element_destroy(cb); 944 } 945 946 static int 947 cdnrcmd_add_filter(ap) 948 struct cdnr_add_filter *ap; 949 { 950 struct top_cdnr *top; 951 struct cdnr_block *cb; 952 953 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 954 return (EBADF); 955 956 if ((cb = cdnr_handle2cb(ap->cdnr_handle)) == NULL) 957 return (EINVAL); 958 959 return acc_add_filter(&top->tc_classifier, &ap->filter, 960 cb, &ap->filter_handle); 961 } 962 963 static int 964 cdnrcmd_delete_filter(ap) 965 struct cdnr_delete_filter *ap; 966 { 967 struct top_cdnr *top; 968 969 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 970 return (EBADF); 971 972 return acc_delete_filter(&top->tc_classifier, ap->filter_handle); 973 } 974 975 static int 976 cdnrcmd_add_tbm(ap) 977 struct cdnr_add_tbmeter *ap; 978 { 979 struct top_cdnr *top; 980 struct tbmeter *tbm; 981 982 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 983 return (EBADF); 984 985 tbm = tbm_create(top, &ap->profile, &ap->in_action, &ap->out_action); 986 if (tbm == NULL) 987 return (EINVAL); 988 /* return a class handle to the user */ 989 ap->cdnr_handle = cdnr_cb2handle(&tbm->cdnrblk); 990 return (0); 991 } 992 993 static int 994 cdnrcmd_modify_tbm(ap) 995 struct cdnr_modify_tbmeter *ap; 996 { 997 struct tbmeter *tbm; 998 999 if ((tbm = (struct tbmeter *)cdnr_handle2cb(ap->cdnr_handle)) == NULL) 1000 return (EINVAL); 1001 1002 tb_import_profile(&tbm->tb, &ap->profile); 1003 1004 return (0); 1005 } 1006 1007 static int 1008 cdnrcmd_tbm_stats(ap) 1009 struct cdnr_tbmeter_stats *ap; 1010 { 1011 struct tbmeter *tbm; 1012 1013 if ((tbm = (struct tbmeter *)cdnr_handle2cb(ap->cdnr_handle)) == NULL) 1014 return (EINVAL); 1015 1016 ap->in_cnt = tbm->in_cnt; 1017 ap->out_cnt = tbm->out_cnt; 1018 1019 return (0); 1020 } 1021 1022 static int 1023 cdnrcmd_add_trtcm(ap) 1024 struct cdnr_add_trtcm *ap; 1025 { 1026 struct top_cdnr *top; 1027 struct trtcm *tcm; 1028 1029 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 1030 return (EBADF); 1031 1032 tcm = trtcm_create(top, &ap->cmtd_profile, &ap->peak_profile, 1033 &ap->green_action, &ap->yellow_action, 1034 &ap->red_action, ap->coloraware); 1035 if (tcm == NULL) 1036 return (EINVAL); 1037 1038 /* return a class handle to the user */ 1039 ap->cdnr_handle = cdnr_cb2handle(&tcm->cdnrblk); 1040 return (0); 1041 } 1042 1043 static int 1044 cdnrcmd_modify_trtcm(ap) 1045 struct cdnr_modify_trtcm *ap; 1046 { 1047 struct trtcm *tcm; 1048 1049 if ((tcm = (struct trtcm *)cdnr_handle2cb(ap->cdnr_handle)) == NULL) 1050 return (EINVAL); 1051 1052 tb_import_profile(&tcm->cmtd_tb, &ap->cmtd_profile); 1053 tb_import_profile(&tcm->peak_tb, &ap->peak_profile); 1054 1055 return (0); 1056 } 1057 1058 static int 1059 cdnrcmd_tcm_stats(ap) 1060 struct cdnr_tcm_stats *ap; 1061 { 1062 struct cdnr_block *cb; 1063 1064 if ((cb = cdnr_handle2cb(ap->cdnr_handle)) == NULL) 1065 return (EINVAL); 1066 1067 if (cb->cb_type == TCETYPE_TRTCM) { 1068 struct trtcm *tcm = (struct trtcm *)cb; 1069 1070 ap->green_cnt = tcm->green_cnt; 1071 ap->yellow_cnt = tcm->yellow_cnt; 1072 ap->red_cnt = tcm->red_cnt; 1073 } else if (cb->cb_type == TCETYPE_TSWTCM) { 1074 struct tswtcm *tsw = (struct tswtcm *)cb; 1075 1076 ap->green_cnt = tsw->green_cnt; 1077 ap->yellow_cnt = tsw->yellow_cnt; 1078 ap->red_cnt = tsw->red_cnt; 1079 } else 1080 return (EINVAL); 1081 1082 return (0); 1083 } 1084 1085 static int 1086 cdnrcmd_add_tswtcm(ap) 1087 struct cdnr_add_tswtcm *ap; 1088 { 1089 struct top_cdnr *top; 1090 struct tswtcm *tsw; 1091 1092 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 1093 return (EBADF); 1094 1095 if (ap->cmtd_rate > ap->peak_rate) 1096 return (EINVAL); 1097 1098 tsw = tswtcm_create(top, ap->cmtd_rate, ap->peak_rate, 1099 ap->avg_interval, &ap->green_action, 1100 &ap->yellow_action, &ap->red_action); 1101 if (tsw == NULL) 1102 return (EINVAL); 1103 1104 /* return a class handle to the user */ 1105 ap->cdnr_handle = cdnr_cb2handle(&tsw->cdnrblk); 1106 return (0); 1107 } 1108 1109 static int 1110 cdnrcmd_modify_tswtcm(ap) 1111 struct cdnr_modify_tswtcm *ap; 1112 { 1113 struct tswtcm *tsw; 1114 1115 if ((tsw = (struct tswtcm *)cdnr_handle2cb(ap->cdnr_handle)) == NULL) 1116 return (EINVAL); 1117 1118 if (ap->cmtd_rate > ap->peak_rate) 1119 return (EINVAL); 1120 1121 /* convert rates from bits/sec to bytes/sec */ 1122 tsw->cmtd_rate = ap->cmtd_rate / 8; 1123 tsw->peak_rate = ap->peak_rate / 8; 1124 tsw->avg_rate = 0; 1125 1126 /* timewin is converted from msec to machine clock unit */ 1127 tsw->timewin = (u_int64_t)machclk_freq * ap->avg_interval / 1000; 1128 1129 return (0); 1130 } 1131 1132 static int 1133 cdnrcmd_get_stats(ap) 1134 struct cdnr_get_stats *ap; 1135 { 1136 struct top_cdnr *top; 1137 struct cdnr_block *cb; 1138 struct tbmeter *tbm; 1139 struct trtcm *tcm; 1140 struct tswtcm *tsw; 1141 struct tce_stats tce, *usp; 1142 int error, n, nskip, nelements; 1143 1144 if ((top = tcb_lookup(ap->iface.cdnr_ifname)) == NULL) 1145 return (EBADF); 1146 1147 /* copy action stats */ 1148 bcopy(top->tc_cnts, ap->cnts, sizeof(ap->cnts)); 1149 1150 /* stats for each element */ 1151 nelements = ap->nelements; 1152 usp = ap->tce_stats; 1153 if (nelements <= 0 || usp == NULL) 1154 return (0); 1155 1156 nskip = ap->nskip; 1157 n = 0; 1158 LIST_FOREACH(cb, &top->tc_elements, cb_next) { 1159 if (nskip > 0) { 1160 nskip--; 1161 continue; 1162 } 1163 1164 bzero(&tce, sizeof(tce)); 1165 tce.tce_handle = cb->cb_handle; 1166 tce.tce_type = cb->cb_type; 1167 switch (cb->cb_type) { 1168 case TCETYPE_TBMETER: 1169 tbm = (struct tbmeter *)cb; 1170 tce.tce_cnts[0] = tbm->in_cnt; 1171 tce.tce_cnts[1] = tbm->out_cnt; 1172 break; 1173 case TCETYPE_TRTCM: 1174 tcm = (struct trtcm *)cb; 1175 tce.tce_cnts[0] = tcm->green_cnt; 1176 tce.tce_cnts[1] = tcm->yellow_cnt; 1177 tce.tce_cnts[2] = tcm->red_cnt; 1178 break; 1179 case TCETYPE_TSWTCM: 1180 tsw = (struct tswtcm *)cb; 1181 tce.tce_cnts[0] = tsw->green_cnt; 1182 tce.tce_cnts[1] = tsw->yellow_cnt; 1183 tce.tce_cnts[2] = tsw->red_cnt; 1184 break; 1185 default: 1186 continue; 1187 } 1188 1189 if ((error = copyout((caddr_t)&tce, (caddr_t)usp++, 1190 sizeof(tce))) != 0) 1191 return (error); 1192 1193 if (++n == nelements) 1194 break; 1195 } 1196 ap->nelements = n; 1197 1198 return (0); 1199 } 1200 1201 /* 1202 * conditioner device interface 1203 */ 1204 int 1205 cdnropen(dev, flag, fmt, p) 1206 dev_t dev; 1207 int flag, fmt; 1208 struct proc *p; 1209 { 1210 if (machclk_freq == 0) 1211 init_machclk(); 1212 1213 if (machclk_freq == 0) { 1214 printf("cdnr: no cpu clock available!\n"); 1215 return (ENXIO); 1216 } 1217 1218 /* everything will be done when the queueing scheme is attached. */ 1219 return 0; 1220 } 1221 1222 int 1223 cdnrclose(dev, flag, fmt, p) 1224 dev_t dev; 1225 int flag, fmt; 1226 struct proc *p; 1227 { 1228 struct top_cdnr *top; 1229 int err, error = 0; 1230 1231 while ((top = LIST_FIRST(&tcb_list)) != NULL) { 1232 /* destroy all */ 1233 err = top_destroy(top); 1234 if (err != 0 && error == 0) 1235 error = err; 1236 } 1237 altq_input = NULL; 1238 1239 return (error); 1240 } 1241 1242 int 1243 cdnrioctl(dev, cmd, addr, flag, p) 1244 dev_t dev; 1245 ioctlcmd_t cmd; 1246 caddr_t addr; 1247 int flag; 1248 struct proc *p; 1249 { 1250 struct top_cdnr *top; 1251 struct cdnr_interface *ifacep; 1252 int s, error = 0; 1253 1254 /* check super-user privilege */ 1255 switch (cmd) { 1256 case CDNR_GETSTATS: 1257 break; 1258 default: 1259 #if (__FreeBSD_version > 400000) 1260 if ((error = suser(p)) != 0) 1261 #else 1262 if ((error = suser(p->p_ucred, &p->p_acflag)) != 0) 1263 #endif 1264 return (error); 1265 break; 1266 } 1267 1268 s = splnet(); 1269 switch (cmd) { 1270 1271 case CDNR_IF_ATTACH: 1272 ifacep = (struct cdnr_interface *)addr; 1273 error = cdnrcmd_if_attach(ifacep->cdnr_ifname); 1274 break; 1275 1276 case CDNR_IF_DETACH: 1277 ifacep = (struct cdnr_interface *)addr; 1278 error = cdnrcmd_if_detach(ifacep->cdnr_ifname); 1279 break; 1280 1281 case CDNR_ENABLE: 1282 case CDNR_DISABLE: 1283 ifacep = (struct cdnr_interface *)addr; 1284 if ((top = tcb_lookup(ifacep->cdnr_ifname)) == NULL) { 1285 error = EBADF; 1286 break; 1287 } 1288 1289 switch (cmd) { 1290 1291 case CDNR_ENABLE: 1292 ALTQ_SET_CNDTNING(top->tc_ifq); 1293 if (altq_input == NULL) 1294 altq_input = altq_cdnr_input; 1295 break; 1296 1297 case CDNR_DISABLE: 1298 ALTQ_CLEAR_CNDTNING(top->tc_ifq); 1299 LIST_FOREACH(top, &tcb_list, tc_next) 1300 if (ALTQ_IS_CNDTNING(top->tc_ifq)) 1301 break; 1302 if (top == NULL) 1303 altq_input = NULL; 1304 break; 1305 } 1306 break; 1307 1308 case CDNR_ADD_ELEM: 1309 error = cdnrcmd_add_element((struct cdnr_add_element *)addr); 1310 break; 1311 1312 case CDNR_DEL_ELEM: 1313 error = cdnrcmd_delete_element((struct cdnr_delete_element *)addr); 1314 break; 1315 1316 case CDNR_ADD_TBM: 1317 error = cdnrcmd_add_tbm((struct cdnr_add_tbmeter *)addr); 1318 break; 1319 1320 case CDNR_MOD_TBM: 1321 error = cdnrcmd_modify_tbm((struct cdnr_modify_tbmeter *)addr); 1322 break; 1323 1324 case CDNR_TBM_STATS: 1325 error = cdnrcmd_tbm_stats((struct cdnr_tbmeter_stats *)addr); 1326 break; 1327 1328 case CDNR_ADD_TCM: 1329 error = cdnrcmd_add_trtcm((struct cdnr_add_trtcm *)addr); 1330 break; 1331 1332 case CDNR_MOD_TCM: 1333 error = cdnrcmd_modify_trtcm((struct cdnr_modify_trtcm *)addr); 1334 break; 1335 1336 case CDNR_TCM_STATS: 1337 error = cdnrcmd_tcm_stats((struct cdnr_tcm_stats *)addr); 1338 break; 1339 1340 case CDNR_ADD_FILTER: 1341 error = cdnrcmd_add_filter((struct cdnr_add_filter *)addr); 1342 break; 1343 1344 case CDNR_DEL_FILTER: 1345 error = cdnrcmd_delete_filter((struct cdnr_delete_filter *)addr); 1346 break; 1347 1348 case CDNR_GETSTATS: 1349 error = cdnrcmd_get_stats((struct cdnr_get_stats *)addr); 1350 break; 1351 1352 case CDNR_ADD_TSW: 1353 error = cdnrcmd_add_tswtcm((struct cdnr_add_tswtcm *)addr); 1354 break; 1355 1356 case CDNR_MOD_TSW: 1357 error = cdnrcmd_modify_tswtcm((struct cdnr_modify_tswtcm *)addr); 1358 break; 1359 1360 default: 1361 error = EINVAL; 1362 break; 1363 } 1364 splx(s); 1365 1366 return error; 1367 } 1368 1369 #ifdef KLD_MODULE 1370 1371 static struct altqsw cdnr_sw = 1372 {"cdnr", cdnropen, cdnrclose, cdnrioctl}; 1373 1374 ALTQ_MODULE(altq_cdnr, ALTQT_CDNR, &cdnr_sw); 1375 1376 #endif /* KLD_MODULE */ 1377 1378 #endif /* ALTQ_CDNR */ 1379