1/*
2 * Codel - The Controlled-Delay Active Queue Management algorithm.
3 *
4 * $FreeBSD: stable/11/sys/netpfil/ipfw/dn_aqm_codel.c 317488 2017-04-27 07:30:48Z truckman $
5 *
6 * Copyright (C) 2016 Centre for Advanced Internet Architectures,
7 *  Swinburne University of Technology, Melbourne, Australia.
8 * Portions of this code were made possible in part by a gift from
9 *  The Comcast Innovation Fund.
10 * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
11 *
12 * Redistribution and use in source and binary forms, with or without
13 * modification, are permitted provided that the following conditions
14 * are met:
15 * 1. Redistributions of source code must retain the above copyright
16 *    notice, this list of conditions and the following disclaimer.
17 * 2. Redistributions in binary form must reproduce the above copyright
18 *    notice, this list of conditions and the following disclaimer in the
19 *    documentation and/or other materials provided with the distribution.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
22 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
25 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
27 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
30 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 * SUCH DAMAGE.
32 */
33
34#include <sys/cdefs.h>
35#include "opt_inet6.h"
36
37#include <sys/param.h>
38#include <sys/systm.h>
39#include <sys/malloc.h>
40#include <sys/mbuf.h>
41#include <sys/kernel.h>
42#include <sys/lock.h>
43#include <sys/module.h>
44#include <sys/priv.h>
45#include <sys/proc.h>
46#include <sys/rwlock.h>
47#include <sys/socket.h>
48#include <sys/time.h>
49#include <sys/sysctl.h>
50
51#include <net/if.h>	/* IFNAMSIZ, struct ifaddr, ifq head, lock.h mutex.h */
52#include <net/netisr.h>
53#include <net/vnet.h>
54
55#include <netinet/in.h>
56#include <netinet/ip.h>		/* ip_len, ip_off */
57#include <netinet/ip_var.h>	/* ip_output(), IP_FORWARDING */
58#include <netinet/ip_fw.h>
59#include <netinet/ip_dummynet.h>
60#include <netinet/if_ether.h> /* various ether_* routines */
61#include <netinet/ip6.h>       /* for ip6_input, ip6_output prototypes */
62#include <netinet6/ip6_var.h>
63#include <netpfil/ipfw/dn_heap.h>
64
65#ifdef NEW_AQM
66#include <netpfil/ipfw/ip_fw_private.h>
67#include <netpfil/ipfw/ip_dn_private.h>
68#include <netpfil/ipfw/dn_aqm.h>
69#include <netpfil/ipfw/dn_aqm_codel.h>
70#include <netpfil/ipfw/dn_sched.h>
71
72#define DN_AQM_CODEL 1
73
74static struct dn_aqm codel_desc;
75
76/* default codel parameters */
77struct dn_aqm_codel_parms codel_sysctl = {5000 * AQM_TIME_1US,
78	100000 * AQM_TIME_1US, 0};
79
80static int
81codel_sysctl_interval_handler(SYSCTL_HANDLER_ARGS)
82{
83	int error;
84	long  value;
85
86	value = codel_sysctl.interval;
87	value /= AQM_TIME_1US;
88	error = sysctl_handle_long(oidp, &value, 0, req);
89	if (error != 0 || req->newptr == NULL)
90		return (error);
91	if (value < 1 || value > 100 * AQM_TIME_1S)
92		return (EINVAL);
93	codel_sysctl.interval = value * AQM_TIME_1US ;
94	return (0);
95}
96
97static int
98codel_sysctl_target_handler(SYSCTL_HANDLER_ARGS)
99{
100	int error;
101	long  value;
102
103	value = codel_sysctl.target;
104	value /= AQM_TIME_1US;
105	error = sysctl_handle_long(oidp, &value, 0, req);
106	if (error != 0 || req->newptr == NULL)
107		return (error);
108	D("%ld", value);
109	if (value < 1 || value > 5 * AQM_TIME_1S)
110		return (EINVAL);
111	codel_sysctl.target = value * AQM_TIME_1US ;
112	return (0);
113}
114
115/* defining Codel sysctl variables */
116SYSBEGIN(f4)
117
118SYSCTL_DECL(_net_inet);
119SYSCTL_DECL(_net_inet_ip);
120SYSCTL_DECL(_net_inet_ip_dummynet);
121static SYSCTL_NODE(_net_inet_ip_dummynet, OID_AUTO,
122	codel, CTLFLAG_RW, 0, "CODEL");
123
124#ifdef SYSCTL_NODE
125SYSCTL_PROC(_net_inet_ip_dummynet_codel, OID_AUTO, target,
126	CTLTYPE_LONG | CTLFLAG_RW, NULL, 0,codel_sysctl_target_handler, "L",
127	"CoDel target in microsecond");
128
129SYSCTL_PROC(_net_inet_ip_dummynet_codel, OID_AUTO, interval,
130	CTLTYPE_LONG | CTLFLAG_RW, NULL, 0, codel_sysctl_interval_handler, "L",
131	"CoDel interval in microsecond");
132#endif
133
134/* This function computes codel_interval/sqrt(count)
135 *  Newton's method of approximation is used to compute 1/sqrt(count).
136 * http://betterexplained.com/articles/
137 * 	understanding-quakes-fast-inverse-square-root/
138 */
139aqm_time_t
140control_law(struct codel_status *cst, struct dn_aqm_codel_parms *cprms,
141	aqm_time_t t)
142{
143	uint32_t count;
144	uint64_t temp;
145	count = cst->count;
146
147	/* we don't calculate isqrt(1) to get more accurate result*/
148	if (count == 1) {
149		/* prepare isqrt (old guess) for the next iteration i.e. 1/sqrt(2)*/
150		cst->isqrt = (1UL<< FIX_POINT_BITS) * 7/10;
151		/* return time + isqrt(1)*interval */
152		return t + cprms->interval;
153	}
154
155	/* newguess = g(1.5 - 0.5*c*g^2)
156	 * Multiplying both sides by 2 to make all the constants intergers
157	 * newguess * 2  = g(3 - c*g^2) g=old guess, c=count
158	 * So, newguess = newguess /2
159	 * Fixed point operations are used here.
160	 */
161
162	/* Calculate g^2 */
163	temp = (uint32_t) cst->isqrt * cst->isqrt;
164	/* Calculate (3 - c*g^2) i.e. (3 - c * temp) */
165	temp = (3ULL<< (FIX_POINT_BITS*2)) - (count * temp);
166
167	/*
168	 * Divide by 2 because we multiplied the original equation by two
169	 * Also, we shift the result by 8 bits to prevent overflow.
170	 * */
171	temp >>= (1 + 8);
172
173	/*  Now, temp = (1.5 - 0.5*c*g^2)
174	 * Calculate g (1.5 - 0.5*c*g^2) i.e. g * temp
175	 */
176	temp = (cst->isqrt * temp) >> (FIX_POINT_BITS + FIX_POINT_BITS - 8);
177	cst->isqrt = temp;
178
179	 /* calculate codel_interval/sqrt(count) */
180	 return t + ((cprms->interval * temp) >> FIX_POINT_BITS);
181}
182
183/*
184 * Extract a packet from the head of queue 'q'
185 * Return a packet or NULL if the queue is empty.
186 * Also extract packet's timestamp from mtag.
187 */
188struct mbuf *
189codel_extract_head(struct dn_queue *q, aqm_time_t *pkt_ts)
190{
191	struct m_tag *mtag;
192	struct mbuf *m = q->mq.head;
193
194	if (m == NULL)
195		return m;
196	q->mq.head = m->m_nextpkt;
197
198	/* Update stats */
199	update_stats(q, -m->m_pkthdr.len, 0);
200
201	if (q->ni.length == 0) /* queue is now idle */
202			q->q_time = dn_cfg.curr_time;
203
204	/* extract packet TS*/
205	mtag = m_tag_locate(m, MTAG_ABI_COMPAT, DN_AQM_MTAG_TS, NULL);
206	if (mtag == NULL) {
207		D("Codel timestamp mtag not found!");
208		*pkt_ts = 0;
209	} else {
210		*pkt_ts = *(aqm_time_t *)(mtag + 1);
211		m_tag_delete(m,mtag);
212	}
213
214	return m;
215}
216
217/*
218 * Enqueue a packet 'm' in queue 'q'
219 */
220static int
221aqm_codel_enqueue(struct dn_queue *q, struct mbuf *m)
222{
223	struct dn_fs *f;
224	uint64_t len;
225	struct codel_status *cst;	/*codel status variables */
226	struct m_tag *mtag;
227
228	f = &(q->fs->fs);
229	len = m->m_pkthdr.len;
230	cst = q->aqm_status;
231	if(!cst) {
232		D("Codel queue is not initialized\n");
233		goto drop;
234	}
235
236	/* Finding maximum packet size */
237	// XXX we can get MTU from driver instead
238	if (len > cst->maxpkt_size)
239		cst->maxpkt_size = len;
240
241	/* check for queue size and drop the tail if exceed queue limit*/
242	if (f->flags & DN_QSIZE_BYTES) {
243		if ( q->ni.len_bytes > f->qsize)
244			goto drop;
245	}
246	else {
247		if ( q->ni.length >= f->qsize)
248			goto drop;
249	}
250
251	/* Add timestamp as mtag */
252	mtag = m_tag_locate(m, MTAG_ABI_COMPAT, DN_AQM_MTAG_TS, NULL);
253	if (mtag == NULL)
254		mtag = m_tag_alloc(MTAG_ABI_COMPAT, DN_AQM_MTAG_TS,
255			sizeof(aqm_time_t), M_NOWAIT);
256	if (mtag == NULL) {
257		m_freem(m);
258		goto drop;
259	}
260
261	*(aqm_time_t *)(mtag + 1) = AQM_UNOW;
262	m_tag_prepend(m, mtag);
263
264	mq_append(&q->mq, m);
265	update_stats(q, len, 0);
266	return (0);
267
268drop:
269	update_stats(q, 0, 1);
270	FREE_PKT(m);
271	return (1);
272}
273
274
275/* Dequeue a pcaket from queue q */
276static struct mbuf *
277aqm_codel_dequeue(struct dn_queue *q)
278{
279	return codel_dequeue(q);
280}
281
282/*
283 * initialize Codel for queue 'q'
284 * First allocate memory for codel status.
285 */
286static int
287aqm_codel_init(struct dn_queue *q)
288{
289	struct codel_status *cst;
290
291	if (!q->fs->aqmcfg) {
292		D("Codel is not configure!d");
293		return EINVAL;
294	}
295
296	q->aqm_status = malloc(sizeof(struct codel_status),
297			 M_DUMMYNET, M_NOWAIT | M_ZERO);
298	if (q->aqm_status == NULL) {
299		D("Cannot allocate AQM_codel private data");
300		return ENOMEM ;
301	}
302
303	/* init codel status variables */
304	cst = q->aqm_status;
305	cst->dropping=0;
306	cst->first_above_time=0;
307	cst->drop_next_time=0;
308	cst->count=0;
309	cst->maxpkt_size = 500;
310
311	/* increase reference counters */
312	codel_desc.ref_count++;
313
314	return 0;
315}
316
317/*
318 * Clean up Codel status for queue 'q'
319 * Destroy memory allocated for codel status.
320 */
321static int
322aqm_codel_cleanup(struct dn_queue *q)
323{
324
325	if (q && q->aqm_status) {
326		free(q->aqm_status, M_DUMMYNET);
327		q->aqm_status = NULL;
328		/* decrease reference counters */
329		codel_desc.ref_count--;
330	}
331	else
332		D("Codel already cleaned up");
333	return 0;
334}
335
336/*
337 * Config codel parameters
338 * also allocate memory for codel configurations
339 */
340static int
341aqm_codel_config(struct dn_fsk* fs, struct dn_extra_parms *ep, int len)
342{
343	struct dn_aqm_codel_parms *ccfg;
344
345	int l = sizeof(struct dn_extra_parms);
346	if (len < l) {
347		D("invalid sched parms length got %d need %d", len, l);
348		return EINVAL;
349	}
350	/* we free the old cfg because maybe the original allocation
351	 * not the same size as the new one (different AQM type).
352	 */
353	if (fs->aqmcfg) {
354		free(fs->aqmcfg, M_DUMMYNET);
355		fs->aqmcfg = NULL;
356	}
357
358	fs->aqmcfg = malloc(sizeof(struct dn_aqm_codel_parms),
359			 M_DUMMYNET, M_NOWAIT | M_ZERO);
360	if (fs->aqmcfg== NULL) {
361		D("cannot allocate AQM_codel configuration parameters");
362		return ENOMEM;
363	}
364
365	/* configure codel parameters */
366	ccfg = fs->aqmcfg;
367
368	if (ep->par[0] < 0)
369		ccfg->target = codel_sysctl.target;
370	else
371		ccfg->target = ep->par[0] * AQM_TIME_1US;
372
373	if (ep->par[1] < 0)
374		ccfg->interval = codel_sysctl.interval;
375	else
376		ccfg->interval = ep->par[1] * AQM_TIME_1US;
377
378	if (ep->par[2] < 0)
379		ccfg->flags = 0;
380	else
381		ccfg->flags = ep->par[2];
382
383	/* bound codel configurations */
384	ccfg->target = BOUND_VAR(ccfg->target,1, 5 * AQM_TIME_1S);
385	ccfg->interval = BOUND_VAR(ccfg->interval,1, 5 * AQM_TIME_1S);
386	/* increase config reference counter */
387	codel_desc.cfg_ref_count++;
388
389	return 0;
390}
391
392/*
393 * Deconfigure Codel and free memory allocation
394 */
395static int
396aqm_codel_deconfig(struct dn_fsk* fs)
397{
398
399	if (fs && fs->aqmcfg) {
400		free(fs->aqmcfg, M_DUMMYNET);
401		fs->aqmcfg = NULL;
402		fs->aqmfp = NULL;
403		/* decrease config reference counter */
404		codel_desc.cfg_ref_count--;
405	}
406
407	return 0;
408}
409
410/*
411 * Retrieve Codel configuration parameters.
412 */
413static int
414aqm_codel_getconfig(struct dn_fsk *fs, struct dn_extra_parms * ep)
415{
416	struct dn_aqm_codel_parms *ccfg;
417
418	if (fs->aqmcfg) {
419		strlcpy(ep->name, codel_desc.name, sizeof(ep->name));
420		ccfg = fs->aqmcfg;
421		ep->par[0] = ccfg->target / AQM_TIME_1US;
422		ep->par[1] = ccfg->interval / AQM_TIME_1US;
423		ep->par[2] = ccfg->flags;
424		return 0;
425	}
426	return 1;
427}
428
429static struct dn_aqm codel_desc = {
430	_SI( .type = )  DN_AQM_CODEL,
431	_SI( .name = )  "CODEL",
432	_SI( .enqueue = )  aqm_codel_enqueue,
433	_SI( .dequeue = )  aqm_codel_dequeue,
434	_SI( .config = )  aqm_codel_config,
435	_SI( .getconfig = )  aqm_codel_getconfig,
436	_SI( .deconfig = )  aqm_codel_deconfig,
437	_SI( .init = )  aqm_codel_init,
438	_SI( .cleanup = )  aqm_codel_cleanup,
439};
440
441DECLARE_DNAQM_MODULE(dn_aqm_codel, &codel_desc);
442
443
444#endif
445