--- a/sys/netinet/ip_dummynet.h	2016-02-12 10:35:59.086083000 +1100
+++ b/sys/netinet/ip_dummynet.h	2016-02-17 13:32:56.176727000 +1100
@@ -29,7 +29,7 @@
 
 #ifndef _IP_DUMMYNET_H
 #define _IP_DUMMYNET_H
-
+#define NEW_AQM
 /*
  * Definition of the kernel-userland API for dummynet.
  *
@@ -85,7 +85,13 @@
 	/* special commands for emulation of sysctl variables */
 	DN_SYSCTL_GET,
 	DN_SYSCTL_SET,
-
+#ifdef NEW_AQM
+	/* subtypes used for setting/getting extra parameters.
+	 * these subtypes used with IP_DUMMYNET3 command (get)
+	 * and DN_TEXT (set). */
+	DN_AQM_PARAMS, /* AQM extra params */
+	DN_SCH_PARAMS, /* scheduler extra params */
+#endif
 	DN_LAST,
 };
 
@@ -105,6 +111,9 @@
 	DN_IS_RED	= 0x0020,
 	DN_IS_GENTLE_RED= 0x0040,
 	DN_IS_ECN	= 0x0080,
+#ifdef NEW_AQM
+	DN_IS_AQM = 0x0100,	/* AQMs: e.g Codel & PIE */
+#endif
 	DN_PIPE_CMD	= 0x1000,	/* pipe config... */
 };
 
@@ -210,7 +219,19 @@
 	int	samples[ED_MAX_SAMPLES_NO];	/* may be shorter */
 };
 
-
+#ifdef NEW_AQM
+/* Extra parameters for AQM and scheduler.
+ * This struct is used to pass and retrieve parameters (configurations)
+ * to/from AQM and Scheduler.
+ */
+struct dn_extra_parms {
+	struct dn_id oid;
+	char name[16];
+	uint32_t nr;
+#define DN_MAX_EXTRA_PARM	10
+	int64_t par[DN_MAX_EXTRA_PARM];
+};
+#endif
 
 /*
  * Overall structure of dummynet
--- a/sys/modules/dummynet/Makefile	2016-02-12 10:35:42.598478000 +1100
+++ b/sys/modules/dummynet/Makefile	2016-02-17 13:57:22.047681000 +1100
@@ -4,8 +4,9 @@
 KMOD=   dummynet
 SRCS=   ip_dummynet.c
 SRCS+= ip_dn_glue.c ip_dn_io.c
+SRCS+= dn_aqm_codel.c
 SRCS+= dn_heap.c dn_sched_fifo.c dn_sched_qfq.c dn_sched_rr.c dn_sched_wf2q.c 
-SRCS+= dn_sched_prio.c
+SRCS+= dn_sched_prio.c dn_sched_fq_codel.c
 SRCS+=	opt_inet6.h
 
 .include <bsd.kmod.mk>
diff -NaubBE a/sys/netpfil/ipfw/dn_aqm.h b/sys/netpfil/ipfw/dn_aqm.h
--- a/sys/netpfil/ipfw/dn_aqm.h	1970-01-01 10:00:00.000000000 +1000
+++ b/sys/netpfil/ipfw/dn_aqm.h	2016-02-17 22:12:11.725303000 +1100
@@ -0,0 +1,157 @@
+/*-
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * API for writting an Active Queue Management algorithm for Dummynet
+ */
+
+#ifndef _IP_DN_AQM_H
+#define _IP_DN_AQM_H
+
+
+/* NOW is the current time in millisecond*/
+#define NOW ((dn_cfg.curr_time*tick)/1000)
+//#define UNOW (dn_cfg.curr_time*tick)
+
+/* Macro for variable bounding */
+#define BOUND_VAR(x,l,h)  (x<l?l:x>h?h:x)
+
+/* sysctl variable to count number of droped packets */
+extern unsigned long io_pkt_drop; 
+
+/*
+ * Structure for holding data and function pointers that together represent a
+ * AQM algorithm.
+ */
+ struct dn_aqm {
+#define DN_AQM_NAME_MAX 50
+	char			name[DN_AQM_NAME_MAX];	/* name of AQM algorithm */
+	uint32_t	type;	/* AQM type number */
+	
+	/* Methods implemented by AQM algorithm:
+	 * 
+	 * enqueue	enqueue packet 'm' on queue 'q'.
+	 * 	Return 0 on success, 1 on drop.
+	 * 
+	 * dequeue	dequeue a packet from queue 'q'.
+	 * 	Return a packet, NULL if no packet available.
+	 * 
+	 * config	configure AQM algorithm
+	 * If required, this function should allocate space to store 
+	 * the configurations and set 'fs->aqmcfg' to point to this space.
+	 * 'dn_extra_parms' includes array of parameters send
+	 * from ipfw userland command.
+	 * 	Return 0 on success, non-zero otherwise.
+	 * 
+	 * deconfig	deconfigure AQM algorithm.
+	 * The allocated configuration memory space should be freed here.
+	 * 	Return 0 on success, non-zero otherwise.
+	 * 
+	 * init	initialise AQM status variables of queue 'q'
+	 * This function is used to allocate space and init AQM status for a
+	 * queue and q->aqm_status to point to this space.
+	 * 	Return 0 on success, non-zero otherwise.
+	 * 
+	 * cleanup	cleanup AQM status variables of queue 'q'
+	 * The allocated memory space for AQM status should be freed here.
+	 * 	Return 0 on success, non-zero otherwise.
+	 * 
+	 * getconfig	retrieve AQM configurations 
+	 * This function is used to return AQM parameters to userland
+	 * command. The function should fill 'dn_extra_parms' struct with 
+	 * the AQM configurations using 'par' array.
+	 * 
+	 */
+	
+	int (*enqueue)(struct dn_queue *, struct mbuf *);
+	struct mbuf * (*dequeue)(struct dn_queue *);
+	int (*config)(struct dn_fsk *, struct dn_extra_parms *ep, int);
+	int (*deconfig)(struct dn_fsk *);
+	int (*init)(struct dn_queue *);
+	int (*cleanup)(struct dn_queue *);
+	int (*getconfig)(struct dn_fsk *, struct dn_extra_parms *);
+
+	int	ref_count; /*Number of queues instances in the system */
+	int	cfg_ref_count;	/*Number of AQM instances in the system */
+	SLIST_ENTRY (dn_aqm) next; /* Next AQM in the list */
+};
+
+/* Helper function to update queue and scheduler statistics.
+ * negative len + drop -> drop
+ * negative len -> dequeue
+ * positive len -> enqueue
+ * positive len + drop -> drop during enqueue
+ */
+__inline static void
+update_stats(struct dn_queue *q, int len, int drop)
+{
+	int inc = 0;
+	struct dn_flow *sni;
+	struct dn_flow *qni;
+	
+	sni = &q->_si->ni;
+	qni = &q->ni;
+
+	if (len < 0)
+			inc = -1;
+	else if(len > 0)
+			inc = 1;
+
+	if(!drop){
+		/*update queue stats */
+		qni->length += inc;
+		qni->len_bytes += len;
+
+		/*update scheduler instance stats */
+		sni->length += inc;
+		sni->len_bytes += len;
+	}
+
+	if (drop) {
+			qni->drops ++;
+			sni->drops ++;
+			io_pkt_drop ++;
+	}
+
+}
+
+
+/* kernel module related function */
+int
+dn_aqm_modevent(module_t mod, int cmd, void *arg);
+
+#define DECLARE_DNAQM_MODULE(name, dnaqm)			\
+	static moduledata_t name##_mod = {			\
+		#name, dn_aqm_modevent, dnaqm		\
+	};							\
+	DECLARE_MODULE(name, name##_mod, 			\
+		SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY); 	\
+        MODULE_DEPEND(name, dummynet, 3, 3, 3)
+
+#endif
diff -NaubBE a/sys/netpfil/ipfw/dn_aqm_codel.c b/sys/netpfil/ipfw/dn_aqm_codel.c
--- a/sys/netpfil/ipfw/dn_aqm_codel.c	1970-01-01 10:00:00.000000000 +1000
+++ b/sys/netpfil/ipfw/dn_aqm_codel.c	2016-02-17 22:12:09.004365000 +1100
@@ -0,0 +1,400 @@
+/*
+ * Codel - The Controlled-Delay Active Queue Management algorithm.
+ * 
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+
+#include <sys/cdefs.h>
+#include "opt_inet6.h"
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/malloc.h>
+#include <sys/mbuf.h>
+#include <sys/kernel.h>
+#include <sys/lock.h>
+#include <sys/module.h>
+#include <sys/priv.h>
+#include <sys/proc.h>
+#include <sys/rwlock.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <sys/sysctl.h>
+
+#include <net/if.h>	/* IFNAMSIZ, struct ifaddr, ifq head, lock.h mutex.h */
+#include <net/netisr.h>
+#include <net/vnet.h>
+
+#include <netinet/in.h>
+#include <netinet/ip.h>		/* ip_len, ip_off */
+#include <netinet/ip_var.h>	/* ip_output(), IP_FORWARDING */
+#include <netinet/ip_fw.h>
+#include <netinet/ip_dummynet.h>
+#include <netinet/if_ether.h> /* various ether_* routines */
+#include <netinet/ip6.h>       /* for ip6_input, ip6_output prototypes */
+#include <netinet6/ip6_var.h>
+#include <netpfil/ipfw/dn_heap.h>
+
+#ifdef NEW_AQM
+#include <netpfil/ipfw/ip_fw_private.h>
+#include <netpfil/ipfw/ip_dn_private.h>
+#include <netpfil/ipfw/dn_aqm.h>
+#include <netpfil/ipfw/dn_aqm_codel.h>
+#include <netpfil/ipfw/dn_sched.h>
+
+#define DN_AQM_CODEL 1
+
+static struct dn_aqm codel_desc;
+
+/* default codel parameters */
+struct dn_aqm_codel_parms codel_sysctl = {5, 100, 0};
+
+/* defining Codel sysctl variables */
+SYSBEGIN(f4)
+
+SYSCTL_DECL(_net_inet);
+SYSCTL_DECL(_net_inet_ip);
+SYSCTL_DECL(_net_inet_ip_dummynet);
+static SYSCTL_NODE(_net_inet_ip_dummynet, OID_AUTO, 
+	codel, CTLFLAG_RW, 0, "CODEL");
+
+#ifdef SYSCTL_NODE
+SYSCTL_INT(_net_inet_ip_dummynet_codel, OID_AUTO, target,
+	CTLFLAG_RW, &codel_sysctl.target, 0, "Target ms for CoDel");
+SYSCTL_INT(_net_inet_ip_dummynet_codel, OID_AUTO, interval,
+	CTLFLAG_RW, &codel_sysctl.interval, 0, "Interval ms for CoDel");
+#endif
+
+/* This function computes codel_interval/sqrt(count) 
+ *  Newton's method of approximation is used to compute 1/sqrt(count).
+ * http://betterexplained.com/articles/
+ * 	understanding-quakes-fast-inverse-square-root/ 
+ */
+uint64_t 
+control_law(struct codel_status *cst, struct dn_aqm_codel_parms *cprms,
+	uint64_t t)
+{
+	uint16_t count;
+	uint32_t temp;
+	count = cst->count;
+
+	/* we don't calculate isqrt(1) to get more accurate result*/
+	if (count == 1) {
+		/* prepare isqrt (old guess) for the next iteration i.e. 1/sqrt(2)*/
+		cst->isqrt = (1UL<< FIX_POINT_BITS) * 7/10;
+		/* return time + isqrt(1)*interval */
+		return t + cprms->interval;
+	}
+
+	/* newguess =g(1.5 - 0.5*c*g^2)
+	 * Multiplying both sides by 2 to make all the constants intergers
+	 * newguess * 2  = g(3 - c*g^2) g=old guess, c=count
+	 * So, newguess = newguess /2
+	 * Fixed point operations are used here.  
+	 */
+	/* calculate g^2 */
+	temp=  ((uint32_t)cst->isqrt * cst->isqrt) 
+		>> FIX_POINT_BITS;
+	/* calculate (3 - c*g^2) i.e. (3 - c * temp) */
+	 temp = (3UL<< FIX_POINT_BITS) - (count * temp);
+	/* divide by 2 because we multiplied the original equation by two */
+	temp>>=1; 
+	/* calculate g(3 - c*g^2) i.e. g*temp */
+	 temp = (cst->isqrt * temp) >> FIX_POINT_BITS;
+	 cst->isqrt = temp;
+
+	 /* calculate codel_interval/sqrt(count) */
+	 return t + (uint16_t)((cprms->interval * temp) >> FIX_POINT_BITS);
+}
+
+/*
+ * Extract a packet from the head of queue 'q'
+ * Return a packet or NULL if the queue is empty.
+ * Also extract packet's timestamp from mtag.
+ */
+struct mbuf *
+codel_extract_head(struct dn_queue *q, uint64_t *pkt_ts)
+{
+	struct m_tag *mtag;
+	struct mbuf *m = q->mq.head;
+
+	if (m == NULL)
+		return m;
+	q->mq.head = m->m_nextpkt;
+
+	/* Update stats */
+	update_stats(q, -m->m_pkthdr.len, 0);
+
+	if (q->ni.length == 0) /* queue is now idle */
+			q->q_time = dn_cfg.curr_time;
+
+	/* extract packet TS*/
+	mtag = m_tag_locate(m, MTAG_ABI_COMPAT, DN_AQM_MTAG_CODEL, NULL);
+	if (mtag == NULL) {
+		D("Codel timestamp mtag not found!");
+		*pkt_ts = 0;
+	} else {
+		*pkt_ts = *(uint64_t *)(mtag + 1);
+		m_tag_delete(m,mtag); 
+	}
+
+	return m;
+}
+
+/*
+ * Enqueue a packet 'm' in queue 'q'
+ */
+static int
+aqm_codel_enqueue(struct dn_queue *q, struct mbuf *m)
+{
+	struct dn_fs *f;
+
+	uint64_t len;
+	struct codel_status *cst;	/*codel status variables */
+	struct m_tag *mtag;
+
+	f = &(q->fs->fs);
+	len = m->m_pkthdr.len;
+	cst = q->aqm_status;
+	if(!cst) {
+		D("Codel queue is not initialized\n");
+		goto drop;
+	}
+	
+	/* Finding maximum packet size */
+	// XXX we can get MTU from driver instead 
+	if (len > cst->maxpkt_size)
+		cst->maxpkt_size = len;
+
+	/* check for queue size and drop the tail if exceed queue limit*/
+	if (f->flags & DN_QSIZE_BYTES) {
+		if ( q->ni.len_bytes > f->qsize)
+			goto drop;
+	}
+	else {
+		if ( q->ni.length >= f->qsize)
+			goto drop;
+	}
+
+	/* Add timestamp as mtag */
+	mtag = m_tag_locate(m, MTAG_ABI_COMPAT, DN_AQM_MTAG_CODEL, NULL);
+	if (mtag == NULL)
+		mtag = m_tag_alloc(MTAG_ABI_COMPAT, DN_AQM_MTAG_CODEL,
+			sizeof(uint64_t), M_NOWAIT);
+	if (mtag == NULL) {
+		m_freem(m); 
+		goto drop;
+	}
+
+	*(uint64_t *)(mtag + 1) = NOW;
+	m_tag_prepend(m, mtag);
+
+	mq_append(&q->mq, m);
+	update_stats(q, len, 0);
+	return (0);
+
+drop:
+	update_stats(q, len, 1);
+	FREE_PKT(m);
+	return (1);
+}
+
+
+/* Dequeue a pcaket form queue q */
+static struct mbuf * 
+aqm_codel_dequeue(struct dn_queue *q)
+{
+	return codel_dequeue(q);
+}
+
+/* 
+ * initialize Codel for queue 'q' 
+ * First allocate memory for codel status.
+ */
+static int 
+aqm_codel_init(struct dn_queue *q)
+{
+	struct codel_status *cst;
+
+	if (!q->fs->aqmcfg) {
+		D("Codel is not configure!d");
+		return EINVAL;
+	}
+
+	q->aqm_status = malloc(sizeof(struct codel_status),
+			 M_DUMMYNET, M_NOWAIT | M_ZERO);
+	if (q->aqm_status == NULL) {
+		D("Cannot allocate AQM_codel private data");
+		return ENOMEM ; 
+	}
+
+	/* init codel status variables */
+	cst = q->aqm_status;
+	cst->dropping=0;
+	cst->first_above_time=0;
+	cst->drop_next_time=0;
+	cst->count=0;
+	cst->maxpkt_size = 500;
+
+	/* increase reference counters */
+	codel_desc.ref_count++;
+	q->fs->nr_of_qes++;
+
+	return 0;
+}
+
+/* 
+ * Clean up Codel status for queue 'q' 
+ * Destroy memory allocated for codel status.
+ */
+static int
+aqm_codel_cleanup(struct dn_queue *q)
+{
+
+	if (q && q->aqm_status) {
+		free(q->aqm_status, M_DUMMYNET);
+		q->aqm_status = NULL;
+		/* decrease reference counters */
+		codel_desc.ref_count--;
+		q->fs->nr_of_qes--;
+	}
+	else
+		D("Codel already cleaned up");
+	return 0;
+}
+
+/* 
+ * Config codel parameters
+ * also allocate memory for codel configurations
+ */
+static int
+aqm_codel_config(struct dn_fsk* fs, struct dn_extra_parms *ep, int len)
+{
+	struct dn_aqm_codel_parms *ccfg;
+
+	int l = sizeof(struct dn_extra_parms);
+	if (len < l) {
+		D("invalid sched parms length got %d need %d", len, l);
+		return EINVAL;
+	}
+	/* we free the old cfg because maybe the original allocation 
+	 * not the same size as the new one (different AQM type).
+	 */
+	if (fs->aqmcfg) {
+		free(fs->aqmcfg, M_DUMMYNET);
+		fs->aqmcfg = NULL;
+	}
+
+	fs->aqmcfg = malloc(sizeof(struct dn_aqm_codel_parms),
+			 M_DUMMYNET, M_NOWAIT | M_ZERO);
+	if (fs->aqmcfg== NULL) {
+		D("cannot allocate AQM_codel configuration parameters");
+		return ENOMEM; 
+	}
+	
+	/* configure codel parameters */
+	ccfg = fs->aqmcfg;
+	
+	if (ep->par[0] < 0)
+		ccfg->target = codel_sysctl.target;
+	else
+		ccfg->target = ep->par[0];
+
+	if (ep->par[1] < 0)
+		ccfg->interval = codel_sysctl.interval;
+	else
+		ccfg->interval = ep->par[1];
+
+	if (ep->par[2] < 0)
+		ccfg->flags = 0;
+	else
+		ccfg->flags = ep->par[2];
+
+	/* bound codel configurations */
+	ccfg->target = BOUND_VAR(ccfg->target,1,1000);
+	ccfg->interval = BOUND_VAR(ccfg->interval,1,5000);
+	/* increase config reference counter */
+	codel_desc.cfg_ref_count++;
+
+	return 0;
+}
+
+/*
+ * Deconfigure Codel and free memory allocation
+ */
+static int
+aqm_codel_deconfig(struct dn_fsk* fs)
+{
+
+	if (fs && fs->aqmcfg) {
+		free(fs->aqmcfg, M_DUMMYNET);
+		fs->aqmcfg = NULL;
+		fs->aqmfp = NULL;
+		/* decrease config reference counter */
+		codel_desc.cfg_ref_count--;
+	}
+
+	return 0;
+}
+
+/* 
+ * Retrieve Codel configuration parameters.
+ */ 
+static int
+aqm_codel_getconfig(struct dn_fsk *fs, struct dn_extra_parms * ep)
+{
+	struct dn_aqm_codel_parms *ccfg;
+
+	if (fs->aqmcfg) {
+		strcpy(ep->name, codel_desc.name);
+		ccfg = fs->aqmcfg;
+		ep->par[0] = ccfg->target;
+		ep->par[1] = ccfg->interval;
+		ep->par[2] = ccfg->flags;
+		return 0;
+	}
+	return 1;
+}
+
+static struct dn_aqm codel_desc = {
+	_SI( .type = )  DN_AQM_CODEL,
+	_SI( .name = )  "CODEL",
+	_SI( .enqueue = )  aqm_codel_enqueue,
+	_SI( .dequeue = )  aqm_codel_dequeue,
+	_SI( .config = )  aqm_codel_config,
+	_SI( .getconfig = )  aqm_codel_getconfig,
+	_SI( .deconfig = )  aqm_codel_deconfig,
+	_SI( .init = )  aqm_codel_init,
+	_SI( .cleanup = )  aqm_codel_cleanup,
+};
+
+DECLARE_DNAQM_MODULE(dn_aqm_codel, &codel_desc);
+
+
+#endif
diff -NaubBE a/sys/netpfil/ipfw/dn_aqm_codel.h b/sys/netpfil/ipfw/dn_aqm_codel.h
--- a/sys/netpfil/ipfw/dn_aqm_codel.h	1970-01-01 10:00:00.000000000 +1000
+++ b/sys/netpfil/ipfw/dn_aqm_codel.h	2016-02-17 22:12:11.725354000 +1100
@@ -0,0 +1,206 @@
+/*
+ * Codel - The Controlled-Delay Active Queue Management algorithm.
+ * 
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ * 
+ * Copyright (C) 2011-2014 Kathleen Nichols <nichols@pollere.com>.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * o  Redistributions of source code must retain the above copyright
+ *  notice, this list of conditions, and the following disclaimer,
+ *  without modification.
+ *
+ * o  Redistributions in binary form must reproduce the above copyright
+ *  notice, this list of conditions and the following disclaimer in
+ *  the documentation and/or other materials provided with the
+ *  distribution.
+ * 
+ * o  The names of the authors may not be used to endorse or promote
+ *  products derived from this software without specific prior written
+ *  permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General Public
+ * License ("GPL") version 2, in which case the provisions of the GPL
+ * apply INSTEAD OF those given above.
+
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _IP_DN_AQM_CODEL_H
+#define _IP_DN_AQM_CODEL_H
+
+
+// XXX How to choose MTAG?
+#define DN_AQM_MTAG_CODEL 55345
+#define FIX_POINT_BITS 16 
+
+enum {
+	CODEL_ECN_ENABLED = 1
+};
+
+/* Codel parameters */
+struct dn_aqm_codel_parms {
+	uint32_t	target;
+	uint32_t	interval;
+	uint32_t	flags;
+};
+
+/* codel status variables */
+struct codel_status {
+	uint16_t	count;	/* number of dropped pkts since entering drop state */
+	uint16_t	dropping;	/* dropping state */
+	uint32_t	drop_next_time;	/* time for next drop */
+	uint32_t	first_above_time;	/* time for next drop */
+	uint16_t	isqrt;	/* last isqrt for control low */
+	uint16_t	maxpkt_size;	/* max packet size seen so far */
+};
+
+struct mbuf *codel_extract_head(struct dn_queue *, uint64_t *);
+uint64_t control_law(struct codel_status *, struct dn_aqm_codel_parms *, uint64_t );
+
+__inline static struct mbuf *
+codel_dodequeue(struct dn_queue *q, uint64_t now, uint16_t *ok_to_drop)
+{
+	struct mbuf * m;
+	struct dn_aqm_codel_parms *cprms;
+	struct codel_status *cst;
+	uint64_t  pkt_ts, sojourn_time;
+
+	*ok_to_drop = 0;
+	m = codel_extract_head(q, &pkt_ts);
+	
+	cst = q->aqm_status;
+	
+	if (m == NULL) {
+		/* queue is empty - we can't be above target */
+		cst->first_above_time= 0;
+		return m;
+	}
+
+	cprms = q->fs->aqmcfg;
+
+	/* To span a large range of bandwidths, CoDel runs two
+	 * different AQMs in parallel. One is sojourn-time-based
+	 * and takes effect when the time to send an MTU-sized
+	 * packet is less than target.  The 1st term of the "if"
+	 * below does this.  The other is backlog-based and takes
+	 * effect when the time to send an MTU-sized packet is >=
+	* target. The goal here is to keep the output link
+	* utilization high by never allowing the queue to get
+	* smaller than the amount that arrives in a typical
+	 * interarrival time (MTU-sized packets arriving spaced
+	 * by the amount of time it takes to send such a packet on
+	 * the bottleneck). The 2nd term of the "if" does this.
+	 */
+	sojourn_time = now - pkt_ts;
+	if (sojourn_time < cprms->target || q->ni.len_bytes <= cst->maxpkt_size) {
+		/* went below - stay below for at least interval */
+		cst->first_above_time = 0;
+	} else {
+		if (cst->first_above_time == 0) {
+			/* just went above from below. if still above at
+			 * first_above_time, will say it's ok to drop. */
+			cst->first_above_time = now + cprms->interval;
+		} else if (now >= cst->first_above_time) {
+			*ok_to_drop = 1;
+		}
+	}
+	return m;
+}
+
+/* 
+ * Dequeue a packet from queue 'q'
+ */
+__inline static struct mbuf * 
+codel_dequeue(struct dn_queue *q)
+{
+	struct mbuf *m;
+	struct dn_aqm_codel_parms *cprms;
+	struct codel_status *cst;
+	uint64_t now;
+	uint16_t ok_to_drop;
+
+	cst = q->aqm_status;;
+	cprms = q->fs->aqmcfg;
+	now = NOW;
+
+	m = codel_dodequeue(q, now, &ok_to_drop);
+	if (cst->dropping) {
+		if (!ok_to_drop) {
+			/* sojourn time below target - leave dropping state */
+			cst->dropping = false;
+		}
+		/*
+		 * Time for the next drop. Drop current packet and dequeue
+		 * next.  If the dequeue doesn't take us out of dropping
+		 * state, schedule the next drop. A large backlog might
+		 * result in drop rates so high that the next drop should
+		 * happen now, hence the 'while' loop.
+		 */
+		while (now >= cst->drop_next_time && cst->dropping) {
+
+			if (!(cprms->flags & CODEL_ECN_ENABLED) || !ecn_mark(m)) {
+				update_stats(q, 0, 1);
+				FREE_PKT(m);
+			}
+
+			m = codel_dodequeue(q, now, &ok_to_drop);
+			if (!ok_to_drop) {
+				// leave dropping state
+				cst->dropping = false;
+			} else {
+					cst->count++;
+				/* schedule the next drop. */
+				cst->drop_next_time = control_law(cst, cprms, cst->drop_next_time);
+			}
+		}
+	/* If we get here we're not in dropping state. The 'ok_to_drop'
+	 * return from dodequeue means that the sojourn time has been
+	 * above 'target' for 'interval' so enter dropping state.
+	 */
+	} else if (ok_to_drop) {
+				if (!(cprms->flags & CODEL_ECN_ENABLED) || !ecn_mark(m)) {
+					update_stats(q, 0, 1);
+					FREE_PKT(m);
+				}
+		m = codel_dodequeue(q, now, &ok_to_drop);
+		cst->dropping = true;
+
+		/* If min went above target close to when it last went
+		 * below, assume that the drop rate that controlled the
+		 * queue on the last cycle is a good starting point to
+		 * control it now. ('drop_next' will be at most 'interval'
+		 * later than the time of the last drop so 'now - drop_next'
+		 * is a good approximation of the time from the last drop
+		 * until now.)
+		 */
+		cst->count = (cst->count > 2 && now - 
+			cst->drop_next_time < 8* cprms->interval)? cst->count - 2 : 1;
+
+		/* set initial guess for Newton's method isqrt to 0.5 */
+		cst->isqrt = (1UL<<FIX_POINT_BITS) /2; 
+		cst->drop_next_time = control_law(cst, cprms, now);
+	}
+	
+	return m;
+}
+
+#endif
diff -NaubBE a/sys/netpfil/ipfw/dn_sched.h b/sys/netpfil/ipfw/dn_sched.h
--- a/sys/netpfil/ipfw/dn_sched.h	2016-02-12 10:35:44.949862000 +1100
+++ b/sys/netpfil/ipfw/dn_sched.h	2016-02-17 22:12:11.725442000 +1100
@@ -132,6 +132,10 @@
 	int (*free_fsk)(struct dn_fsk *f);
 	int (*new_queue)(struct dn_queue *q);
 	int (*free_queue)(struct dn_queue *q);
+#ifdef NEW_AQM
+	/* Getting scheduler extra parameters */
+	int (*getconfig)(struct dn_schk *, struct dn_extra_parms *);
+#endif
 
 	/* run-time fields */
 	int ref_count;      /* XXX number of instances in the system */
@@ -165,6 +169,11 @@
 	struct mbuf *m = q->mq.head;
 	if (m == NULL)
 		return NULL;
+#ifdef NEW_AQM
+	/* Call AQM dequeue function  */
+	if (q->fs->aqmfp && q->fs->aqmfp->dequeue )
+		return q->fs->aqmfp->dequeue(q);
+#endif
 	q->mq.head = m->m_nextpkt;
 	q->mq.count--;
 
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_fifo.c b/sys/netpfil/ipfw/dn_sched_fifo.c
--- a/sys/netpfil/ipfw/dn_sched_fifo.c	2016-02-12 10:35:44.951284000 +1100
+++ b/sys/netpfil/ipfw/dn_sched_fifo.c	2016-02-17 22:12:09.004654000 +1100
@@ -42,6 +42,9 @@
 #include <netinet/ip_dummynet.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 #else
 #include <dn_test.h>
@@ -116,6 +119,9 @@
 	_SI( .free_fsk = )  NULL,
 	_SI( .new_queue = )  NULL,
 	_SI( .free_queue = )  NULL,
+#ifdef NEW_AQM
+	_SI( .getconfig = )  NULL,
+#endif
 };
 
 DECLARE_DNSCHED_MODULE(dn_fifo, &fifo_desc);
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_fq_codel.c b/sys/netpfil/ipfw/dn_sched_fq_codel.c
--- a/sys/netpfil/ipfw/dn_sched_fq_codel.c	1970-01-01 10:00:00.000000000 +1000
+++ b/sys/netpfil/ipfw/dn_sched_fq_codel.c	2016-02-17 22:12:09.004766000 +1100
@@ -0,0 +1,569 @@
+/* 
+ * FQ_Codel - The FlowQueue-Codel scheduler/AQM
+ * 
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifdef _KERNEL
+#include <sys/malloc.h>
+#include <sys/socket.h>
+//#include <sys/socketvar.h>
+#include <sys/kernel.h>
+#include <sys/mbuf.h>
+#include <sys/module.h>
+#include <net/if.h>	/* IFNAMSIZ */
+#include <netinet/in.h>
+#include <netinet/ip_var.h>		/* ipfw_rule_ref */
+#include <netinet/ip_fw.h>	/* flow_id */
+#include <netinet/ip_dummynet.h>
+
+#include <sys/proc.h>
+#include <sys/rwlock.h>
+
+#include <netpfil/ipfw/ip_fw_private.h>
+#include <sys/sysctl.h>
+#include <netinet/ip.h>
+#include <netinet/ip6.h>
+#include <netinet/ip_icmp.h>
+#include <netinet/tcp.h>
+#include <netinet/udp.h>
+#include <sys/queue.h>
+#include <sys/hash.h>
+
+#include <netpfil/ipfw/dn_heap.h>
+#include <netpfil/ipfw/ip_dn_private.h>
+
+#include <netpfil/ipfw/dn_aqm.h>
+#include <netpfil/ipfw/dn_aqm_codel.h>
+#include <netpfil/ipfw/dn_sched.h>
+#include <netpfil/ipfw/dn_sched_fq_codel.h>
+#include <netpfil/ipfw/dn_sched_fq_codel_helper.h>
+
+#else
+#include <dn_test.h>
+#endif
+
+/* NOTE: In fq_codel module, we reimplements CoDel AQM functions 
+ * because fq_codel use different flows (sub-queues) structure and 
+ * dn_queue includes many variables not needed by a flow (sub-queue 
+ * )i.e. avoid extra overhead (88 bytes vs 208 bytes).
+ * Also, CoDel functions manages stats of sub-queues as well as the main queue.
+ */
+
+#define DN_SCHED_FQ_CODEL 6
+
+static struct dn_alg fq_codel_desc;
+
+/* fq_codel parameters including codel */
+struct dn_sch_fq_codel_parms 
+ fq_codel_sysctl = {{5,100,0},1024,10240,1514};	/* defaults */
+
+SYSBEGIN(f4)
+
+SYSCTL_DECL(_net_inet);
+SYSCTL_DECL(_net_inet_ip);
+SYSCTL_DECL(_net_inet_ip_dummynet);
+static SYSCTL_NODE(_net_inet_ip_dummynet, OID_AUTO, fqcodel, CTLFLAG_RW, 0, "FQ_CODEL");
+
+#ifdef SYSCTL_NODE
+SYSCTL_UINT(_net_inet_ip_dummynet_fqcodel, OID_AUTO, target,
+	CTLFLAG_RW, &fq_codel_sysctl.ccfg.target, 5, "Target in ms for FQ_CoDel");
+SYSCTL_UINT(_net_inet_ip_dummynet_fqcodel, OID_AUTO, interval,
+	CTLFLAG_RW, &fq_codel_sysctl.ccfg.interval, 100, "Interval in ms for FQ_CoDel");
+SYSCTL_UINT(_net_inet_ip_dummynet_fqcodel, OID_AUTO, quantum,
+	CTLFLAG_RW, &fq_codel_sysctl.quantum, 1514, "quantum for FQ_CoDel");
+SYSCTL_UINT(_net_inet_ip_dummynet_fqcodel, OID_AUTO, flows,
+	CTLFLAG_RW, &fq_codel_sysctl.flows_cnt, 1024, "Number of queues for FQ_CoDel");
+SYSCTL_UINT(_net_inet_ip_dummynet_fqcodel, OID_AUTO, limit,
+	CTLFLAG_RW, &fq_codel_sysctl.limit, 10240, "limit for FQ_CoDel");
+#endif
+
+/* Drop a packet form the head of codel queue */
+static void
+codel_drop_head(struct fq_codel_flow *q, struct fq_codel_si *si)
+{
+	struct mbuf *m = q->mq.head;
+
+	if (m == NULL)
+		return;
+	q->mq.head = m->m_nextpkt;
+
+	fq_update_stats(q, si, -m->m_pkthdr.len, 1);
+
+	if (si->main_q.ni.length == 0) /* queue is now idle */
+			si->main_q.q_time = dn_cfg.curr_time;
+
+	m_freem(m);
+}
+
+/* Enqueue a packet 'm' to a queue 'q' and add timestamp to that packet.
+ * Return 1 when unable to add timestamp, otherwise return 0 
+ */
+static int
+codel_enqueue(struct fq_codel_flow *q, struct mbuf *m, struct fq_codel_si *si)
+{
+	uint64_t len;
+
+	len = m->m_pkthdr.len;
+	/* finding maximum packet size */
+	if (len > q->cst.maxpkt_size)
+		q->cst.maxpkt_size = len;
+
+	/* Add timestamp to mbuf as MTAG */
+	struct m_tag *mtag;
+	mtag = m_tag_locate(m, MTAG_ABI_COMPAT, DN_AQM_MTAG_CODEL, NULL);
+	if (mtag == NULL)
+		mtag = m_tag_alloc(MTAG_ABI_COMPAT, DN_AQM_MTAG_CODEL, sizeof(uint64_t),
+			M_NOWAIT);
+	if (mtag == NULL) {
+		m_freem(m); 
+		goto drop;
+	}
+	*(uint64_t *)(mtag + 1) = NOW;
+	m_tag_prepend(m, mtag);
+
+	mq_append(&q->mq, m);
+	fq_update_stats(q, si, len, 0);
+	return 0;
+
+drop:
+	fq_update_stats(q, si, len, 1);
+	m_freem(m);
+	return 1;
+}
+
+/*
+ * Classify a packet to queue number using Jenkins hash function.
+ * Return: queue number 
+ * the input of the hash are protocol no, perturbation, src IP, dst IP,
+ * src port, dst port,
+ */
+static inline int
+fq_codel_classify_flow(struct mbuf *m, uint16_t fcount, struct fq_codel_si *si)
+{
+	struct ip *ip;
+	struct tcphdr *th;
+	struct udphdr *uh;
+	uint8_t tuple[39];
+	uint16_t hash=0;
+
+//#ifdef INET6
+	struct ip6_hdr *ip6;
+	int isip6;
+	isip6 = (mtod(m, struct ip *)->ip_v == 6) ? 1 : 0;
+
+	if(isip6) {
+		ip6 = mtod(m, struct ip6_hdr *);
+		*((uint8_t *) &tuple[0]) = ip6->ip6_nxt;
+		*((uint16_t *) &tuple[1]) = si->perturbation;
+		memcpy(&tuple[3],ip6->ip6_src.s6_addr,16);
+		memcpy(&tuple[19],ip6->ip6_dst.s6_addr,16);
+
+		switch (ip6->ip6_nxt) {
+		case IPPROTO_TCP:
+			th = (struct tcphdr *)(ip6 + 1);
+			*((uint16_t *) &tuple[35]) =th->th_dport;
+			*((uint16_t *) &tuple[37]) = th->th_sport;
+			break;
+
+		case IPPROTO_UDP:
+			uh = (struct udphdr *)(ip6 + 1);
+			*((uint16_t *) &tuple[35]) = uh->uh_dport;
+			*((uint16_t *) &tuple[37]) = uh->uh_sport;
+			break;
+		default:
+			memset(&tuple[35],0,4);
+			
+		}
+
+		hash = jenkins_hash(tuple, 39, HASHINIT) %  fcount;
+		return hash;
+	} 
+//#endif
+
+	/* IPv4 */
+	ip = mtod(m, struct ip *);
+	*((uint8_t *) &tuple[0]) = ip->ip_p;
+	*((uint16_t *) &tuple[1]) = si->perturbation;
+	*((uint32_t *) &tuple[3]) = ip->ip_src.s_addr;
+	*((uint32_t *) &tuple[7]) = ip->ip_dst.s_addr;
+
+	switch (ip->ip_p) {
+		case IPPROTO_TCP:
+			th = (struct tcphdr *)(ip + 1);
+			*((uint16_t *) &tuple[11]) =th->th_dport;
+			*((uint16_t *) &tuple[13]) = th->th_sport;
+			break;
+
+		case IPPROTO_UDP:
+			uh = (struct udphdr *)(ip + 1);
+			*((uint16_t *) &tuple[11]) = uh->uh_dport;
+			*((uint16_t *) &tuple[13]) = uh->uh_sport;
+			break;
+		default:
+			memset(&tuple[11],0,4);
+
+	}
+	hash = jenkins_hash(tuple, 15, HASHINIT) %  fcount;
+
+	return hash;
+}
+
+/*
+ * Enqueue a packet into an appropriate queue according to
+ * FQ_CODEL algorithm.
+ */
+static int 
+fq_codel_enqueue(struct dn_sch_inst *_si, struct dn_queue *_q, 
+	struct mbuf *m)
+{
+	struct fq_codel_si *si;
+	struct fq_codel_schk *schk;
+	struct dn_sch_fq_codel_parms *param;
+	struct dn_queue *mainq;
+	int idx, drop, i, maxidx;
+
+	mainq = (struct dn_queue *)(_si + 1);
+	si = (struct fq_codel_si *)_si;
+	schk = (struct fq_codel_schk *)(si->_si.sched+1);
+	param = &schk->cfg;
+
+	 /* classify a packet to queue number*/
+	idx = fq_codel_classify_flow(m, param->flows_cnt, si);
+	/* enqueue packet into appropriate queue using CoDel AQM.
+	 * Note: 'codel_enqueue' function returns 1 only when it unable to 
+	 * add timestamp to packet (no limit check)*/
+	drop = codel_enqueue(&si->flows[idx], m, si);
+	
+	/* codel unable to timestamp a packet */ 
+	if (drop)
+		return 1;
+	
+	/* If the flow (sub-queue) is not active ,then add it to tail of
+	 * new flows list, initialize and activate it.
+	 */
+	if (!si->flows[idx].active ) {
+		STAILQ_INSERT_TAIL(&si->newflows, &si->flows[idx], flowchain);
+		si->flows[idx].deficit = param->quantum;
+		si->flows[idx].cst.dropping = false;
+		si->flows[idx].cst.first_above_time = 0;
+		si->flows[idx].active = 1;
+	}
+
+	/* check the limit for all queues and remove a packet from the
+	 * largest one 
+	 */
+	if (mainq->ni.length > schk->cfg.limit) {
+		/* find first active flow */
+		for (maxidx = 0; maxidx < schk->cfg.flows_cnt; maxidx++)
+			if (si->flows[maxidx].active)
+				break;
+		if (maxidx < schk->cfg.flows_cnt) {
+			/* find the largest sub- queue */
+			for (i = maxidx + 1; i < schk->cfg.flows_cnt; i++) 
+				if (si->flows[i].active && si->flows[i].stats.length >
+					si->flows[maxidx].stats.length)
+					maxidx = i;
+			codel_drop_head(&si->flows[maxidx], si);
+			drop = 1;
+		}
+	}
+
+	return drop;
+}
+
+/*
+ * Dequeue a packet from an appropriate queue according to
+ * FQ_CODEL algorithm.
+ */
+static struct mbuf *
+fq_codel_dequeue(struct dn_sch_inst *_si)
+{
+	struct fq_codel_si *si;
+	struct fq_codel_schk *schk;
+	struct dn_sch_fq_codel_parms *param;
+	struct fq_codel_flow *f;
+	struct mbuf *mbuf;
+	struct fq_codel_list *fq_codel_flowlist;
+
+	si = (struct fq_codel_si *)_si;
+	schk = (struct fq_codel_schk *)(si->_si.sched+1);
+	param = &schk->cfg;
+
+	do {
+		/* select a list to start with */
+		if (STAILQ_EMPTY(&si->newflows))
+			fq_codel_flowlist = &si->oldflows;
+		else
+			fq_codel_flowlist = &si->newflows;
+
+		/* Both new and old queue lists are empty, return NULL */
+		if (STAILQ_EMPTY(fq_codel_flowlist)) 
+			return NULL;
+
+		f = STAILQ_FIRST(fq_codel_flowlist);
+		while (f != NULL)	{
+			/* if there is no flow(sub-queue) deficit, increase deficit
+			 * by quantum, move the flow to the tail of old flows list
+			 * and try another flow.
+			 * Otherwise, the flow will be used for dequeue.
+			 */
+			if (f->deficit < 0) {
+				 f->deficit += param->quantum;
+				 STAILQ_REMOVE_HEAD(fq_codel_flowlist, flowchain);
+				 STAILQ_INSERT_TAIL(&si->oldflows, f, flowchain);
+			 } else 
+				 break;
+
+			f = STAILQ_FIRST(fq_codel_flowlist);
+		}
+		
+		/* the new flows list is empty, try old flows list */
+		if (STAILQ_EMPTY(fq_codel_flowlist)) 
+			continue;
+
+		/* Dequeue a packet from the selected flow */
+		mbuf = fqc_codel_dequeue(f, si);
+
+		/* Codel did not return a packet */
+		if (!mbuf) {
+			/* If the selected flow belongs to new flows list, then move 
+			 * it to the tail of old flows list. Otherwise, deactivate it and
+			 * remove it from the old list and
+			 */
+			if (fq_codel_flowlist == &si->newflows) {
+				STAILQ_REMOVE_HEAD(fq_codel_flowlist, flowchain);
+				STAILQ_INSERT_TAIL(&si->oldflows, f, flowchain);
+			}	else {
+				f->active = 0;
+				STAILQ_REMOVE_HEAD(fq_codel_flowlist, flowchain);
+			}
+			/* start again */
+			continue;
+		}
+
+		/* we have a packet to return, 
+		 * update flow deficit and return the packet*/
+		f->deficit -= mbuf->m_pkthdr.len;
+		return mbuf;
+
+	} while (1);
+	
+	/* unreachable point */
+	return NULL;
+}
+
+/*
+ * Initialize fq_codel scheduler instance.
+ * also, allocate memory for flows array.
+ */
+static int
+fq_codel_new_sched(struct dn_sch_inst *_si)
+{
+	struct fq_codel_si *si;
+	struct dn_queue *q;
+	struct fq_codel_schk *schk;
+	int i;
+
+	si = (struct fq_codel_si *)_si;
+	schk = (struct fq_codel_schk *)(_si->sched+1);
+
+	if(si->flows) {
+		D("si already configured!");
+		return 0;
+	}
+
+	/* init the main queue */
+	q = &si->main_q;
+	set_oid(&q->ni.oid, DN_QUEUE, sizeof(*q));
+	q->_si = _si;
+	q->fs = _si->sched->fs;
+
+	/* allocate memory for flows array */
+	si->flows = malloc(schk->cfg.flows_cnt * sizeof(struct fq_codel_flow),
+		 M_DUMMYNET, M_NOWAIT | M_ZERO);
+	if (si->flows == NULL) {
+		D("cannot allocate memory for fq_codel configuration parameters");
+		return ENOMEM ; 
+	}
+
+	/* init perturbation for this si */
+	si->perturbation = random();
+
+	/* init the old and new flows lists */
+	STAILQ_INIT(&si->newflows);
+	STAILQ_INIT(&si->oldflows);
+
+	/* init the flows (sub-queues) */
+	for (i = 0; i < schk->cfg.flows_cnt; i++) {
+		/* init mq queue */
+		memset(&si->flows[i], 0, sizeof(si->flows[i]));
+		/* init codel */
+		si->flows[i].cst.maxpkt_size = 500;
+	}
+
+	fq_codel_desc.ref_count++;
+	return 0;
+}
+
+/*
+ * Free fq_codel scheduler instance.
+ */
+static int
+fq_codel_free_sched(struct dn_sch_inst *_si)
+{
+	struct fq_codel_si *si = (struct fq_codel_si *)_si ;
+
+	/* free the flows array */
+	free(si->flows , M_DUMMYNET);
+	si->flows = NULL;
+	fq_codel_desc.ref_count--;
+
+	return 0;
+}
+
+/*
+ * Configure fq_codel scheduler.
+ * the configurations for the scheduler is passed from userland.
+ */
+static int
+fq_codel_config(struct dn_schk *_schk)
+{
+	struct fq_codel_schk *schk;
+	struct dn_extra_parms *ep;
+	struct dn_sch_fq_codel_parms *fqc_cfg;
+	
+	schk = (struct fq_codel_schk *)(_schk+1);
+	ep = (struct dn_extra_parms *) _schk->cfg;
+
+	/* par array contains fq_codel configuration as follow
+	 * Codel: 0- target,1- interval, 2- flags
+	 * FQ_CODEL: 3- quantum, 4- limit, 5- flows
+	 */
+	if (ep && ep->oid.len ==sizeof(*ep) &&
+		ep->oid.subtype == DN_SCH_PARAMS) {
+
+		fqc_cfg = &schk->cfg;
+		if (ep->par[0] < 0)
+			fqc_cfg->ccfg.target = fq_codel_sysctl.ccfg.target;
+		else
+			fqc_cfg->ccfg.target = ep->par[0];
+
+		if (ep->par[1] < 0)
+			fqc_cfg->ccfg.interval = fq_codel_sysctl.ccfg.interval;
+		else
+			fqc_cfg->ccfg.interval = ep->par[1];
+
+		if (ep->par[2] < 0)
+			fqc_cfg->ccfg.flags = 0;
+		else
+			fqc_cfg->ccfg.flags = ep->par[2];
+
+		/* FQ configurations */
+		if (ep->par[3] < 0)
+			fqc_cfg->quantum = fq_codel_sysctl.quantum;
+		else
+			fqc_cfg->quantum = ep->par[3];
+
+		if (ep->par[4] < 0)
+			fqc_cfg->limit = fq_codel_sysctl.limit;
+		else
+			fqc_cfg->limit = ep->par[4];
+
+		if (ep->par[5] < 0)
+			fqc_cfg->flows_cnt = fq_codel_sysctl.flows_cnt;
+		else
+			fqc_cfg->flows_cnt = ep->par[5];
+
+		/* Bound the configurations */
+		fqc_cfg->ccfg.target = BOUND_VAR(fqc_cfg->ccfg.target,1,1000); ;
+		fqc_cfg->ccfg.interval = BOUND_VAR(fqc_cfg->ccfg.interval,1,5000);
+
+		fqc_cfg->quantum = BOUND_VAR(fqc_cfg->quantum,256,6056);
+		fqc_cfg->limit= BOUND_VAR(fqc_cfg->limit,1,20480);
+		fqc_cfg->flows_cnt= BOUND_VAR(fqc_cfg->flows_cnt,1,65536);
+	}
+	else
+		return 1;
+
+	return 0;
+}
+
+/*
+ * Return fq_codel scheduler configurations
+ * the configurations for the scheduler is passed to userland.
+ */
+static int 
+fq_codel_getconfig (struct dn_schk *_schk, struct dn_extra_parms *ep) {
+	
+	struct fq_codel_schk *schk = (struct fq_codel_schk *)(_schk+1);
+	struct dn_sch_fq_codel_parms *fqc_cfg;
+
+	fqc_cfg = &schk->cfg;
+
+	strcpy(ep->name, fq_codel_desc.name);
+	ep->par[0] = fqc_cfg->ccfg.target;
+	ep->par[1] = fqc_cfg->ccfg.interval;
+	ep->par[2] = fqc_cfg->ccfg.flags;
+
+	ep->par[3] = fqc_cfg->quantum;
+	ep->par[4] = fqc_cfg->limit;
+	ep->par[5] = fqc_cfg->flows_cnt;
+
+	return 0;
+}
+
+/*
+ * fq_codel scheduler descriptor
+ * contains the type of the scheduler, the name, the size of extra
+ * data structures, and function pointers.
+ */
+static struct dn_alg fq_codel_desc = {
+	_SI( .type = )  DN_SCHED_FQ_CODEL,
+	_SI( .name = ) "FQ_CODEL",
+	_SI( .flags = ) 0,
+
+	_SI( .schk_datalen = ) sizeof(struct fq_codel_schk),
+	_SI( .si_datalen = ) sizeof(struct fq_codel_si) - sizeof(struct dn_sch_inst),
+	_SI( .q_datalen = ) 0,
+
+	_SI( .enqueue = ) fq_codel_enqueue,
+	_SI( .dequeue = ) fq_codel_dequeue,
+	_SI( .config = ) fq_codel_config, /* new sched i.e. sched X config ...*/
+	_SI( .destroy = ) NULL,  /*sched x delete */
+	_SI( .new_sched = ) fq_codel_new_sched, /* new schd instance */
+	_SI( .free_sched = ) fq_codel_free_sched,	/* delete schd instance */
+	_SI( .new_fsk = ) NULL,
+	_SI( .free_fsk = ) NULL,
+	_SI( .new_queue = ) NULL,
+	_SI( .free_queue = ) NULL,
+	_SI( .getconfig = )  fq_codel_getconfig,
+	_SI( .ref_count = ) 0
+};
+
+DECLARE_DNSCHED_MODULE(dn_fq_codel, &fq_codel_desc);
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_fq_codel.h b/sys/netpfil/ipfw/dn_sched_fq_codel.h
--- a/sys/netpfil/ipfw/dn_sched_fq_codel.h	1970-01-01 10:00:00.000000000 +1000
+++ b/sys/netpfil/ipfw/dn_sched_fq_codel.h	2016-02-17 22:14:03.000000000 +1100
@@ -0,0 +1,165 @@
+/*-
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * FQ_Codel Structures and helper functions
+ */
+
+#ifndef _IP_DN_SCHED_FQ_CODEL_H
+#define _IP_DN_SCHED_FQ_CODEL_H
+
+/* list of queues */
+STAILQ_HEAD(fq_codel_list, fq_codel_flow) ;
+
+/* fq_codel parameters including codel */
+struct dn_sch_fq_codel_parms {
+	struct dn_aqm_codel_parms	ccfg;	/* CoDel Parameters */
+	/* FQ_CODEL Parameters */
+	uint32_t flows_cnt;	/* number of flows */
+	uint32_t limit;	/* hard limit of fq_codel queue size*/
+	uint32_t quantum;
+};	/* defaults */
+
+/* flow (sub-queue) stats */
+struct flow_stats {
+	uint64_t tot_pkts;	/* statistics counters  */
+	uint64_t tot_bytes;
+	uint32_t length;		/* Queue length, in packets */
+	uint32_t len_bytes;	/* Queue length, in bytes */
+	uint32_t drops;
+};
+
+/* A flow of packets (sub-queue).*/
+struct fq_codel_flow {
+	struct mq	mq;	/* list of packets */
+	struct flow_stats stats;	/* statistics */
+	int	deficit;
+	int active;		/* 1: flow is active (in a list) */
+	struct codel_status cst;
+	STAILQ_ENTRY(fq_codel_flow) flowchain;
+};
+
+/* extra fq_codel scheduler configurations */
+struct fq_codel_schk {
+	struct dn_sch_fq_codel_parms cfg;
+};
+
+/* fq_codel scheduler instance */
+struct fq_codel_si {
+	struct dn_sch_inst _si;	/* standard scheduler instance */
+	struct dn_queue main_q; /* main queue is after si directly */
+
+	struct fq_codel_flow *flows; /* array of flows (queues) */
+	uint32_t perturbation; /* random value */
+	struct fq_codel_list newflows;	/* list of new queues */
+	struct fq_codel_list oldflows;		/* list of old queues */
+};
+
+/* Helper function to update queue&main-queue and scheduler statistics.
+ * negative len + drop -> drop
+ * negative len -> dequeue
+ * positive len -> enqueue
+ * positive len + drop -> drop during enqueue
+ */
+__inline static void
+fq_update_stats(struct fq_codel_flow *q, struct fq_codel_si *si, int len,
+	int drop)
+{
+	int inc = 0;
+
+	if (len < 0) 
+		inc = -1;
+	else if(len > 0)
+		inc = 1;
+
+	if(!drop) {
+		/* Update stats for the main queue */
+		si->main_q.ni.length += inc;
+		si->main_q.ni.len_bytes += len;
+
+		/*update sub-queue stats */
+		q->stats.length += inc;
+		q->stats.len_bytes += len;
+
+		/*update scheduler instance stats */
+		si->_si.ni.length += inc;
+		si->_si.ni.len_bytes += len;
+	}
+	
+	if (inc > 0) {
+		si->main_q.ni.tot_bytes += len;
+		si->main_q.ni.tot_pkts ++;
+		
+		q->stats.tot_bytes +=len;
+		q->stats.tot_pkts++;
+		
+		si->_si.ni.tot_bytes +=len;
+		si->_si.ni.tot_pkts ++;
+	}
+
+	if (drop) {
+		si->main_q.ni.drops ++;
+		q->stats.drops ++;
+		si->_si.ni.drops ++;
+		io_pkt_drop ++;
+	}
+
+}
+
+/* extract the head of fq_codel sub-queue */
+__inline static struct mbuf *
+fq_codel_extract_head(struct fq_codel_flow *q, uint64_t *pkt_ts, struct fq_codel_si *si)
+{
+	struct mbuf *m = q->mq.head;
+
+	if (m == NULL)
+		return m;
+	q->mq.head = m->m_nextpkt;
+
+	fq_update_stats(q, si, -m->m_pkthdr.len, 0);
+
+	if (si->main_q.ni.length == 0) /* queue is now idle */
+			si->main_q.q_time = dn_cfg.curr_time;
+
+	/* extract packet timestamp*/
+	struct m_tag *mtag;
+	mtag = m_tag_locate(m, MTAG_ABI_COMPAT, DN_AQM_MTAG_CODEL, NULL);
+	if (mtag == NULL){
+		D("timestamp tag is not found!");
+		*pkt_ts = 0;
+	} else {
+		*pkt_ts = *(uint64_t *)(mtag + 1);
+		m_tag_delete(m,mtag); 
+	}
+
+	return m;
+}
+
+
+#endif
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_fq_codel_helper.h b/sys/netpfil/ipfw/dn_sched_fq_codel_helper.h
--- a/sys/netpfil/ipfw/dn_sched_fq_codel_helper.h	1970-01-01 10:00:00.000000000 +1000
+++ b/sys/netpfil/ipfw/dn_sched_fq_codel_helper.h	2016-02-17 22:15:20.000000000 +1100
@@ -0,0 +1,171 @@
+/*
+ * Codel - The Controlled-Delay Active Queue Management algorithm.
+ * 
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ *
+ * Copyright (C) 2011-2014 Kathleen Nichols <nichols@pollere.com>.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * o  Redistributions of source code must retain the above copyright
+ *  notice, this list of conditions, and the following disclaimer,
+ *  without modification.
+ *
+ * o  Redistributions in binary form must reproduce the above copyright
+ *  notice, this list of conditions and the following disclaimer in
+ *  the documentation and/or other materials provided with the
+ *  distribution.
+ * 
+ * o  The names of the authors may not be used to endorse or promote
+ *  products derived from this software without specific prior written
+ *  permission.
+ *
+ * Alternatively, provided that this notice is retained in full, this
+ * software may be distributed under the terms of the GNU General Public
+ * License ("GPL") version 2, in which case the provisions of the GPL
+ * apply INSTEAD OF those given above.
+
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _IP_DN_SCHED_FQ_CODEL_HELPER_H
+#define _IP_DN_SCHED_FQ_CODEL_HELPER_H
+
+__inline static struct mbuf *
+fqc_dodequeue(struct fq_codel_flow *q, uint64_t now, uint16_t *ok_to_drop,
+	struct fq_codel_si *si)
+{
+	struct mbuf * m;
+	struct fq_codel_schk *schk = (struct fq_codel_schk *)(si->_si.sched+1);
+	uint64_t  pkt_ts, sojourn_time;
+
+	*ok_to_drop = 0;
+	m = fq_codel_extract_head(q, &pkt_ts, si);
+
+	if (m == NULL) {
+		/*queue is empty - we can't be above target*/
+		q->cst.first_above_time= 0;
+		return m;
+	}
+
+	/* To span a large range of bandwidths, CoDel runs two
+	 * different AQMs in parallel. One is sojourn-time-based
+	 * and takes effect when the time to send an MTU-sized
+	 * packet is less than target.  The 1st term of the "if"
+	 * below does this.  The other is backlog-based and takes
+	 * effect when the time to send an MTU-sized packet is >=
+	* target. The goal here is to keep the output link
+	* utilization high by never allowing the queue to get
+	* smaller than the amount that arrives in a typical
+	 * interarrival time (MTU-sized packets arriving spaced
+	 * by the amount of time it takes to send such a packet on
+	 * the bottleneck). The 2nd term of the "if" does this.
+	 */
+	sojourn_time = now - pkt_ts;
+	if (sojourn_time < schk->cfg.ccfg.target || q->stats.len_bytes <= q->cst.maxpkt_size) {
+		/* went below - stay below for at least interval */
+		q->cst.first_above_time = 0;
+	} else {
+		if (q->cst.first_above_time == 0) {
+			/* just went above from below. if still above at
+			 * first_above_time, will say it's ok to drop. */
+			q->cst.first_above_time = now + schk->cfg.ccfg.interval;
+		} else if (now >= q->cst.first_above_time) {
+			*ok_to_drop = 1;
+		}
+	}
+	return m;
+}
+
+/* Codel dequeue function */
+__inline static struct mbuf * 
+fqc_codel_dequeue(struct fq_codel_flow *q, struct fq_codel_si *si)
+{
+	struct mbuf *m;
+	struct dn_aqm_codel_parms *cprms;
+	struct codel_status *cst;
+	uint64_t now;
+	uint16_t ok_to_drop;
+	struct fq_codel_schk *schk = (struct fq_codel_schk *)(si->_si.sched+1);
+
+	cst = &q->cst;
+	cprms = &schk->cfg.ccfg;
+
+	now = NOW;
+	m = fqc_dodequeue(q, now, &ok_to_drop, si);
+
+	if (cst->dropping) {
+		if (!ok_to_drop) {
+			/* sojourn time below target - leave dropping state */
+			cst->dropping = false;
+		}
+
+		/* Time for the next drop. Drop current packet and dequeue
+		 * next.  If the dequeue doesn't take us out of dropping
+		 * state, schedule the next drop. A large backlog might
+		 * result in drop rates so high that the next drop should
+		 * happen now, hence the 'while' loop.
+		 */
+		while (now >= cst->drop_next_time && cst->dropping) {
+			if (!(cprms->flags & CODEL_ECN_ENABLED) || !ecn_mark(m)) {
+				fq_update_stats(q, si, 0, 1);
+				m_freem(m);
+			}
+
+			m = fqc_dodequeue(q, now, &ok_to_drop, si);
+			if (!ok_to_drop) {
+				/* leave dropping state */
+				cst->dropping = false;
+			} else {
+					cst->count++;
+				/* schedule the next drop. */
+				cst->drop_next_time = control_law(cst, cprms, cst->drop_next_time);
+			}
+		}
+	/* If we get here we're not in dropping state. The 'ok_to_drop'
+	 * return from dodequeue means that the sojourn time has been
+	 * above 'target' for 'interval' so enter dropping state.
+	 */
+	} else if (ok_to_drop) {
+				if (!(cprms->flags & CODEL_ECN_ENABLED) || !ecn_mark(m)) {
+					fq_update_stats(q, si, 0, 1);
+					m_freem(m);
+				}
+		m = fqc_dodequeue(q, now, &ok_to_drop,si);
+		cst->dropping = true;
+
+		/* If min went above target close to when it last went
+		 * below, assume that the drop rate that controlled the
+		 * queue on the last cycle is a good starting point to
+		 * control it now. ('drop_next' will be at most 'interval'
+		 * later than the time of the last drop so 'now - drop_next'
+		 * is a good approximation of the time from the last drop
+		 * until now.)
+		 */
+		cst->count = (cst->count > 2 && now - 
+			cst->drop_next_time < 8* cprms->interval)? cst->count - 2 : 1;
+		/* set initial guess for Newton's method isqrt to 0.5 */
+		cst->isqrt = (1UL<<FIX_POINT_BITS) /2; 
+		cst->drop_next_time = control_law(cst, cprms, now);
+	}
+
+	return m;
+}
+
+#endif
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_prio.c b/sys/netpfil/ipfw/dn_sched_prio.c
--- a/sys/netpfil/ipfw/dn_sched_prio.c	2016-02-12 10:35:44.897451000 +1100
+++ b/sys/netpfil/ipfw/dn_sched_prio.c	2016-02-17 22:12:09.004853000 +1100
@@ -41,6 +41,9 @@
 #include <netinet/ip_dummynet.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 #else
 #include <dn_test.h>
@@ -223,6 +226,9 @@
 
 	_SI( .new_queue = ) prio_new_queue,
 	_SI( .free_queue = ) prio_free_queue,
+#ifdef NEW_AQM
+	_SI( .getconfig = )  NULL,
+#endif
 };
 
 
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_qfq.c b/sys/netpfil/ipfw/dn_sched_qfq.c
--- a/sys/netpfil/ipfw/dn_sched_qfq.c	2016-02-12 10:35:44.891998000 +1100
+++ b/sys/netpfil/ipfw/dn_sched_qfq.c	2016-02-17 22:12:09.004940000 +1100
@@ -42,6 +42,9 @@
 #include <netinet/ip_dummynet.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 #else
 #include <dn_test.h>
@@ -837,6 +840,9 @@
 	_SI( .free_fsk = )  NULL,
 	_SI( .new_queue = ) qfq_new_queue,
 	_SI( .free_queue = ) qfq_free_queue,
+#ifdef NEW_AQM
+	_SI( .getconfig = )  NULL,
+#endif
 };
 
 DECLARE_DNSCHED_MODULE(dn_qfq, &qfq_desc);
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_rr.c b/sys/netpfil/ipfw/dn_sched_rr.c
--- a/sys/netpfil/ipfw/dn_sched_rr.c	2016-02-12 10:35:44.895033000 +1100
+++ b/sys/netpfil/ipfw/dn_sched_rr.c	2016-02-17 22:12:09.005008000 +1100
@@ -42,6 +42,9 @@
 #include <netinet/ip_dummynet.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 #else
 #include <dn_test.h>
@@ -309,6 +312,9 @@
 	_SI( .free_fsk = ) NULL,
 	_SI( .new_queue = ) rr_new_queue,
 	_SI( .free_queue = ) rr_free_queue,
+#ifdef NEW_AQM
+	_SI( .getconfig = )  NULL,
+#endif
 };
 
 
diff -NaubBE a/sys/netpfil/ipfw/dn_sched_wf2q.c b/sys/netpfil/ipfw/dn_sched_wf2q.c
--- a/sys/netpfil/ipfw/dn_sched_wf2q.c	2016-02-12 10:35:44.891628000 +1100
+++ b/sys/netpfil/ipfw/dn_sched_wf2q.c	2016-02-17 22:12:09.005072000 +1100
@@ -41,8 +41,12 @@
 #include <netinet/ip_var.h>		/* ipfw_rule_ref */
 #include <netinet/ip_fw.h>	/* flow_id */
 #include <netinet/ip_dummynet.h>
+
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 #else
 #include <dn_test.h>
@@ -367,6 +371,10 @@
 
 	_SI( .new_queue = ) wf2qp_new_queue,
 	_SI( .free_queue = ) wf2qp_free_queue,
+#ifdef NEW_AQM
+	_SI( .getconfig = )  NULL,
+#endif
+
 };
 
 
diff -NaubBE a/sys/netpfil/ipfw/ip_dn_glue.c b/sys/netpfil/ipfw/ip_dn_glue.c
--- a/sys/netpfil/ipfw/ip_dn_glue.c	2016-02-12 10:35:44.892625000 +1100
+++ b/sys/netpfil/ipfw/ip_dn_glue.c	2016-02-17 22:12:09.005163000 +1100
@@ -55,6 +55,9 @@
 #include <netpfil/ipfw/ip_fw_private.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 
 /* FREEBSD7.2 ip_dummynet.h r191715*/
diff -NaubBE a/sys/netpfil/ipfw/ip_dn_io.c b/sys/netpfil/ipfw/ip_dn_io.c
--- a/sys/netpfil/ipfw/ip_dn_io.c	2016-02-12 10:35:44.896732000 +1100
+++ b/sys/netpfil/ipfw/ip_dn_io.c	2016-02-17 22:12:09.005220000 +1100
@@ -63,6 +63,9 @@
 #include <netpfil/ipfw/ip_fw_private.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 
 /*
@@ -84,8 +87,12 @@
 
 static unsigned long	io_pkt;
 static unsigned long	io_pkt_fast;
-static unsigned long	io_pkt_drop;
 
+#ifdef NEW_AQM
+unsigned long	io_pkt_drop;
+#else
+static unsigned long	io_pkt_drop;
+#endif
 /*
  * We use a heap to store entities for which we have pending timer events.
  * The heap is checked at every tick and all entities with expired events
@@ -250,6 +257,15 @@
 dn_tag_get(struct mbuf *m)
 {
 	struct m_tag *mtag = m_tag_first(m);
+#ifdef NEW_AQM
+	/* XXX: to skip m_tag Codel timestamp. For Debugging only*/
+	#define DN_AQM_MTAG_CODEL 55345
+	if (mtag != NULL && mtag->m_tag_id == DN_AQM_MTAG_CODEL) {
+		m_tag_delete(m,mtag); 
+		mtag = m_tag_first(m);
+		D("skip codel tag");
+	}
+#endif
 	KASSERT(mtag != NULL &&
 	    mtag->m_tag_cookie == MTAG_ABI_COMPAT &&
 	    mtag->m_tag_id == PACKET_TAG_DUMMYNET,
@@ -257,6 +273,7 @@
 	return (struct dn_pkt_tag *)(mtag+1);
 }
 
+#ifndef NEW_AQM
 static inline void
 mq_append(struct mq *q, struct mbuf *m)
 {
@@ -296,6 +313,7 @@
 	q->tail = m;
 	m->m_nextpkt = NULL;
 }
+#endif
 
 /*
  * Dispose a list of packet. Use a functions so if we need to do
@@ -420,7 +438,10 @@
 /*
  * ECN/ECT Processing (partially adopted from altq)
  */
-static int
+#ifndef NEW_AQM
+static
+#endif
+int
 ecn_mark(struct mbuf* m)
 {
 	struct ip *ip;
@@ -503,6 +524,11 @@
 		goto drop;
 	if (f->plr && random() < f->plr)
 		goto drop;
+#ifdef NEW_AQM
+	/* Call AQM enqueue function */
+	if (q->fs->aqmfp)
+		return q->fs->aqmfp->enqueue(q ,m);
+#endif
 	if (f->flags & DN_IS_RED && red_drops(q, m->m_pkthdr.len)) {
 		if (!(f->flags & DN_IS_ECN) || !ecn_mark(m))
 			goto drop;
diff -NaubBE a/sys/netpfil/ipfw/ip_dn_private.h b/sys/netpfil/ipfw/ip_dn_private.h
--- a/sys/netpfil/ipfw/ip_dn_private.h	2016-02-12 10:35:44.897287000 +1100
+++ b/sys/netpfil/ipfw/ip_dn_private.h	2016-02-17 22:12:11.725650000 +1100
@@ -81,6 +81,10 @@
 SLIST_HEAD(dn_queue_head, dn_queue);
 SLIST_HEAD(dn_alg_head, dn_alg);
 
+#ifdef NEW_AQM
+SLIST_HEAD(dn_aqm_head, dn_aqm); /* for new AQMs */
+#endif
+
 struct mq {	/* a basic queue of packets*/
         struct mbuf *head, *tail;
 	int count;
@@ -136,6 +140,9 @@
 	/* list of flowsets without a scheduler -- use sch_chain */
 	struct dn_fsk_head	fsu;	/* list of unlinked flowsets */
 	struct dn_alg_head	schedlist;	/* list of algorithms */
+#ifdef NEW_AQM
+	struct dn_aqm_head	aqmlist;	/* list of AQMs */
+#endif
 
 	/* Store the fs/sch to scan when draining. The value is the
 	 * bucket number of the hash table. Expire can be disabled
@@ -232,6 +239,11 @@
 	int lookup_weight ;	/* equal to (1-w_q)^t / (1-w_q)^(t+1) */
 	int avg_pkt_size ;	/* medium packet size */
 	int max_pkt_size ;	/* max packet size */
+#ifdef NEW_AQM
+	struct dn_aqm *aqmfp;	/* Pointer to AQM functions */
+	void *aqmcfg;	/* configuration parameters for AQM */
+	uint32_t nr_of_qes;	/* number of queues used by this fs */
+#endif
 };
 
 /*
@@ -254,6 +266,9 @@
 	int count;		/* arrivals since last RED drop */
 	int random;		/* random value (scaled) */
 	uint64_t q_time;	/* start of queue idle time */
+#ifdef NEW_AQM
+	void *aqm_status;	/* per-queue status variables*/
+#endif
 
 };
 
@@ -401,4 +416,49 @@
 void dn_drain_scheduler(void);
 void dn_drain_queue(void);
 
+#ifdef NEW_AQM
+int ecn_mark(struct mbuf* m);
+
+/* moved from ip_dn_io.c to here to be available for AQMs modules*/
+static inline void
+mq_append(struct mq *q, struct mbuf *m)
+{
+#ifdef USERSPACE
+	// buffers from netmap need to be copied
+	// XXX note that the routine is not expected to fail
+	ND("append %p to %p", m, q);
+	if (m->m_flags & M_STACK) {
+		struct mbuf *m_new;
+		void *p;
+		int l, ofs;
+
+		ofs = m->m_data - m->__m_extbuf;
+		// XXX allocate
+		MGETHDR(m_new, M_NOWAIT, MT_DATA);
+		ND("*** WARNING, volatile buf %p ext %p %d dofs %d m_new %p",
+			m, m->__m_extbuf, m->__m_extlen, ofs, m_new);
+		p = m_new->__m_extbuf;	/* new pointer */
+		l = m_new->__m_extlen;	/* new len */
+		if (l <= m->__m_extlen) {
+			panic("extlen too large");
+		}
+
+		*m_new = *m;	// copy
+		m_new->m_flags &= ~M_STACK;
+		m_new->__m_extbuf = p; // point to new buffer
+		_pkt_copy(m->__m_extbuf, p, m->__m_extlen);
+		m_new->m_data = p + ofs;
+		m = m_new;
+	}
+#endif /* USERSPACE */
+	if (q->head == NULL)
+		q->head = m;
+	else
+		q->tail->m_nextpkt = m;
+	q->count++;
+	q->tail = m;
+	m->m_nextpkt = NULL;
+}
+#endif /* NEW_AQM */
+
 #endif /* _IP_DN_PRIVATE_H */
diff -NaubBE a/sys/netpfil/ipfw/ip_dummynet.c b/sys/netpfil/ipfw/ip_dummynet.c
--- a/sys/netpfil/ipfw/ip_dummynet.c	2016-02-12 10:35:44.891327000 +1100
+++ b/sys/netpfil/ipfw/ip_dummynet.c	2016-02-17 22:12:09.005339000 +1100
@@ -1,4 +1,11 @@
 /*-
+ * Codel/FQ_Codel Code:
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ * 
  * Copyright (c) 1998-2002,2010 Luigi Rizzo, Universita` di Pisa
  * Portions Copyright (c) 2000 Akamba Corp.
  * All rights reserved
@@ -58,6 +64,9 @@
 #include <netpfil/ipfw/ip_fw_private.h>
 #include <netpfil/ipfw/dn_heap.h>
 #include <netpfil/ipfw/ip_dn_private.h>
+#ifdef NEW_AQM
+#include <netpfil/ipfw/dn_aqm.h>
+#endif
 #include <netpfil/ipfw/dn_sched.h>
 
 /* which objects to copy */
@@ -98,6 +107,21 @@
 }
 /*----- end of callout hooks -----*/
 
+#ifdef NEW_AQM
+/* Return AQM descriptor for given type or name. */
+static struct dn_aqm *
+find_aqm_type(int type, char *name)
+{
+	struct dn_aqm *d;
+
+	SLIST_FOREACH(d, &dn_cfg.aqmlist, next) {
+		if (d->type == type || (name && !strcasecmp(d->name, name)))
+			return d;
+	}
+	return NULL; /* not found */
+}
+#endif
+
 /* Return a scheduler descriptor given the type or name. */
 static struct dn_alg *
 find_sched_type(int type, char *name)
@@ -320,7 +344,15 @@
 
 	if (fs->sched->fp->new_queue)
 		fs->sched->fp->new_queue(q);
+
+#ifdef NEW_AQM
+	/* call AQM init function after creating a queue*/
+	if (fs->aqmfp && fs->aqmfp->init)
+		if(fs->aqmfp->init(q))
+			D("unable to init AQM for fs %d", fs->fs.fs_nr);
+#endif
 	dn_cfg.queue_count++;
+
 	return q;
 }
 
@@ -334,6 +366,13 @@
 {
 	struct dn_fsk *fs = q->fs;
 
+#ifdef NEW_AQM
+	/* clean up AQM status for queue 'q'
+	 * cleanup here is called just with MULTIQUEUE
+	 */
+	if (fs && fs->aqmfp && fs->aqmfp->cleanup)
+		fs->aqmfp->cleanup(q);
+#endif
 	// D("fs %p si %p\n", fs, q->_si);
 	/* notify the parent scheduler that the queue is going away */
 	if (fs && fs->sched->fp->free_queue)
@@ -475,6 +514,16 @@
 	if (s->sch.flags & DN_HAVE_MASK)
 		si->ni.fid = *(struct ipfw_flow_id *)key;
 
+#ifdef NEW_AQM
+	/* init AQM status for !DN_MULTIQUEUE sched*/
+	if (!(s->fp->flags & DN_MULTIQUEUE))
+		if (s->fs->aqmfp && s->fs->aqmfp->init)
+			if(s->fs->aqmfp->init((struct dn_queue *)(si + 1))) {
+				D("unable to init AQM for fs %d", s->fs->fs.fs_nr);
+				goto error;
+			}
+#endif
+
 	dn_cfg.si_count++;
 	return si;
 
@@ -504,6 +553,20 @@
 	dn_free_pkts(dl->mq.head);	/* drain delay line */
 	if (si->kflags & DN_ACTIVE) /* remove si from event heap */
 		heap_extract(&dn_cfg.evheap, si);
+
+#ifdef NEW_AQM
+	/* clean up AQM status for !DN_MULTIQUEUE sched
+	 * Note that all queues belong to fs were cleaned up in fsk_detach.
+	 * When drain_scheduler is called s->fs and q->fs are pointing 
+	 * to a correct fs, so we can use fs in this case.
+	 */
+	if (!(s->fp->flags & DN_MULTIQUEUE)) {
+		struct dn_queue *q = (struct dn_queue *)(si + 1);
+		if (q->aqm_status && q->fs->aqmfp)
+			if (q->fs->aqmfp->cleanup)
+				q->fs->aqmfp->cleanup(q);
+	}
+#endif
 	if (s->fp->free_sched)
 		s->fp->free_sched(si);
 	bzero(si, sizeof(*si));	/* safety */
@@ -592,6 +655,67 @@
 	return fs;
 }
 
+#ifdef NEW_AQM
+/* callback to cleanup queue AQM status belong to a flowset connected
+ * scheduler instance '_si' (for !DN_MULTIQUEUE only).
+ */
+static int
+si_cleanup_q(void *_si, void *arg)
+{
+	struct dn_sch_inst *si = _si;
+
+	if (!(si->sched->fp->flags & DN_MULTIQUEUE)) {
+		if (si->sched->fs->aqmfp && si->sched->fs->aqmfp->cleanup)
+			si->sched->fs->aqmfp->cleanup((struct dn_queue *) (si+1));
+	}
+	return 0;
+}
+
+/* callback to clean up queue AQM status.*/
+static int
+q_cleanup(void *_q, void *arg)
+{
+	struct dn_queue *q = _q;
+	q->fs->aqmfp->cleanup(q);
+	return 0;
+}
+
+/* Clean up all queues AQM status belonging to a flowset and then
+ * deconfig AQM for flowset 'fs'
+ */
+static void 
+aqm_cleanup_deconfig_fs(struct dn_fsk *fs)
+{
+	struct dn_sch_inst *si;
+	
+	/* clean up AQM status for all queues for !DN_MULTIQUEUE sched*/
+	if (fs->fs.fs_nr > DN_MAX_ID) {
+		if (fs->sched && !(fs->sched->fp->flags & DN_MULTIQUEUE)) {
+			if (fs->sched->sch.flags & DN_HAVE_MASK)
+				dn_ht_scan(fs->sched->siht, si_cleanup_q, NULL);
+			else {
+					/* single si i.e. no sched mask */
+					si = (struct dn_sch_inst *) fs->sched->siht;
+					if (si && fs->aqmfp && fs->aqmfp->cleanup)
+						fs->aqmfp->cleanup((struct dn_queue *) (si+1));
+			}
+		} 
+	}
+	
+	/* clean up AQM status for all queues for DN_MULTIQUEUE sched*/
+	if (fs->sched && fs->sched->fp->flags & DN_MULTIQUEUE && fs->qht) {
+			if (fs->fs.flags & DN_QHT_HASH)
+				dn_ht_scan(fs->qht, q_cleanup, NULL);
+			else
+				fs->aqmfp->cleanup((struct dn_queue *)(fs->qht));
+	}
+	
+	/* deconfig AQM */
+	if(fs->aqmcfg && fs->aqmfp && fs->aqmfp->deconfig)
+		fs->aqmfp->deconfig(fs);
+}
+#endif
+
 /*
  * detach flowset from its current scheduler. Flags as follows:
  * DN_DETACH removes from the fsk_list
@@ -620,6 +744,10 @@
 		free(fs->w_q_lookup, M_DUMMYNET);
 	fs->w_q_lookup = NULL;
 	qht_delete(fs, flags);
+#ifdef NEW_AQM
+	aqm_cleanup_deconfig_fs(fs);
+#endif
+
 	if (fs->sched && fs->sched->fp->free_fsk)
 		fs->sched->fp->free_fsk(fs);
 	fs->sched = NULL;
@@ -1191,6 +1319,193 @@
 	}
 }
 
+#ifdef NEW_AQM
+/* Retrieve AQM configurations to userland
+ */
+static int
+get_aqm_parms(struct sockopt *sopt)
+{
+	struct dn_extra_parms  *ep;
+	struct dn_fsk *fs;
+	size_t sopt_valsize;
+	int l, err = 0;
+	
+	sopt_valsize = sopt->sopt_valsize;
+	l = sizeof(*ep);
+	if (sopt->sopt_valsize < l) {
+		D("bad len sopt->sopt_valsize %d len %d",
+			(int) sopt->sopt_valsize , l);
+		err = EINVAL;
+		return err;
+	}
+	ep = malloc(l, M_DUMMYNET, M_WAITOK);
+	if(!ep) {
+		err = ENOMEM ;
+		return err;
+	}
+	do {
+		err = sooptcopyin(sopt, ep, l, l);
+		if(err)
+			break;
+		sopt->sopt_valsize = sopt_valsize;
+		if (ep->oid.len < l) {
+			err = EINVAL;
+			break;
+		}
+
+		fs = dn_ht_find(dn_cfg.fshash, ep->nr, 0, NULL);
+		if (!fs) {
+			D("fs %d not found", ep->nr);
+			err = EINVAL;
+			break;
+		}
+		
+		if (fs->aqmfp && fs->aqmfp->getconfig) {
+			if(fs->aqmfp->getconfig(fs, ep)) {
+				D("Error while trying to get AQM params");
+				err = EINVAL;
+				break;
+			}
+			ep->oid.len = l;
+			err = sooptcopyout(sopt, ep, l);
+		}
+	}while(0);
+
+	free(ep, M_DUMMYNET);
+	return err;
+}
+
+/* Retrieve AQM configurations to userland
+ */
+static int
+get_sched_parms(struct sockopt *sopt)
+{
+	struct dn_extra_parms  *ep;
+	struct dn_schk *schk;
+	size_t sopt_valsize;
+	int l, err = 0;
+	
+	sopt_valsize = sopt->sopt_valsize;
+	l = sizeof(*ep);
+	if (sopt->sopt_valsize < l) {
+		D("bad len sopt->sopt_valsize %d len %d",
+			(int) sopt->sopt_valsize , l);
+		err = EINVAL;
+		return err;
+	}
+	ep = malloc(l, M_DUMMYNET, M_WAITOK);
+	if(!ep) {
+		err = ENOMEM ;
+		return err;
+	}
+	do {
+		err = sooptcopyin(sopt, ep, l, l);
+		if(err)
+			break;
+		sopt->sopt_valsize = sopt_valsize;
+		if (ep->oid.len < l) {
+			err = EINVAL;
+			break;
+		}
+
+		schk = locate_scheduler(ep->nr);
+		if (!schk) {
+			D("sched %d not found", ep->nr);
+			err = EINVAL;
+			break;
+		}
+		
+		if (schk->fp && schk->fp->getconfig) {
+			if(schk->fp->getconfig(schk, ep)) {
+				D("Error while trying to get sched params");
+				err = EINVAL;
+				break;
+			}
+			ep->oid.len = l;
+			err = sooptcopyout(sopt, ep, l);
+		}
+	}while(0);
+	free(ep, M_DUMMYNET);
+
+	return err;
+}
+
+/* Configure AQM for flowset 'fs'.
+ * extra parameters are originally pass from userland.
+ */
+static int
+config_aqm(struct dn_fsk *fs, struct  dn_extra_parms *ep)
+{
+	int err = 0;
+
+	do {
+		/* no configurations */
+		if (!ep) {
+			err = 0;
+			break;
+		}
+
+		/* no AQM for this flowset*/
+		if (!strcmp(ep->name,"")) {
+			err = 0;
+			break;
+		}
+		if (ep->oid.len < sizeof(*ep)) {
+			D("short aqm len %d", ep->oid.len);
+				err = EINVAL;
+				break;
+		}
+
+		if (fs->sched && fs->sched->siht && !(fs->sched->fp->flags & DN_MULTIQUEUE)) {
+			D("cannot configure FS that already has si");
+			err = EINVAL;
+			break;
+		}
+
+		/* check if there are some queues for this flowset */
+		if (fs->aqmfp && fs->nr_of_qes) {
+			D("This flowset (%d) is busy now no. of q=%d!\n", fs->fs.fs_nr ,fs->nr_of_qes);
+			err = EINVAL;
+			break;
+		}
+
+		/* deconfigure old aqm if exist */
+		if (fs->aqmcfg && fs->aqmfp && fs->aqmfp->deconfig) {
+			aqm_cleanup_deconfig_fs(fs);
+		}
+
+		if (!(fs->aqmfp = find_aqm_type(0, ep->name))) {
+			D("AQM functions not found for type %s!", ep->name);
+			fs->fs.flags &= ~DN_IS_AQM;
+			err = EINVAL;
+			break;
+		} else
+			fs->fs.flags |= DN_IS_AQM;
+
+		if (ep->oid.subtype != DN_AQM_PARAMS) {
+				D("Wrong subtype");
+				err = EINVAL;
+				break;
+		}
+
+		if (fs->aqmfp->config) {
+			err = fs->aqmfp->config(fs, ep, ep->oid.len);
+			if (err) {
+					D("Unable to configure AQM for FS %d", fs->fs.fs_nr );
+					fs->fs.flags &= ~DN_IS_AQM;
+					fs->aqmfp = NULL;
+					break;
+			}
+
+			//D("AQM %s configured for fs=%d",fs->aqmfp->name, fs->fs.fs_nr);
+		}
+		
+	} while(0);
+
+	return err;
+}
+#endif
+
 /*
  * Configuration -- to preserve backward compatibility we use
  * the following scheme (N is 65536)
@@ -1314,6 +1629,14 @@
 		D("missing sched for flowset %d", i);
 	        break;
 	    }
+#ifdef NEW_AQM
+		/* don't allow to reconfigure AQM for  active fs (has active queue(s)) */
+		if(fs->nr_of_qes) {
+			D("The flowset (%d) is busy now, no. of queues=%d",i ,fs->nr_of_qes);
+			fs =  NULL;
+			break;
+		}
+#endif
 	    /* grab some defaults from the existing one */
 	    if (nfs->sched_nr == 0) /* reuse */
 		nfs->sched_nr = fs->fs.sched_nr;
@@ -1323,6 +1646,9 @@
 	    }
 	    if (bcmp(&fs->fs, nfs, sizeof(*nfs)) == 0) {
 		ND("flowset %d unchanged", i);
+#ifdef NEW_AQM
+		config_aqm(fs, (struct dn_extra_parms *) arg);
+#endif
 		break; /* no change, nothing to do */
 	    }
 	    if (oldc != dn_cfg.fsk_count)	/* new item */
@@ -1341,6 +1667,10 @@
 		fsk_detach(fs, flags);
 	    }
 	    fs->fs = *nfs; /* copy configuration */
+#ifdef NEW_AQM
+		fs->aqmfp = NULL;
+		config_aqm(fs, (struct dn_extra_parms *) arg);
+#endif
 	    if (s != NULL)
 		fsk_attach(fs, s);
 	} while (0);
@@ -1866,6 +2196,19 @@
 		// cmd->id = sopt_valsize;
 		D("compatibility mode");
 	}
+
+#ifdef NEW_AQM
+	/* get AQM params */
+	if(cmd->subtype == DN_AQM_PARAMS) {
+		error = get_aqm_parms(sopt);
+		goto done;
+	/* get Scheduler params */
+	} else if (cmd->subtype == DN_SCH_PARAMS) {
+		error = get_sched_parms(sopt);
+		goto done;
+	}
+#endif
+
 	a.extra = (struct copy_range *)cmd;
 	if (cmd->len == sizeof(*cmd)) { /* no range, create a default */
 		uint32_t *rp = (uint32_t *)(cmd + 1);
@@ -2318,4 +2661,99 @@
  */
 //VNET_SYSUNINIT(vnet_dn_uninit, DN_SI_SUB, DN_MODEV_ORD+2, ip_dn_destroy, NULL);
 
+#ifdef NEW_AQM
+
+/* modevent helpers for the AQM modules */
+static int
+load_dn_aqm(struct dn_aqm *d)
+{
+	struct dn_aqm *aqm=NULL;
+
+	if (d == NULL)
+		return 1; /* error */
+	ip_dn_init();	/* just in case, we need the lock */
+
+	/* Check that mandatory funcs exists */
+	if (d->enqueue == NULL || d->dequeue == NULL) {
+		D("missing enqueue or dequeue for %s", d->name);
+		return 1;
+	}
+
+	/* Search if AQM already exists */
+	DN_BH_WLOCK();
+	SLIST_FOREACH(aqm, &dn_cfg.aqmlist, next) {
+		if (strcmp(aqm->name, d->name) == 0) {
+			D("%s already loaded", d->name);
+			break; /* AQM already exists */
+		}
+	}
+	if (aqm == NULL)
+		SLIST_INSERT_HEAD(&dn_cfg.aqmlist, d, next);
+	DN_BH_WUNLOCK();
+	D("dn_aqm %s %sloaded", d->name, aqm ? "not ":"");
+	return aqm ? 1 : 0;
+}
+
+
+/* Callback to clean up AQM status for queues connected to a flowset
+ * and then deconfigure the flowset.
+ * This function is called before an AQM module is unloaded
+ */
+static int
+fs_cleanup(void *_fs, void *arg)
+{
+	struct dn_fsk *fs = _fs;
+	uint32_t type = *(uint32_t *)arg;
+
+	if (fs->aqmfp && fs->aqmfp->type == type)
+		aqm_cleanup_deconfig_fs(fs);
+
+	return 0;
+}
+
+static int
+unload_dn_aqm(struct dn_aqm *aqm)
+{
+	struct dn_aqm *tmp, *r;
+	int err = EINVAL;
+	err = 0;
+	ND("called for %s", aqm->name);
+
+	DN_BH_WLOCK();
+
+	/* clean up AQM status and deconfig flowset */
+	dn_ht_scan(dn_cfg.fshash, fs_cleanup, &aqm->type);
+
+	SLIST_FOREACH_SAFE(r, &dn_cfg.aqmlist, next, tmp) {
+		if (strcmp(aqm->name, r->name) != 0)
+			continue;
+		ND("ref_count = %d", r->ref_count);
+		err = (r->ref_count != 0 || r->cfg_ref_count != 0) ? EBUSY : 0;
+		if (err == 0)
+			SLIST_REMOVE(&dn_cfg.aqmlist, r, dn_aqm, next);
+		break;
+	}
+	DN_BH_WUNLOCK();
+	D("%s %sunloaded", aqm->name, err ? "not ":"");
+	if (err)
+		D("ref_count=%d, cfg_ref_count=%d", r->ref_count, r->cfg_ref_count);
+	return err;
+}
+
+int
+dn_aqm_modevent(module_t mod, int cmd, void *arg)
+{
+	struct dn_aqm *aqm = arg;
+
+	if (cmd == MOD_LOAD)
+		return load_dn_aqm(aqm);
+	else if (cmd == MOD_UNLOAD)
+		return unload_dn_aqm(aqm);
+	else
+		return EINVAL;
+}
+#endif
+
 /* end of file */
+
+
diff -NaubBE a/sbin/ipfw/dummynet.c b/sbin/ipfw/dummynet.c
--- a/sbin/ipfw/dummynet.c	2016-02-12 10:34:07.101062000 +1100
+++ b/sbin/ipfw/dummynet.c	2016-02-17 13:32:56.192062000 +1100
@@ -1,4 +1,11 @@
 /*
+ * Codel/FQ_Codel Code:
+ * Copyright (C) 2016 Centre for Advanced Internet Architectures,
+ *  Swinburne University of Technology, Melbourne, Australia.
+ * Portions of this code were made possible in part by a gift from 
+ *  The Comcast Innovation Fund.
+ * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
+ * 
  * Copyright (c) 2002-2003,2010 Luigi Rizzo
  *
  * Redistribution and use in source forms, with and without modification,
@@ -15,6 +21,7 @@
  * dummynet support
  */
 
+#define NEW_AQM
 #include <sys/types.h>
 #include <sys/socket.h>
 /* XXX there are several sysctl leftover here */
@@ -59,6 +66,10 @@
 	{ "ecn",		TOK_ECN },
 	{ "red",		TOK_RED },
 	{ "gred",		TOK_GRED },
+#ifdef NEW_AQM
+	{ "codel",		TOK_CODEL}, /* Codel AQM */
+	{ "fq_codel",	TOK_FQ_CODEL}, /* Codel AQM */
+#endif
 	{ "bw",			TOK_BW },
 	{ "bandwidth",		TOK_BW },
 	{ "delay",		TOK_DELAY },
@@ -81,6 +92,19 @@
 	{ NULL, 0 }	/* terminator */
 };
 
+#ifdef NEW_AQM
+/* AQM/extra sched parameters  tokens*/
+static struct _s_x aqm_params[] = {
+	{ "target",		TOK_TARGET},
+	{ "interval",		TOK_INTERVAL},
+	{ "limit",		TOK_LIMIT},
+	{ "flows",		TOK_FLOWS},
+	{ "quantum",		TOK_QUANTUM},
+	{ "ecn",		TOK_ECN},
+	{ NULL, 0 }	/* terminator */
+};
+#endif
+
 #define O_NEXT(p, len) ((void *)((char *)p + len))
 
 static void
@@ -102,6 +126,71 @@
 	return ret;
 }
 
+#ifdef NEW_AQM
+
+/* Codel flags */
+enum {
+	CODEL_ECN_ENABLED = 1
+};
+
+/* Get AQM or scheduler extra parameters  */
+void
+get_extra_parms(uint32_t nr, char *out, int subtype)
+{ 
+	struct dn_extra_parms *ep;
+	int ret;
+	u_int l;
+
+	/* prepare the request */
+	l = sizeof(struct dn_extra_parms);
+	ep = safe_calloc(1, l);
+	memset(ep, 0, sizeof(*ep));
+	*out = '\0';
+
+	oid_fill(&ep->oid, l, DN_CMD_GET, DN_API_VERSION);
+	ep->oid.len = l;
+	ep->oid.subtype = subtype;
+	ep->nr = nr;
+
+	ret = do_cmd(-IP_DUMMYNET3, ep, (uintptr_t)&l);
+	if (ret) {
+		free(ep);
+		errx(EX_DATAERR, "Error getting extra parameters\n");
+	}
+
+	switch (subtype) {
+	case DN_AQM_PARAMS:
+		if( !strcasecmp(ep->name, "codel")) {
+			l = sprintf(out, " AQM CoDel target %ld interval %ld",
+				ep->par[0], 
+				ep->par[1] );
+			if (ep->par[2] & CODEL_ECN_ENABLED)
+				l = sprintf(out + l, " ECN");
+		}
+		break;
+
+	case	DN_SCH_PARAMS:
+		if (!strcasecmp(ep->name,"FQ_CODEL")) {
+			l = sprintf(out," FQ_CODEL target %ld interval %ld"
+				" quantum %ld limit %ld flows %ld",
+				ep->par[0], 
+				ep->par[1],
+				ep->par[3],
+				ep->par[4],
+				ep->par[5]
+				);
+			if (ep->par[2] & CODEL_ECN_ENABLED)
+				l += sprintf(out + l, " ECN");
+			l += sprintf(out + l, "\n");
+		}
+		break;
+	}
+
+	free(ep);
+}
+#endif
+
+
 #if 0
 static int
 sort_q(void *arg, const void *pa, const void *pb)
@@ -246,6 +335,11 @@
 		    1.0 * fs->max_p / (double)(1 << SCALE_RED));
 		if (fs->flags & DN_IS_ECN)
 			strncat(red, " (ecn)", 6);
+#ifdef NEW_AQM
+	/* get AQM parameters */
+	} else if (fs->flags & DN_IS_AQM) {
+			get_extra_parms(fs->fs_nr, red, DN_AQM_PARAMS);
+#endif
 	} else
 		sprintf(red, "droptail");
 
@@ -338,6 +432,11 @@
 	    printf(" sched %d type %s flags 0x%x %d buckets %d active\n",
 			s->sched_nr,
 			s->name, s->flags, s->buckets, s->oid.id);
+#ifdef NEW_AQM
+		char parms[100];
+		get_extra_parms(s->sched_nr, parms, DN_SCH_PARAMS);
+		printf("%s",parms);
+#endif
 	    if (s->flags & DN_HAVE_MASK)
 		print_mask(&s->sched_mask);
 	    }
@@ -745,6 +844,95 @@
 	strncpy(p->name, profile_name, sizeof(p->name));
 }
 
+#ifdef NEW_AQM
+/* Parse AQM/extra scheduler parameters */
+static int 
+process_extra_parms(int *ac, char **av, struct dn_extra_parms *ep,
+	uint16_t type)
+{
+	int i;
+	
+	/* use kernel defaults */
+	for (i=0; i<DN_MAX_EXTRA_PARM; i++)
+		ep->par[i] = -1;
+		
+	switch(type) {
+	case TOK_CODEL:
+	case TOK_FQ_CODEL:
+	/* Codel
+	 * 0- target, 1- interval, 2- flags,
+	 * FQ_CODEL
+	 * 3- quantum, 4- limit, 5- flows
+	 */
+		while (*ac > 0) {
+			int tok = match_token(aqm_params, *av);
+			(*ac)--; av++;
+			switch(tok) {
+			case TOK_TARGET:
+				if (*ac <= 0 || !is_valid_number(av[0]))
+					errx(EX_DATAERR, "target needs number\n");
+
+				ep->par[0] = atoi(av[0]);
+				(*ac)--; av++;
+				break;
+
+			case TOK_INTERVAL:
+				if (*ac <= 0 || !is_valid_number(av[0]))
+					errx(EX_DATAERR, "interval needs number\n");
+
+				ep->par[1] = atoi(av[0]);
+				(*ac)--; av++;
+				break;
+
+			case TOK_ECN:
+				ep->par[2] = CODEL_ECN_ENABLED;
+				break;
+
+			/* Config fq_codel parameters */
+			case TOK_QUANTUM:
+				if (type != TOK_FQ_CODEL)
+					errx(EX_DATAERR, "quantum is not for codel\n");
+				if (*ac <= 0 || !is_valid_number(av[0]))
+					errx(EX_DATAERR, "quantum needs number\n");
+
+				ep->par[3]= atoi(av[0]);
+				(*ac)--; av++;
+				break;
+
+			case TOK_LIMIT:
+				if (type != TOK_FQ_CODEL)
+					errx(EX_DATAERR, "limit is not for codel, use queue instead\n");
+				if (*ac <= 0 || !is_valid_number(av[0]))
+					errx(EX_DATAERR, "limit needs number\n");
+
+				ep->par[4] = atoi(av[0]);
+				(*ac)--; av++;
+				break;
+
+			case TOK_FLOWS:
+				if (type != TOK_FQ_CODEL)
+					errx(EX_DATAERR, "flows is not for codel\n");
+				if (*ac <= 0 || !is_valid_number(av[0]))
+					errx(EX_DATAERR, "flows needs number\n");
+
+				ep->par[5] = atoi(av[0]);
+				(*ac)--; av++;
+				break;
+
+			default:
+				printf("%s is Invalid parameter\n", av[-1]);
+			}
+		}
+		break;
+
+	}
+
+	return 0;
+}
+
+#endif
+
+
 /*
  * configuration of pipes, schedulers, flowsets.
  * When we configure a new scheduler, an empty pipe is created, so:
@@ -776,6 +964,12 @@
 	struct dn_fs *fs = NULL;
 	struct dn_profile *pf = NULL;
 	struct ipfw_flow_id *mask = NULL;
+#ifdef NEW_AQM
+	struct dn_extra_parms *aqm_extra;
+	struct dn_extra_parms *sch_extra;
+	int lmax_extra;
+#endif
+	
 	int lmax;
 	uint32_t _foo = 0, *flags = &_foo , *buckets = &_foo;
 
@@ -787,6 +981,15 @@
 	lmax += sizeof(struct dn_sch) + sizeof(struct dn_link) +
 		sizeof(struct dn_fs) + sizeof(struct dn_profile);
 
+#ifdef NEW_AQM
+	/* Extra Params */
+	lmax_extra = sizeof(struct dn_extra_parms);
+	/* two lmax_extra because one for AQM params and another
+	 * sch params 
+	 */
+	lmax += lmax_extra*2; 
+#endif
+
 	av++; ac--;
 	/* Pipe number */
 	if (ac && isdigit(**av)) {
@@ -812,8 +1015,16 @@
 		 * The FIFO scheduler and link are derived from the
 		 * WF2Q+ one in the kernel.
 		 */
+#ifdef NEW_AQM
+		sch_extra = o_next(&buf, lmax_extra, DN_TEXT);
+		sch_extra ->oid.subtype = 0; /* don't configure scheduler */
+#endif
 		sch = o_next(&buf, sizeof(*sch), DN_SCH);
 		p = o_next(&buf, sizeof(*p), DN_LINK);
+#ifdef NEW_AQM
+		aqm_extra = o_next(&buf, lmax_extra, DN_TEXT);
+		aqm_extra ->oid.subtype = 0; /* don't configure AQM */
+#endif
 		fs = o_next(&buf, sizeof(*fs), DN_FS);
 
 		sch->sched_nr = i;
@@ -831,6 +1042,10 @@
 		break;
 
 	case 2: /* "queue N config ... " */
+#ifdef NEW_AQM
+		aqm_extra = o_next(&buf, lmax_extra, DN_TEXT);
+		aqm_extra ->oid.subtype = 0; 
+#endif
 		fs = o_next(&buf, sizeof(*fs), DN_FS);
 		fs->fs_nr = i;
 		mask = &fs->flow_mask;
@@ -839,7 +1054,15 @@
 		break;
 
 	case 3: /* "sched N config ..." */
+#ifdef NEW_AQM
+		sch_extra = o_next(&buf, lmax_extra, DN_TEXT);
+		sch_extra ->oid.subtype = 0; 
+#endif
 		sch = o_next(&buf, sizeof(*sch), DN_SCH);
+#ifdef NEW_AQM
+		aqm_extra = o_next(&buf, lmax_extra, DN_TEXT);
+		aqm_extra ->oid.subtype = 0;
+#endif
 		fs = o_next(&buf, sizeof(*fs), DN_FS);
 		sch->sched_nr = i;
 		mask = &sch->sched_mask;
@@ -1026,7 +1249,29 @@
 			} /* end while, config masks */
 end_mask:
 			break;
+#ifdef NEW_AQM
+		case TOK_CODEL:
+			NEED(fs, "codel is only for flowsets,");
+
+			fs->flags &= ~(DN_IS_RED|DN_IS_GENTLE_RED);
+			fs->flags |= DN_IS_AQM;
 
+			strcpy(aqm_extra->name,av[-1]);
+			aqm_extra->oid.subtype = DN_AQM_PARAMS;
+			
+			process_extra_parms(&ac, av, aqm_extra, tok);
+			break;
+
+		case TOK_FQ_CODEL:
+			if (!strcmp(av[-1],"type"))
+				errx(EX_DATAERR, "use type before fq_codel");
+
+			NEED(sch, "fq_codel is only for schd");
+			strcpy(sch_extra->name,av[-1]);
+			sch_extra->oid.subtype = DN_SCH_PARAMS;
+			process_extra_parms(&ac, av, sch_extra, tok);
+			break;
+#endif
 		case TOK_RED:
 		case TOK_GRED:
 			NEED1("red/gred needs w_q/min_th/max_th/max_p\n");
@@ -1093,7 +1338,20 @@
 				errx(1, "type %s too long\n", av[0]);
 			strcpy(sch->name, av[0]);
 			sch->oid.subtype = 0; /* use string */
-			ac--; av++;
+#ifdef NEW_AQM
+			/* if fq_codel is selected, consider all tokens after it
+			 * as parameters
+			 */
+			if (!strcasecmp(av[0],"fq_codel")){
+				strcpy(sch_extra->name,av[0]);
+				sch_extra->oid.subtype = DN_SCH_PARAMS;
+				process_extra_parms(&ac, av, sch_extra, tok);
+			} else {
+				ac--;av++;
+			}
+#else
+			ac--;av++;
+#endif
 			break;
 		    }
 
@@ -1187,9 +1445,16 @@
 			errx(EX_DATAERR, "2 <= queue size <= %ld", limit);
 	    }
 
+#ifdef NEW_AQM
+		if ((fs->flags & DN_IS_ECN) && !((fs->flags & DN_IS_RED)|| 
+			(fs->flags & DN_IS_AQM)))
+			errx(EX_USAGE, "ECN can be used with red/gred/"
+				"codel/fq_codel only!");
+#else
 	    if ((fs->flags & DN_IS_ECN) && !(fs->flags & DN_IS_RED))
 		errx(EX_USAGE, "enable red/gred for ECN");
 
+#endif
 	    if (fs->flags & DN_IS_RED) {
 		size_t len;
 		int lookup_depth, avg_pkt_size;
diff -NaubBE a/sbin/ipfw/ipfw2.h b/sbin/ipfw/ipfw2.h
--- a/sbin/ipfw/ipfw2.h	2016-02-12 10:34:07.130604000 +1100
+++ b/sbin/ipfw/ipfw2.h	2016-02-17 13:32:56.192213000 +1100
@@ -170,6 +170,15 @@
 	TOK_ECN,
 	TOK_DROPTAIL,
 	TOK_PROTO,
+#ifdef NEW_AQM
+	/* AQM tokens*/
+	TOK_CODEL, 
+	TOK_FQ_CODEL,
+	TOK_TARGET,
+	TOK_INTERVAL,
+	TOK_FLOWS,
+	TOK_QUANTUM,
+#endif
 	/* dummynet tokens */
 	TOK_WEIGHT,
 	TOK_LMAX,
