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 #include "opt_altq.h"
  41 #include "opt_inet.h"
  42 #include "opt_inet6.h"
  43
  44 #ifdef ALTQ_CODEL  /* CoDel is enabled by ALTQ_CODEL option in opt_altq.h */
  45
  46 #include <sys/param.h>
  47 #include <sys/malloc.h>
  48 #include <sys/mbuf.h>
  49 #include <sys/socket.h>
  50 #include <sys/systm.h>
  51
  52 #include <net/if.h>
  53 #include <net/if_var.h>
  54 #include <net/if_private.h>
  55 #include <netinet/in.h>
  56
  57 #include <netpfil/pf/pf.h>
  58 #include <netpfil/pf/pf_altq.h>
  59 #include <net/altq/if_altq.h>
  60 #include <net/altq/altq.h>
  61 #include <net/altq/altq_codel.h>
  62
  63 static int               codel_should_drop(struct codel *, class_queue_t *,
  64                             struct mbuf *, u_int64_t);
  65 static void              codel_Newton_step(struct codel_vars *);
  66 static u_int64_t         codel_control_law(u_int64_t t, u_int64_t, u_int32_t);
  67
  68 #define codel_time_after(a, b)          ((int64_t)(a) - (int64_t)(b) > 0)
  69 #define codel_time_after_eq(a, b)       ((int64_t)(a) - (int64_t)(b) >= 0)
  70 #define codel_time_before(a, b)         ((int64_t)(a) - (int64_t)(b) < 0)
  71 #define codel_time_before_eq(a, b)      ((int64_t)(a) - (int64_t)(b) <= 0)
  72
  73 static int codel_request(struct ifaltq *, int, void *);
  74
  75 static int codel_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
  76 static struct mbuf *codel_dequeue(struct ifaltq *, int);
  77
  78 int
  79 codel_pfattach(struct pf_altq *a)
  80 {
  81         struct ifnet *ifp;
  82
  83         if ((ifp = ifunit(a->ifname)) == NULL || a->altq_disc == NULL)
  84                 return (EINVAL);
  85
  86         return (altq_attach(&ifp->if_snd, ALTQT_CODEL, a->altq_disc,
  87             codel_enqueue, codel_dequeue, codel_request));
  88 }
  89
  90 int
  91 codel_add_altq(struct ifnet *ifp, struct pf_altq *a)
  92 {
  93         struct codel_if *cif;
  94         struct codel_opts       *opts;
  95
  96         if (ifp == NULL)
  97                 return (EINVAL);
  98         if (!ALTQ_IS_READY(&ifp->if_snd))
  99                 return (ENODEV);
 100
 101         opts = &a->pq_u.codel_opts;
 102
 103         cif = malloc(sizeof(struct codel_if), M_DEVBUF, M_NOWAIT | M_ZERO);
 104         if (cif == NULL)
 105                 return (ENOMEM);
 106         cif->cif_bandwidth = a->ifbandwidth;
 107         cif->cif_ifq = &ifp->if_snd;
 108
 109         cif->cl_q = malloc(sizeof(class_queue_t), M_DEVBUF, M_NOWAIT | M_ZERO);
 110         if (cif->cl_q == NULL) {
 111                 free(cif, M_DEVBUF);
 112                 return (ENOMEM);
 113         }
 114
 115         if (a->qlimit == 0)
 116                 a->qlimit = 50; /* use default. */
 117         qlimit(cif->cl_q) = a->qlimit;
 118         qtype(cif->cl_q) = Q_CODEL;
 119         qlen(cif->cl_q) = 0;
 120         qsize(cif->cl_q) = 0;
 121
 122         if (opts->target == 0)
 123                 opts->target = 5;
 124         if (opts->interval == 0)
 125                 opts->interval = 100;
 126         cif->codel.params.target = machclk_freq * opts->target / 1000;
 127         cif->codel.params.interval = machclk_freq * opts->interval / 1000;
 128         cif->codel.params.ecn = opts->ecn;
 129         cif->codel.stats.maxpacket = 256;
 130
 131         cif->cl_stats.qlength = qlen(cif->cl_q);
 132         cif->cl_stats.qlimit = qlimit(cif->cl_q);
 133
 134         /* keep the state in pf_altq */
 135         a->altq_disc = cif;
 136
 137         return (0);
 138 }
 139
 140 int
 141 codel_remove_altq(struct pf_altq *a)
 142 {
 143         struct codel_if *cif;
 144
 145         if ((cif = a->altq_disc) == NULL)
 146                 return (EINVAL);
 147         a->altq_disc = NULL;
 148
 149         if (cif->cl_q)
 150                 free(cif->cl_q, M_DEVBUF);
 151         free(cif, M_DEVBUF);
 152
 153         return (0);
 154 }
 155
 156 int
 157 codel_getqstats(struct pf_altq *a, void *ubuf, int *nbytes, int version)
 158 {
 159         struct codel_if *cif;
 160         struct codel_ifstats stats;
 161         int error = 0;
 162
 163         if ((cif = altq_lookup(a->ifname, ALTQT_CODEL)) == NULL)
 164                 return (EBADF);
 165
 166         if (*nbytes < sizeof(stats))
 167                 return (EINVAL);
 168
 169         stats = cif->cl_stats;
 170         stats.stats = cif->codel.stats;
 171
 172         if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
 173                 return (error);
 174         *nbytes = sizeof(stats);
 175
 176         return (0);
 177 }
 178
 179 static int
 180 codel_request(struct ifaltq *ifq, int req, void *arg)
 181 {
 182         struct codel_if *cif = (struct codel_if *)ifq->altq_disc;
 183         struct mbuf *m;
 184
 185         IFQ_LOCK_ASSERT(ifq);
 186
 187         switch (req) {
 188         case ALTRQ_PURGE:
 189                 if (!ALTQ_IS_ENABLED(cif->cif_ifq))
 190                         break;
 191
 192                 if (qempty(cif->cl_q))
 193                         break;
 194
 195                 while ((m = _getq(cif->cl_q)) != NULL) {
 196                         PKTCNTR_ADD(&cif->cl_stats.cl_dropcnt, m_pktlen(m));
 197                         m_freem(m);
 198                         IFQ_DEC_LEN(cif->cif_ifq);
 199                 }
 200                 cif->cif_ifq->ifq_len = 0;
 201                 break;
 202         }
 203
 204         return (0);
 205 }
 206
 207 static int
 208 codel_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
 209 {
 210
 211         struct codel_if *cif = (struct codel_if *) ifq->altq_disc;
 212
 213         IFQ_LOCK_ASSERT(ifq);
 214
 215         /* grab class set by classifier */
 216         if ((m->m_flags & M_PKTHDR) == 0) {
 217                 /* should not happen */
 218                 printf("altq: packet for %s does not have pkthdr\n",
 219                    ifq->altq_ifp->if_xname);
 220                 m_freem(m);
 221                 PKTCNTR_ADD(&cif->cl_stats.cl_dropcnt, m_pktlen(m));
 222                 return (ENOBUFS);
 223         }
 224
 225         if (codel_addq(&cif->codel, cif->cl_q, m)) {
 226                 PKTCNTR_ADD(&cif->cl_stats.cl_dropcnt, m_pktlen(m));
 227                 return (ENOBUFS);
 228         }
 229         IFQ_INC_LEN(ifq);
 230
 231         return (0);
 232 }
 233
 234 static struct mbuf *
 235 codel_dequeue(struct ifaltq *ifq, int op)
 236 {
 237         struct codel_if *cif = (struct codel_if *)ifq->altq_disc;
 238         struct mbuf *m;
 239
 240         IFQ_LOCK_ASSERT(ifq);
 241
 242         if (IFQ_IS_EMPTY(ifq))
 243                 return (NULL);
 244
 245         if (op == ALTDQ_POLL)
 246                 return (qhead(cif->cl_q));
 247
 248         m = codel_getq(&cif->codel, cif->cl_q);
 249         if (m != NULL) {
 250                 IFQ_DEC_LEN(ifq);
 251                 PKTCNTR_ADD(&cif->cl_stats.cl_xmitcnt, m_pktlen(m));
 252                 return (m);
 253         }
 254
 255         return (NULL);
 256 }
 257
 258 struct codel *
 259 codel_alloc(int target, int interval, int ecn)
 260 {
 261         struct codel *c;
 262
 263         c = malloc(sizeof(*c), M_DEVBUF, M_NOWAIT | M_ZERO);
 264         if (c != NULL) {
 265                 c->params.target = machclk_freq * target / 1000;
 266                 c->params.interval = machclk_freq * interval / 1000;
 267                 c->params.ecn = ecn;
 268                 c->stats.maxpacket = 256;
 269         }
 270
 271         return (c);
 272 }
 273
 274 void
 275 codel_destroy(struct codel *c)
 276 {
 277
 278         free(c, M_DEVBUF);
 279 }
 280
 281 #define MTAG_CODEL      1438031249
 282 int
 283 codel_addq(struct codel *c, class_queue_t *q, struct mbuf *m)
 284 {
 285         struct m_tag *mtag;
 286         uint64_t *enqueue_time;
 287
 288         if (qlen(q) < qlimit(q)) {
 289                 mtag = m_tag_locate(m, MTAG_CODEL, 0, NULL);
 290                 if (mtag == NULL) {
 291                         mtag = m_tag_alloc(MTAG_CODEL, 0, sizeof(uint64_t),
 292                             M_NOWAIT);
 293                         if (mtag != NULL)
 294                                 m_tag_prepend(m, mtag);
 295                 }
 296                 if (mtag == NULL) {
 297                         m_freem(m);
 298                         return (-1);
 299                 }
 300                 enqueue_time = (uint64_t *)(mtag + 1);
 301                 *enqueue_time = read_machclk();
 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 */