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
 250         m = codel_getq(&cif->codel, cif->cl_q);
 251         if (m != NULL) {
 252                 IFQ_DEC_LEN(ifq);
 253                 PKTCNTR_ADD(&cif->cl_stats.cl_xmitcnt, m_pktlen(m));
 254                 return (m);
 255         }
 256
 257         return (NULL);
 258 }
 259
 260 struct codel *
 261 codel_alloc(int target, int interval, int ecn)
 262 {
 263         struct codel *c;
 264
 265         c = malloc(sizeof(*c), M_DEVBUF, M_NOWAIT | M_ZERO);
 266         if (c != NULL) {
 267                 c->params.target = machclk_freq * target / 1000;
 268                 c->params.interval = machclk_freq * interval / 1000;
 269                 c->params.ecn = ecn;
 270                 c->stats.maxpacket = 256;
 271         }
 272
 273         return (c);
 274 }
 275
 276 void
 277 codel_destroy(struct codel *c)
 278 {
 279
 280         free(c, M_DEVBUF);
 281 }
 282
 283 #define MTAG_CODEL      1438031249
 284 int
 285 codel_addq(struct codel *c, class_queue_t *q, struct mbuf *m)
 286 {
 287         struct m_tag *mtag;
 288         uint64_t *enqueue_time;
 289
 290         if (qlen(q) < qlimit(q)) {
 291                 mtag = m_tag_locate(m, MTAG_CODEL, 0, NULL);
 292                 if (mtag == NULL)
 293                         mtag = m_tag_alloc(MTAG_CODEL, 0, sizeof(uint64_t),
 294                             M_NOWAIT);
 295                 if (mtag == NULL) {
 296                         m_freem(m);
 297                         return (-1);
 298                 }
 299                 enqueue_time = (uint64_t *)(mtag + 1);
 300                 *enqueue_time = read_machclk();
 301                 m_tag_prepend(m, mtag);
 302                 _addq(q, m);
 303                 return (0);
 304         }
 305         c->drop_overlimit++;
 306         m_freem(m);
 307
 308         return (-1);
 309 }
 310
 311 static int
 312 codel_should_drop(struct codel *c, class_queue_t *q, struct mbuf *m,
 313     u_int64_t now)
 314 {
 315         struct m_tag *mtag;
 316         uint64_t *enqueue_time;
 317
 318         if (m == NULL) {
 319                 c->vars.first_above_time = 0;
 320                 return (0);
 321         }
 322
 323         mtag = m_tag_locate(m, MTAG_CODEL, 0, NULL);
 324         if (mtag == NULL) {
 325                 /* Only one warning per second. */
 326                 if (ppsratecheck(&c->last_log, &c->last_pps, 1))
 327                         printf("%s: could not found the packet mtag!\n",
 328                             __func__);
 329                 c->vars.first_above_time = 0;
 330                 return (0);
 331         }
 332         enqueue_time = (uint64_t *)(mtag + 1);
 333         c->vars.ldelay = now - *enqueue_time;
 334         c->stats.maxpacket = MAX(c->stats.maxpacket, m_pktlen(m));
 335
 336         if (codel_time_before(c->vars.ldelay, c->params.target) ||
 337             qsize(q) <= c->stats.maxpacket) {
 338                 /* went below - stay below for at least interval */
 339                 c->vars.first_above_time = 0;
 340                 return (0);
 341         }
 342         if (c->vars.first_above_time == 0) {
 343                 /* just went above from below. If we stay above
 344                  * for at least interval we'll say it's ok to drop
 345                  */
 346                 c->vars.first_above_time = now + c->params.interval;
 347                 return (0);
 348         }
 349         if (codel_time_after(now, c->vars.first_above_time))
 350                 return (1);
 351
 352         return (0);
 353 }
 354
 355 /*
 356  * Run a Newton method step:
 357  * new_invsqrt = (invsqrt / 2) * (3 - count * invsqrt^2)
 358  *
 359  * Here, invsqrt is a fixed point number (< 1.0), 32bit mantissa, aka Q0.32
 360  */
 361 static void
 362 codel_Newton_step(struct codel_vars *vars)
 363 {
 364         uint32_t invsqrt, invsqrt2;
 365         uint64_t val;
 366
 367 /* sizeof_in_bits(rec_inv_sqrt) */
 368 #define REC_INV_SQRT_BITS (8 * sizeof(u_int16_t))
 369 /* needed shift to get a Q0.32 number from rec_inv_sqrt */
 370 #define REC_INV_SQRT_SHIFT (32 - REC_INV_SQRT_BITS)
 371
 372         invsqrt = ((u_int32_t)vars->rec_inv_sqrt) << REC_INV_SQRT_SHIFT;
 373         invsqrt2 = ((u_int64_t)invsqrt * invsqrt) >> 32;
 374         val = (3LL << 32) - ((u_int64_t)vars->count * invsqrt2);
 375         val >>= 2; /* avoid overflow in following multiply */
 376         val = (val * invsqrt) >> (32 - 2 + 1);
 377
 378         vars->rec_inv_sqrt = val >> REC_INV_SQRT_SHIFT;
 379 }
 380
 381 static u_int64_t
 382 codel_control_law(u_int64_t t, u_int64_t interval, u_int32_t rec_inv_sqrt)
 383 {
 384
 385         return (t + (u_int32_t)(((u_int64_t)interval *
 386             (rec_inv_sqrt << REC_INV_SQRT_SHIFT)) >> 32));
 387 }
 388
 389 struct mbuf *
 390 codel_getq(struct codel *c, class_queue_t *q)
 391 {
 392         struct mbuf     *m;
 393         u_int64_t        now;
 394         int              drop;
 395
 396         if ((m = _getq(q)) == NULL) {
 397                 c->vars.dropping = 0;
 398                 return (m);
 399         }
 400
 401         now = read_machclk();
 402         drop = codel_should_drop(c, q, m, now);
 403         if (c->vars.dropping) {
 404                 if (!drop) {
 405                         /* sojourn time below target - leave dropping state */
 406                         c->vars.dropping = 0;
 407                 } else if (codel_time_after_eq(now, c->vars.drop_next)) {
 408                         /* It's time for the next drop. Drop the current
 409                          * packet and dequeue the next. The dequeue might
 410                          * take us out of dropping state.
 411                          * If not, schedule the next drop.
 412                          * A large backlog might result in drop rates so high
 413                          * that the next drop should happen now,
 414                          * hence the while loop.
 415                          */
 416                         while (c->vars.dropping &&
 417                             codel_time_after_eq(now, c->vars.drop_next)) {
 418                                 c->vars.count++; /* don't care of possible wrap
 419                                                   * since there is no more
 420                                                   * divide */
 421                                 codel_Newton_step(&c->vars);
 422                                 /* TODO ECN */
 423                                 PKTCNTR_ADD(&c->stats.drop_cnt, m_pktlen(m));
 424                                 m_freem(m);
 425                                 m = _getq(q);
 426                                 if (!codel_should_drop(c, q, m, now))
 427                                         /* leave dropping state */
 428                                         c->vars.dropping = 0;
 429                                 else
 430                                         /* and schedule the next drop */
 431                                         c->vars.drop_next =
 432                                             codel_control_law(c->vars.drop_next,
 433                                                 c->params.interval,
 434                                                 c->vars.rec_inv_sqrt);
 435                         }
 436                 }
 437         } else if (drop) {
 438                 /* TODO ECN */
 439                 PKTCNTR_ADD(&c->stats.drop_cnt, m_pktlen(m));
 440                 m_freem(m);
 441
 442                 m = _getq(q);
 443                 drop = codel_should_drop(c, q, m, now);
 444
 445                 c->vars.dropping = 1;
 446                 /* if min went above target close to when we last went below it
 447                  * assume that the drop rate that controlled the queue on the
 448                  * last cycle is a good starting point to control it now.
 449                  */
 450                 if (codel_time_before(now - c->vars.drop_next,
 451                     16 * c->params.interval)) {
 452                         c->vars.count = (c->vars.count - c->vars.lastcount) | 1;
 453                         /* we dont care if rec_inv_sqrt approximation
 454                          * is not very precise :
 455                          * Next Newton steps will correct it quadratically.
 456                          */
 457                         codel_Newton_step(&c->vars);
 458                 } else {
 459                         c->vars.count = 1;
 460                         c->vars.rec_inv_sqrt = ~0U >> REC_INV_SQRT_SHIFT;
 461                 }
 462                 c->vars.lastcount = c->vars.count;
 463                 c->vars.drop_next = codel_control_law(now, c->params.interval,
 464                     c->vars.rec_inv_sqrt);
 465         }
 466
 467         return (m);
 468 }
 469
 470 void
 471 codel_getstats(struct codel *c, struct codel_stats *s)
 472 {
 473         *s = c->stats;
 474 }
 475
 476 #endif /* ALTQ_CODEL */