sys/net/altq/altq_codel.c

   1 /*
   2  * CoDel - The Controlled-Delay Active Queue Management algorithm
   3  *
   4  *  Copyright (C) 2013 Ermal Luçi <eri@FreeBSD.org>
   5  *  Copyright (C) 2011-2012 Kathleen Nichols <nichols@pollere.com>
   6  *  Copyright (C) 2011-2012 Van Jacobson <van@pollere.net>
   7  *  Copyright (C) 2012 Michael D. Taht <dave.taht@bufferbloat.net>
   8  *  Copyright (C) 2012 Eric Dumazet <edumazet@google.com>
   9  *
  10  * Redistribution and use in source and binary forms, with or without
  11  * modification, are permitted provided that the following conditions
  12  * are met:
  13  * 1. Redistributions of source code must retain the above copyright
  14  *    notice, this list of conditions, and the following disclaimer,
  15  *    without modification.
  16  * 2. Redistributions in binary form must reproduce the above copyright
  17  *    notice, this list of conditions and the following disclaimer in the
  18  *    documentation and/or other materials provided with the distribution.
  19  * 3. The names of the authors may not be used to endorse or promote products
  20  *    derived from this software without specific prior written permission.
  21  *
  22  * Alternatively, provided that this notice is retained in full, this
  23  * software may be distributed under the terms of the GNU General
  24  * Public License ("GPL") version 2, in which case the provisions of the
  25  * GPL apply INSTEAD OF those given above.
  26  *
  27  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  30  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  31  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  32  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  33  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  34  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  35  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  36  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  37  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  38  * DAMAGE.
  39  *
  40  * $FreeBSD$
  41  */
  42 #include "opt_altq.h"
  43 #include "opt_inet.h"
  44 #include "opt_inet6.h"
  45
  46 #ifdef ALTQ_CODEL  /* CoDel is enabled by ALTQ_CODEL option in opt_altq.h */
  47
  48 #include <sys/param.h>
  49 #include <sys/malloc.h>
  50 #include <sys/mbuf.h>
  51 #include <sys/socket.h>
  52 #include <sys/systm.h>
  53
  54 #include <net/if.h>
  55 #include <net/if_var.h>
  56 #include <netinet/in.h>
  57
  58 #include <netpfil/pf/pf.h>
  59 #include <netpfil/pf/pf_altq.h>
  60 #include <net/altq/if_altq.h>
  61 #include <net/altq/altq.h>
  62 #include <net/altq/altq_codel.h>
  63
  64 static int               codel_should_drop(struct codel *, class_queue_t *,
  65                             struct mbuf *, u_int64_t);
  66 static void              codel_Newton_step(struct codel_vars *);
  67 static u_int64_t         codel_control_law(u_int64_t t, u_int64_t, u_int32_t);
  68
  69 #define codel_time_after(a, b)          ((int64_t)(a) - (int64_t)(b) > 0)
  70 #define codel_time_after_eq(a, b)       ((int64_t)(a) - (int64_t)(b) >= 0)
  71 #define codel_time_before(a, b)         ((int64_t)(a) - (int64_t)(b) < 0)
  72 #define codel_time_before_eq(a, b)      ((int64_t)(a) - (int64_t)(b) <= 0)
  73
  74 static int codel_request(struct ifaltq *, int, void *);
  75
  76 static int codel_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
  77 static struct mbuf *codel_dequeue(struct ifaltq *, int);
  78
  79 int
  80 codel_pfattach(struct pf_altq *a)
  81 {
  82         struct ifnet *ifp;
  83
  84         if ((ifp = ifunit(a->ifname)) == NULL || a->altq_disc == NULL)
  85                 return (EINVAL);
  86
  87         return (altq_attach(&ifp->if_snd, ALTQT_CODEL, a->altq_disc,
  88             codel_enqueue, codel_dequeue, codel_request, NULL, NULL));
  89 }
  90
  91 int
  92 codel_add_altq(struct ifnet *ifp, struct pf_altq *a)
  93 {
  94         struct codel_if *cif;
  95         struct codel_opts       *opts;
  96
  97         if (ifp == NULL)
  98                 return (EINVAL);
  99         if (!ALTQ_IS_READY(&ifp->if_snd))
 100                 return (ENODEV);
 101
 102         opts = &a->pq_u.codel_opts;
 103
 104         cif = malloc(sizeof(struct codel_if), M_DEVBUF, M_NOWAIT | M_ZERO);
 105         if (cif == NULL)
 106                 return (ENOMEM);
 107         cif->cif_bandwidth = a->ifbandwidth;
 108         cif->cif_ifq = &ifp->if_snd;
 109
 110         cif->cl_q = malloc(sizeof(class_queue_t), M_DEVBUF, M_NOWAIT | M_ZERO);
 111         if (cif->cl_q == NULL) {
 112                 free(cif, M_DEVBUF);
 113                 return (ENOMEM);
 114         }
 115
 116         if (a->qlimit == 0)
 117                 a->qlimit = 50; /* use default. */
 118         qlimit(cif->cl_q) = a->qlimit;
 119         qtype(cif->cl_q) = Q_CODEL;
 120         qlen(cif->cl_q) = 0;
 121         qsize(cif->cl_q) = 0;
 122
 123         if (opts->target == 0)
 124                 opts->target = 5;
 125         if (opts->interval == 0)
 126                 opts->interval = 100;
 127         cif->codel.params.target = machclk_freq * opts->target / 1000;
 128         cif->codel.params.interval = machclk_freq * opts->interval / 1000;
 129         cif->codel.params.ecn = opts->ecn;
 130         cif->codel.stats.maxpacket = 256;
 131
 132         cif->cl_stats.qlength = qlen(cif->cl_q);
 133         cif->cl_stats.qlimit = qlimit(cif->cl_q);
 134
 135         /* keep the state in pf_altq */
 136         a->altq_disc = cif;
 137
 138         return (0);
 139 }
 140
 141 int
 142 codel_remove_altq(struct pf_altq *a)
 143 {
 144         struct codel_if *cif;
 145
 146         if ((cif = a->altq_disc) == NULL)
 147                 return (EINVAL);
 148         a->altq_disc = NULL;
 149
 150         if (cif->cl_q)
 151                 free(cif->cl_q, M_DEVBUF);
 152         free(cif, M_DEVBUF);
 153
 154         return (0);
 155 }
 156
 157 int
 158 codel_getqstats(struct pf_altq *a, void *ubuf, int *nbytes, int version)
 159 {
 160         struct codel_if *cif;
 161         struct codel_ifstats stats;
 162         int error = 0;
 163
 164         if ((cif = altq_lookup(a->ifname, ALTQT_CODEL)) == NULL)
 165                 return (EBADF);
 166
 167         if (*nbytes < sizeof(stats))
 168                 return (EINVAL);
 169
 170         stats = cif->cl_stats;
 171         stats.stats = cif->codel.stats;
 172
 173         if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
 174                 return (error);
 175         *nbytes = sizeof(stats);
 176
 177         return (0);
 178 }
 179
 180 static int
 181 codel_request(struct ifaltq *ifq, int req, void *arg)
 182 {
 183         struct codel_if *cif = (struct codel_if *)ifq->altq_disc;
 184         struct mbuf *m;
 185
 186         IFQ_LOCK_ASSERT(ifq);
 187
 188         switch (req) {
 189         case ALTRQ_PURGE:
 190                 if (!ALTQ_IS_ENABLED(cif->cif_ifq))
 191                         break;
 192
 193                 if (qempty(cif->cl_q))
 194                         break;
 195
 196                 while ((m = _getq(cif->cl_q)) != NULL) {
 197                         PKTCNTR_ADD(&cif->cl_stats.cl_dropcnt, m_pktlen(m));
 198                         m_freem(m);
 199                         IFQ_DEC_LEN(cif->cif_ifq);
 200                 }
 201                 cif->cif_ifq->ifq_len = 0;
 202                 break;
 203         }
 204
 205         return (0);
 206 }
 207
 208 static int
 209 codel_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
 210 {
 211
 212         struct codel_if *cif = (struct codel_if *) ifq->altq_disc;
 213
 214         IFQ_LOCK_ASSERT(ifq);
 215
 216         /* grab class set by classifier */
 217         if ((m->m_flags & M_PKTHDR) == 0) {
 218                 /* should not happen */
 219                 printf("altq: packet for %s does not have pkthdr\n",
 220                    ifq->altq_ifp->if_xname);
 221                 m_freem(m);
 222                 PKTCNTR_ADD(&cif->cl_stats.cl_dropcnt, m_pktlen(m));
 223                 return (ENOBUFS);
 224         }
 225
 226         if (codel_addq(&cif->codel, cif->cl_q, m)) {
 227                 PKTCNTR_ADD(&cif->cl_stats.cl_dropcnt, m_pktlen(m));
 228                 return (ENOBUFS);
 229         }
 230         IFQ_INC_LEN(ifq);
 231
 232         return (0);
 233 }
 234
 235 static struct mbuf *
 236 codel_dequeue(struct ifaltq *ifq, int op)
 237 {
 238         struct codel_if *cif = (struct codel_if *)ifq->altq_disc;
 239         struct mbuf *m;
 240
 241         IFQ_LOCK_ASSERT(ifq);
 242
 243         if (IFQ_IS_EMPTY(ifq))
 244                 return (NULL);
 245
 246         if (op == ALTDQ_POLL)
 247                 return (qhead(cif->cl_q));
 248
 249         m = codel_getq(&cif->codel, cif->cl_q);
 250         if (m != NULL) {
 251                 IFQ_DEC_LEN(ifq);
 252                 PKTCNTR_ADD(&cif->cl_stats.cl_xmitcnt, m_pktlen(m));
 253                 return (m);
 254         }
 255
 256         return (NULL);
 257 }
 258
 259 struct codel *
 260 codel_alloc(int target, int interval, int ecn)
 261 {
 262         struct codel *c;
 263
 264         c = malloc(sizeof(*c), M_DEVBUF, M_NOWAIT | M_ZERO);
 265         if (c != NULL) {
 266                 c->params.target = machclk_freq * target / 1000;
 267                 c->params.interval = machclk_freq * interval / 1000;
 268                 c->params.ecn = ecn;
 269                 c->stats.maxpacket = 256;
 270         }
 271
 272         return (c);
 273 }
 274
 275 void
 276 codel_destroy(struct codel *c)
 277 {
 278
 279         free(c, M_DEVBUF);
 280 }
 281
 282 #define MTAG_CODEL      1438031249
 283 int
 284 codel_addq(struct codel *c, class_queue_t *q, struct mbuf *m)
 285 {
 286         struct m_tag *mtag;
 287         uint64_t *enqueue_time;
 288
 289         if (qlen(q) < qlimit(q)) {
 290                 mtag = m_tag_locate(m, MTAG_CODEL, 0, NULL);
 291                 if (mtag == NULL)
 292                         mtag = m_tag_alloc(MTAG_CODEL, 0, sizeof(uint64_t),
 293                             M_NOWAIT);
 294                 if (mtag == NULL) {
 295                         m_freem(m);
 296                         return (-1);
 297                 }
 298                 enqueue_time = (uint64_t *)(mtag + 1);
 299                 *enqueue_time = read_machclk();
 300                 m_tag_prepend(m, mtag);
 301                 _addq(q, m);
 302                 return (0);
 303         }
 304         c->drop_overlimit++;
 305         m_freem(m);
 306
 307         return (-1);
 308 }
 309
 310 static int
 311 codel_should_drop(struct codel *c, class_queue_t *q, struct mbuf *m,
 312     u_int64_t now)
 313 {
 314         struct m_tag *mtag;
 315         uint64_t *enqueue_time;
 316
 317         if (m == NULL) {
 318                 c->vars.first_above_time = 0;
 319                 return (0);
 320         }
 321
 322         mtag = m_tag_locate(m, MTAG_CODEL, 0, NULL);
 323         if (mtag == NULL) {
 324                 /* Only one warning per second. */
 325                 if (ppsratecheck(&c->last_log, &c->last_pps, 1))
 326                         printf("%s: could not found the packet mtag!\n",
 327                             __func__);
 328                 c->vars.first_above_time = 0;
 329                 return (0);
 330         }
 331         enqueue_time = (uint64_t *)(mtag + 1);
 332         c->vars.ldelay = now - *enqueue_time;
 333         c->stats.maxpacket = MAX(c->stats.maxpacket, m_pktlen(m));
 334
 335         if (codel_time_before(c->vars.ldelay, c->params.target) ||
 336             qsize(q) <= c->stats.maxpacket) {
 337                 /* went below - stay below for at least interval */
 338                 c->vars.first_above_time = 0;
 339                 return (0);
 340         }
 341         if (c->vars.first_above_time == 0) {
 342                 /* just went above from below. If we stay above
 343                  * for at least interval we'll say it's ok to drop
 344                  */
 345                 c->vars.first_above_time = now + c->params.interval;
 346                 return (0);
 347         }
 348         if (codel_time_after(now, c->vars.first_above_time))
 349                 return (1);
 350
 351         return (0);
 352 }
 353
 354 /*
 355  * Run a Newton method step:
 356  * new_invsqrt = (invsqrt / 2) * (3 - count * invsqrt^2)
 357  *
 358  * Here, invsqrt is a fixed point number (< 1.0), 32bit mantissa, aka Q0.32
 359  */
 360 static void
 361 codel_Newton_step(struct codel_vars *vars)
 362 {
 363         uint32_t invsqrt, invsqrt2;
 364         uint64_t val;
 365
 366 /* sizeof_in_bits(rec_inv_sqrt) */
 367 #define REC_INV_SQRT_BITS (8 * sizeof(u_int16_t))
 368 /* needed shift to get a Q0.32 number from rec_inv_sqrt */
 369 #define REC_INV_SQRT_SHIFT (32 - REC_INV_SQRT_BITS)
 370
 371         invsqrt = ((u_int32_t)vars->rec_inv_sqrt) << REC_INV_SQRT_SHIFT;
 372         invsqrt2 = ((u_int64_t)invsqrt * invsqrt) >> 32;
 373         val = (3LL << 32) - ((u_int64_t)vars->count * invsqrt2);
 374         val >>= 2; /* avoid overflow in following multiply */
 375         val = (val * invsqrt) >> (32 - 2 + 1);
 376
 377         vars->rec_inv_sqrt = val >> REC_INV_SQRT_SHIFT;
 378 }
 379
 380 static u_int64_t
 381 codel_control_law(u_int64_t t, u_int64_t interval, u_int32_t rec_inv_sqrt)
 382 {
 383
 384         return (t + (u_int32_t)(((u_int64_t)interval *
 385             (rec_inv_sqrt << REC_INV_SQRT_SHIFT)) >> 32));
 386 }
 387
 388 struct mbuf *
 389 codel_getq(struct codel *c, class_queue_t *q)
 390 {
 391         struct mbuf     *m;
 392         u_int64_t        now;
 393         int              drop;
 394
 395         if ((m = _getq(q)) == NULL) {
 396                 c->vars.dropping = 0;
 397                 return (m);
 398         }
 399
 400         now = read_machclk();
 401         drop = codel_should_drop(c, q, m, now);
 402         if (c->vars.dropping) {
 403                 if (!drop) {
 404                         /* sojourn time below target - leave dropping state */
 405                         c->vars.dropping = 0;
 406                 } else if (codel_time_after_eq(now, c->vars.drop_next)) {
 407                         /* It's time for the next drop. Drop the current
 408                          * packet and dequeue the next. The dequeue might
 409                          * take us out of dropping state.
 410                          * If not, schedule the next drop.
 411                          * A large backlog might result in drop rates so high
 412                          * that the next drop should happen now,
 413                          * hence the while loop.
 414                          */
 415                         while (c->vars.dropping &&
 416                             codel_time_after_eq(now, c->vars.drop_next)) {
 417                                 c->vars.count++; /* don't care of possible wrap
 418                                                   * since there is no more
 419                                                   * divide */
 420                                 codel_Newton_step(&c->vars);
 421                                 /* TODO ECN */
 422                                 PKTCNTR_ADD(&c->stats.drop_cnt, m_pktlen(m));
 423                                 m_freem(m);
 424                                 m = _getq(q);
 425                                 if (!codel_should_drop(c, q, m, now))
 426                                         /* leave dropping state */
 427                                         c->vars.dropping = 0;
 428                                 else
 429                                         /* and schedule the next drop */
 430                                         c->vars.drop_next =
 431                                             codel_control_law(c->vars.drop_next,
 432                                                 c->params.interval,
 433                                                 c->vars.rec_inv_sqrt);
 434                         }
 435                 }
 436         } else if (drop) {
 437                 /* TODO ECN */
 438                 PKTCNTR_ADD(&c->stats.drop_cnt, m_pktlen(m));
 439                 m_freem(m);
 440
 441                 m = _getq(q);
 442                 drop = codel_should_drop(c, q, m, now);
 443
 444                 c->vars.dropping = 1;
 445                 /* if min went above target close to when we last went below it
 446                  * assume that the drop rate that controlled the queue on the
 447                  * last cycle is a good starting point to control it now.
 448                  */
 449                 if (codel_time_before(now - c->vars.drop_next,
 450                     16 * c->params.interval)) {
 451                         c->vars.count = (c->vars.count - c->vars.lastcount) | 1;
 452                         /* we dont care if rec_inv_sqrt approximation
 453                          * is not very precise :
 454                          * Next Newton steps will correct it quadratically.
 455                          */
 456                         codel_Newton_step(&c->vars);
 457                 } else {
 458                         c->vars.count = 1;
 459                         c->vars.rec_inv_sqrt = ~0U >> REC_INV_SQRT_SHIFT;
 460                 }
 461                 c->vars.lastcount = c->vars.count;
 462                 c->vars.drop_next = codel_control_law(now, c->params.interval,
 463                     c->vars.rec_inv_sqrt);
 464         }
 465
 466         return (m);
 467 }
 468
 469 void
 470 codel_getstats(struct codel *c, struct codel_stats *s)
 471 {
 472         *s = c->stats;
 473 }
 474
 475 #endif /* ALTQ_CODEL */