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
64static int		 codel_should_drop(struct codel *, class_queue_t *,
65			    struct mbuf *, u_int64_t);
66static void		 codel_Newton_step(struct codel_vars *);
67static 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
74static int codel_request(struct ifaltq *, int, void *);
75
76static int codel_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
77static struct mbuf *codel_dequeue(struct ifaltq *, int);
78
79int
80codel_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
91int
92codel_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
141int
142codel_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
157int
158codel_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
180static int
181codel_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
208static int
209codel_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
235static struct mbuf *
236codel_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
260struct codel *
261codel_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
276void
277codel_destroy(struct codel *c)
278{
279
280	free(c, M_DEVBUF);
281}
282
283#define	MTAG_CODEL	1438031249
284int
285codel_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
311static int
312codel_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 */
361static void
362codel_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
381static u_int64_t
382codel_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
389struct mbuf *
390codel_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
470void
471codel_getstats(struct codel *c, struct codel_stats *s)
472{
473	*s = c->stats;
474}
475
476#endif /* ALTQ_CODEL */
477