New CAM I/O scheduler for FreeBSD. The default I/O scheduler is the same

as before. The common scheduling bits have moved from inline code in
each of the CAM periph drivers into a library that implements the
default scheduling.

In addition, a number of rate-limiting and I/O preference options can
be enabled by adding CAM_IOSCHED_NETFLIX to your config file. A number
of extra stats are also maintained. CAM_IOSCHED_NETFLIX isn't on by
default because it uses a separate BIO_READ and BIO_WRITE queue, so
doesn't honor BIO_ORDERED between these two types of operations. We
already didn't honor it for BIO_DELETE, and we don't depend on
BIO_ORDERED between reads and writes anywhere in the system (it is
currently used with BIO_FLUSH in ZFS to make sure some writes are
complete before others start and as a poor-man's soft dependency in
one place in UFS where we won't be issuing READs until after the
operation completes). However, out of an abundance of caution, it
isn't enabled by default.

Plus, this also brings in NCQ TRIM support for those SSDs that support
it. A black list is also provided for known rogues that use NCQ trim
as an excuse to corrupt the drive. It was difficult to separate out
into a separate commit.

This code has run in production at Netflix for over a year now.

Sponsored by: Netflix, Inc
Differential Revision: https://reviews.freebsd.org/D4609

5 files changed, 597 insertions, 168 deletions

diff --git a/sys/cam/ata/ata_all.h b/sys/cam/ata/ata_all.h
index 433c61c..0eda4f4 100644
--- a/sys/cam/ata/ata_all.h
+++ b/sys/cam/ata/ata_all.h
@@ -46,6 +46,7 @@ struct ata_cmd {
 #define		CAM_ATAIO_CONTROL	0x04	/* Control, not a command */
 #define		CAM_ATAIO_NEEDRESULT	0x08	/* Request requires result. */
 #define		CAM_ATAIO_DMA		0x10	/* DMA command */
+#define		CAM_ATAIO_AUX_HACK	0x20	/* Kludge to make FPDMA DSM TRIM work */
 
 	u_int8_t	command;
 	u_int8_t	features;
diff --git a/sys/cam/ata/ata_da.c b/sys/cam/ata/ata_da.c
index 38cf46e..3d3afbd 100644
--- a/sys/cam/ata/ata_da.c
+++ b/sys/cam/ata/ata_da.c
@@ -59,6 +59,7 @@ __FBSDID("$FreeBSD$");
 #include <cam/cam_periph.h>
 #include <cam/cam_xpt_periph.h>
 #include <cam/cam_sim.h>
+#include <cam/cam_iosched.h>
 
 #include <cam/ata/ata_all.h>
 
@@ -68,6 +69,8 @@ __FBSDID("$FreeBSD$");
 
 #define ATA_MAX_28BIT_LBA               268435455UL
 
+extern int iosched_debug;
+
 typedef enum {
 	ADA_STATE_RAHEAD,
 	ADA_STATE_WCACHE,
@@ -87,17 +90,21 @@ typedef enum {
 	ADA_FLAG_CAN_CFA        = 0x0400,
 	ADA_FLAG_CAN_POWERMGT   = 0x0800,
 	ADA_FLAG_CAN_DMA48	= 0x1000,
-	ADA_FLAG_DIRTY		= 0x2000
+	ADA_FLAG_DIRTY		= 0x2000,
+	ADA_FLAG_CAN_NCQ_TRIM	= 0x4000,	/* CAN_TRIM also set */
+	ADA_FLAG_PIM_CAN_NCQ_TRIM = 0x8000
 } ada_flags;
 
 typedef enum {
 	ADA_Q_NONE		= 0x00,
 	ADA_Q_4K		= 0x01,
+	ADA_Q_NCQ_TRIM_BROKEN	= 0x02,
 } ada_quirks;
 
 #define ADA_Q_BIT_STRING	\
 	"\020"			\
-	"\0014K"
+	"\0014K"		\
+	"\002NCQ_TRIM_BROKEN"
 
 typedef enum {
 	ADA_CCB_RAHEAD		= 0x01,
@@ -112,6 +119,23 @@ typedef enum {
 #define ccb_state	ppriv_field0
 #define ccb_bp		ppriv_ptr1
 
+typedef enum {
+	ADA_DELETE_NONE,
+	ADA_DELETE_DISABLE,
+	ADA_DELETE_CFA_ERASE,
+	ADA_DELETE_DSM_TRIM,
+	ADA_DELETE_NCQ_DSM_TRIM,
+	ADA_DELETE_MIN = ADA_DELETE_CFA_ERASE,
+	ADA_DELETE_MAX = ADA_DELETE_NCQ_DSM_TRIM,
+} ada_delete_methods;
+
+static const char *ada_delete_method_names[] =
+    { "NONE", "DISABLE", "CFA_ERASE", "DSM_TRIM", "NCQ_DSM_TRIM" };
+#if 0
+static const char *ada_delete_method_desc[] =
+    { "NONE", "DISABLED", "CFA Erase", "DSM Trim", "DSM Trim via NCQ" };
+#endif
+
 struct disk_params {
 	u_int8_t  heads;
 	u_int8_t  secs_per_track;
@@ -128,18 +152,18 @@ struct trim_request {
 };
 
 struct ada_softc {
-	struct	 bio_queue_head bio_queue;
-	struct	 bio_queue_head trim_queue;
+	struct   cam_iosched_softc *cam_iosched;
 	int	 outstanding_cmds;	/* Number of active commands */
 	int	 refcount;		/* Active xpt_action() calls */
 	ada_state state;
 	ada_flags flags;
 	ada_quirks quirks;
-	int	 sort_io_queue;
+	ada_delete_methods delete_method;
 	int	 trim_max_ranges;
-	int	 trim_running;
 	int	 read_ahead;
 	int	 write_cache;
+	int	 unmappedio;
+	int	 rotating;
 #ifdef ADA_TEST_FAILURE
 	int      force_read_error;
 	int      force_write_error;
@@ -153,6 +177,13 @@ struct ada_softc {
 	struct sysctl_oid	*sysctl_tree;
 	struct callout		sendordered_c;
 	struct trim_request	trim_req;
+#ifdef CAM_IO_STATS
+	struct sysctl_ctx_list	sysctl_stats_ctx;
+	struct sysctl_oid	*sysctl_stats_tree;
+	u_int	timeouts;
+	u_int	errors;
+	u_int	invalidations;
+#endif
 };
 
 struct ada_quirk_entry {
@@ -330,6 +361,38 @@ static struct ada_quirk_entry ada_quirk_table[] =
 	},
 	{
 		/*
+		 * Crucial M500 SSDs EU07 firmware
+		 * NCQ Trim works ? 
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Crucial CT*M500*", "EU07" },
+		/*quirks*/0
+	},
+	{
+		/*
+		 * Crucial M500 SSDs all other firmware
+		 * NCQ Trim doesn't work
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Crucial CT*M500*", "*" },
+		/*quirks*/ADA_Q_NCQ_TRIM_BROKEN
+	},
+	{
+		/*
+		 * Crucial M550 SSDs
+		 * NCQ Trim doesn't work, but only on MU01 firmware
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Crucial CT*M550*", "MU01" },
+		/*quirks*/ADA_Q_NCQ_TRIM_BROKEN
+	},
+	{
+		/*
+		 * Crucial MX100 SSDs
+		 * NCQ Trim doesn't work, but only on MU01 firmware
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Crucial CT*MX100*", "MU01" },
+		/*quirks*/ADA_Q_NCQ_TRIM_BROKEN
+	},
+	{
+		/*
 		 * Crucial RealSSD C300 SSDs
 		 * 4k optimised
 		 */
@@ -402,6 +465,30 @@ static struct ada_quirk_entry ada_quirk_table[] =
 	},
 	{
 		/*
+		 * Micron M500 SSDs firmware EU07
+		 * NCQ Trim works?
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Micron M500*", "EU07" },
+		/*quirks*/0
+	},
+	{
+		/*
+		 * Micron M500 SSDs all other firmware
+		 * NCQ Trim doesn't work
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Micron M500*", "*" },
+		/*quirks*/ADA_Q_NCQ_TRIM_BROKEN
+	},
+	{
+		/*
+		 * Micron M5[15]0 SSDs
+		 * NCQ Trim doesn't work, but only MU01 firmware
+		 */
+		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Micron M5[15]0*", "MU01" },
+		/*quirks*/ADA_Q_NCQ_TRIM_BROKEN
+	},
+	{
+		/*
 		 * OCZ Agility 2 SSDs
 		 * 4k optimised & trim only works in 4k requests + 4k aligned
 		 */
@@ -451,26 +538,26 @@ static struct ada_quirk_entry ada_quirk_table[] =
 	{
 		/*
 		 * Samsung 830 Series SSDs
-		 * 4k optimised
+		 * 4k optimised, NCQ TRIM Broken (normal TRIM is fine)
 		 */
 		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "SAMSUNG SSD 830 Series*", "*" },
-		/*quirks*/ADA_Q_4K
+		/*quirks*/ADA_Q_4K | ADA_Q_NCQ_TRIM_BROKEN
 	},
 	{
 		/*
 		 * Samsung 840 SSDs
-		 * 4k optimised
+		 * 4k optimised, NCQ TRIM Broken (normal TRIM is fine)
 		 */
 		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Samsung SSD 840*", "*" },
-		/*quirks*/ADA_Q_4K
+		/*quirks*/ADA_Q_4K | ADA_Q_NCQ_TRIM_BROKEN
 	},
 	{
 		/*
 		 * Samsung 850 SSDs
-		 * 4k optimised
+		 * 4k optimised, NCQ TRIM broken (normal TRIM fine)
 		 */
 		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "Samsung SSD 850*", "*" },
-		/*quirks*/ADA_Q_4K
+		/*quirks*/ADA_Q_4K | ADA_Q_NCQ_TRIM_BROKEN
 	},
 	{
 		/*
@@ -478,7 +565,7 @@ static struct ada_quirk_entry ada_quirk_table[] =
 		 * Samsung PM851 Series SSDs (MZ7TE*)
 		 * Samsung PM853T Series SSDs (MZ7GE*)
 		 * Samsung SM863 Series SSDs (MZ7KM*)
-		 * 4k optimised
+		 * 4k optimised, NCQ Trim believed working
 		 */
 		{ T_DIRECT, SIP_MEDIA_FIXED, "*", "SAMSUNG MZ7*", "*" },
 		/*quirks*/ADA_Q_4K
@@ -566,8 +653,6 @@ static void		adaresume(void *arg);
 		 softc->read_ahead : ada_read_ahead)
 #define	ADA_WC	(softc->write_cache >= 0 ? \
 		 softc->write_cache : ada_write_cache)
-#define	ADA_SIO	(softc->sort_io_queue >= 0 ? \
-		 softc->sort_io_queue : cam_sort_io_queues)
 
 /*
  * Most platforms map firmware geometry to actual, but some don't.  If
@@ -624,6 +709,8 @@ static struct periph_driver adadriver =
 	TAILQ_HEAD_INITIALIZER(adadriver.units), /* generation */ 0
 };
 
+static int adadeletemethodsysctl(SYSCTL_HANDLER_ARGS);
+
 PERIPHDRIVER_DECLARE(ada, adadriver);
 
 static int
@@ -719,11 +806,7 @@ adaschedule(struct cam_periph *periph)
 	if (softc->state != ADA_STATE_NORMAL)
 		return;
 
-	/* Check if we have more work to do. */
-	if (bioq_first(&softc->bio_queue) ||
-	    (!softc->trim_running && bioq_first(&softc->trim_queue))) {
-		xpt_schedule(periph, CAM_PRIORITY_NORMAL);
-	}
+	cam_iosched_schedule(softc->cam_iosched, periph);
 }
 
 /*
@@ -756,14 +839,7 @@ adastrategy(struct bio *bp)
 	/*
 	 * Place it in the queue of disk activities for this disk
 	 */
-	if (bp->bio_cmd == BIO_DELETE) {
-		bioq_disksort(&softc->trim_queue, bp);
-	} else {
-		if (ADA_SIO)
-		    bioq_disksort(&softc->bio_queue, bp);
-		else
-		    bioq_insert_tail(&softc->bio_queue, bp);
-	}
+	cam_iosched_queue_work(softc->cam_iosched, bp);
 
 	/*
 	 * Schedule ourselves for performing the work.
@@ -844,7 +920,7 @@ adadump(void *arg, void *virtual, vm_offset_t physical, off_t offset, size_t len
 				    0,
 				    NULL,
 				    0,
-				    ada_default_timeout*1000);
+				    5*1000);
 
 		if (softc->flags & ADA_FLAG_CAN_48BIT)
 			ata_48bit_cmd(&ccb.ataio, ATA_FLUSHCACHE48, 0, 0, 0);
@@ -918,14 +994,16 @@ adaoninvalidate(struct cam_periph *periph)
 	 * De-register any async callbacks.
 	 */
 	xpt_register_async(0, adaasync, periph, periph->path);
+#ifdef CAM_IO_STATS
+	softc->invalidations++;
+#endif
 
 	/*
 	 * Return all queued I/O with ENXIO.
 	 * XXX Handle any transactions queued to the card
 	 *     with XPT_ABORT_CCB.
 	 */
-	bioq_flush(&softc->bio_queue, NULL, ENXIO);
-	bioq_flush(&softc->trim_queue, NULL, ENXIO);
+	cam_iosched_flush(softc->cam_iosched, NULL, ENXIO);
 
 	disk_gone(softc->disk);
 }
@@ -939,12 +1017,20 @@ adacleanup(struct cam_periph *periph)
 
 	cam_periph_unlock(periph);
 
+	cam_iosched_fini(softc->cam_iosched);
+
 	/*
 	 * If we can't free the sysctl tree, oh well...
 	 */
-	if ((softc->flags & ADA_FLAG_SCTX_INIT) != 0
-	    && sysctl_ctx_free(&softc->sysctl_ctx) != 0) {
-		xpt_print(periph->path, "can't remove sysctl context\n");
+	if ((softc->flags & ADA_FLAG_SCTX_INIT) != 0) {
+#ifdef CAM_IO_STATS
+		if (sysctl_ctx_free(&softc->sysctl_stats_ctx) != 0)
+			xpt_print(periph->path,
+			    "can't remove sysctl stats context\n");
+#endif
+		if (sysctl_ctx_free(&softc->sysctl_ctx) != 0)
+			xpt_print(periph->path,
+			    "can't remove sysctl context\n");
 	}
 
 	disk_destroy(softc->disk);
@@ -954,6 +1040,20 @@ adacleanup(struct cam_periph *periph)
 }
 
 static void
+adasetdeletemethod(struct ada_softc *softc)
+{
+
+	if (softc->flags & ADA_FLAG_CAN_NCQ_TRIM)
+		softc->delete_method = ADA_DELETE_NCQ_DSM_TRIM;
+	else if (softc->flags & ADA_FLAG_CAN_TRIM)
+		softc->delete_method = ADA_DELETE_DSM_TRIM;
+	else if ((softc->flags & ADA_FLAG_CAN_CFA) && !(softc->flags & ADA_FLAG_CAN_48BIT))
+		softc->delete_method = ADA_DELETE_CFA_ERASE;
+	else
+		softc->delete_method = ADA_DELETE_NONE;
+}
+
+static void
 adaasync(void *callback_arg, u_int32_t code,
 	struct cam_path *path, void *arg)
 {
@@ -1018,11 +1118,26 @@ adaasync(void *callback_arg, u_int32_t code,
 			softc->flags |= ADA_FLAG_CAN_NCQ;
 		else
 			softc->flags &= ~ADA_FLAG_CAN_NCQ;
+
 		if ((cgd.ident_data.support_dsm & ATA_SUPPORT_DSM_TRIM) &&
-		    (cgd.inq_flags & SID_DMA))
+		    (cgd.inq_flags & SID_DMA)) {
 			softc->flags |= ADA_FLAG_CAN_TRIM;
-		else
-			softc->flags &= ~ADA_FLAG_CAN_TRIM;
+			/*
+			 * If we can do RCVSND_FPDMA_QUEUED commands, we may be able to do
+			 * NCQ trims, if we support trims at all. We also need support from
+			 * the sim do do things properly. Perhaps we should look at log 13
+			 * dword 0 bit 0 and dword 1 bit 0 are set too...
+			 */
+			if ((softc->quirks & ADA_Q_NCQ_TRIM_BROKEN) == 0 &&
+			    (softc->flags & ADA_FLAG_PIM_CAN_NCQ_TRIM) != 0 &&
+			    (cgd.ident_data.satacapabilities2 & ATA_SUPPORT_RCVSND_FPDMA_QUEUED) != 0 &&
+			    (softc->flags & ADA_FLAG_CAN_TRIM) != 0)
+				softc->flags |= ADA_FLAG_CAN_NCQ_TRIM;
+			else
+				softc->flags &= ~ADA_FLAG_CAN_NCQ_TRIM;
+		} else
+			softc->flags &= ~(ADA_FLAG_CAN_TRIM | ADA_FLAG_CAN_NCQ_TRIM);
+		adasetdeletemethod(softc);
 
 		cam_periph_async(periph, code, path, arg);
 		break;
@@ -1100,6 +1215,10 @@ adasysctlinit(void *context, int pending)
 		return;
 	}
 
+	SYSCTL_ADD_PROC(&softc->sysctl_ctx, SYSCTL_CHILDREN(softc->sysctl_tree),
+		OID_AUTO, "delete_method", CTLTYPE_STRING | CTLFLAG_RW,
+		softc, 0, adadeletemethodsysctl, "A",
+		"BIO_DELETE execution method");
 	SYSCTL_ADD_INT(&softc->sysctl_ctx, SYSCTL_CHILDREN(softc->sysctl_tree),
 		OID_AUTO, "read_ahead", CTLFLAG_RW | CTLFLAG_MPSAFE,
 		&softc->read_ahead, 0, "Enable disk read ahead.");
@@ -1107,9 +1226,11 @@ adasysctlinit(void *context, int pending)
 		OID_AUTO, "write_cache", CTLFLAG_RW | CTLFLAG_MPSAFE,
 		&softc->write_cache, 0, "Enable disk write cache.");
 	SYSCTL_ADD_INT(&softc->sysctl_ctx, SYSCTL_CHILDREN(softc->sysctl_tree),
-		OID_AUTO, "sort_io_queue", CTLFLAG_RW | CTLFLAG_MPSAFE,
-		&softc->sort_io_queue, 0,
-		"Sort IO queue to try and optimise disk access patterns");
+		OID_AUTO, "unmapped_io", CTLFLAG_RD | CTLFLAG_MPSAFE,
+		&softc->unmappedio, 0, "Unmapped I/O leaf");
+	SYSCTL_ADD_INT(&softc->sysctl_ctx, SYSCTL_CHILDREN(softc->sysctl_tree),
+		OID_AUTO, "rotating", CTLFLAG_RD | CTLFLAG_MPSAFE,
+		&softc->rotating, 0, "Rotating media");
 #ifdef ADA_TEST_FAILURE
 	/*
 	 * Add a 'door bell' sysctl which allows one to set it from userland
@@ -1129,6 +1250,31 @@ adasysctlinit(void *context, int pending)
 		&softc->periodic_read_error, 0,
 		"Force a read error every N reads (don't set too low).");
 #endif
+
+#ifdef CAM_IO_STATS
+	softc->sysctl_stats_tree = SYSCTL_ADD_NODE(&softc->sysctl_stats_ctx,
+		SYSCTL_CHILDREN(softc->sysctl_tree), OID_AUTO, "stats",
+		CTLFLAG_RD, 0, "Statistics");
+	SYSCTL_ADD_INT(&softc->sysctl_stats_ctx,
+		SYSCTL_CHILDREN(softc->sysctl_stats_tree),
+		OID_AUTO, "timeouts", CTLFLAG_RD | CTLFLAG_MPSAFE,
+		&softc->timeouts, 0,
+		"Device timeouts reported by the SIM");
+	SYSCTL_ADD_INT(&softc->sysctl_stats_ctx,
+		SYSCTL_CHILDREN(softc->sysctl_stats_tree),
+		OID_AUTO, "errors", CTLFLAG_RD | CTLFLAG_MPSAFE,
+		&softc->errors, 0,
+		"Transport errors reported by the SIM.");
+	SYSCTL_ADD_INT(&softc->sysctl_stats_ctx,
+		SYSCTL_CHILDREN(softc->sysctl_stats_tree),
+		OID_AUTO, "pack_invalidations", CTLFLAG_RD | CTLFLAG_MPSAFE,
+		&softc->invalidations, 0,
+		"Device pack invalidations.");
+#endif
+
+	cam_iosched_sysctl_init(softc->cam_iosched, &softc->sysctl_ctx,
+	    softc->sysctl_tree);
+
 	cam_periph_release(periph);
 }
 
@@ -1148,6 +1294,43 @@ adagetattr(struct bio *bp)
 	return ret;
 }
 
+static int
+adadeletemethodsysctl(SYSCTL_HANDLER_ARGS)
+{
+	char buf[16];
+	const char *p;
+	struct ada_softc *softc;
+	int i, error, value, methods;
+
+	softc = (struct ada_softc *)arg1;
+
+	value = softc->delete_method;
+	if (value < 0 || value > ADA_DELETE_MAX)
+		p = "UNKNOWN";
+	else
+		p = ada_delete_method_names[value];
+	strncpy(buf, p, sizeof(buf));
+	error = sysctl_handle_string(oidp, buf, sizeof(buf), req);
+	if (error != 0 || req->newptr == NULL)
+		return (error);
+	methods = 1 << ADA_DELETE_DISABLE;
+	if ((softc->flags & ADA_FLAG_CAN_CFA) &&
+	    !(softc->flags & ADA_FLAG_CAN_48BIT))
+		methods |= 1 << ADA_DELETE_CFA_ERASE;
+	if (softc->flags & ADA_FLAG_CAN_TRIM)
+		methods |= 1 << ADA_DELETE_DSM_TRIM;
+	if (softc->flags & ADA_FLAG_CAN_NCQ_TRIM)
+		methods |= 1 << ADA_DELETE_NCQ_DSM_TRIM;
+	for (i = 0; i <= ADA_DELETE_MAX; i++) {
+		if (!(methods & (1 << i)) ||
+		    strcmp(buf, ada_delete_method_names[i]) != 0)
+			continue;
+		softc->delete_method = i;
+		return (0);
+	}
+	return (EINVAL);
+}
+
 static cam_status
 adaregister(struct cam_periph *periph, void *arg)
 {
@@ -1175,8 +1358,11 @@ adaregister(struct cam_periph *periph, void *arg)
 		return(CAM_REQ_CMP_ERR);
 	}
 
-	bioq_init(&softc->bio_queue);
-	bioq_init(&softc->trim_queue);
+	if (cam_iosched_init(&softc->cam_iosched, periph) != 0) {
+		printf("adaregister: Unable to probe new device. "
+		       "Unable to allocate iosched memory\n");
+		return(CAM_REQ_CMP_ERR);
+	}
 
 	if ((cgd->ident_data.capabilities1 & ATA_SUPPORT_DMA) &&
 	    (cgd->inq_flags & SID_DMA))
@@ -1206,6 +1392,8 @@ adaregister(struct cam_periph *periph, void *arg)
 	if (cgd->ident_data.support.command2 & ATA_SUPPORT_CFA)
 		softc->flags |= ADA_FLAG_CAN_CFA;
 
+	adasetdeletemethod(softc);
+
 	periph->softc = softc;
 
 	/*
@@ -1246,10 +1434,12 @@ adaregister(struct cam_periph *periph, void *arg)
 	    "kern.cam.ada.%d.write_cache", periph->unit_number);
 	TUNABLE_INT_FETCH(announce_buf, &softc->write_cache);
 	/* Disable queue sorting for non-rotational media by default. */
-	if (cgd->ident_data.media_rotation_rate == ATA_RATE_NON_ROTATING)
-		softc->sort_io_queue = 0;
-	else
-		softc->sort_io_queue = -1;
+	if (cgd->ident_data.media_rotation_rate == ATA_RATE_NON_ROTATING) {
+		softc->rotating = 0;
+	} else {
+		softc->rotating = 1;
+	}
+	cam_iosched_set_sort_queue(softc->cam_iosched,  softc->rotating ? -1 : 0);
 	adagetparams(periph, cgd);
 	softc->disk = disk_alloc();
 	softc->disk->d_rotation_rate = cgd->ident_data.media_rotation_rate;
@@ -1292,8 +1482,23 @@ adaregister(struct cam_periph *periph, void *arg)
 		softc->disk->d_delmaxsize = 256 * softc->params.secsize;
 	} else
 		softc->disk->d_delmaxsize = maxio;
-	if ((cpi.hba_misc & PIM_UNMAPPED) != 0)
+	if ((cpi.hba_misc & PIM_UNMAPPED) != 0) {
 		softc->disk->d_flags |= DISKFLAG_UNMAPPED_BIO;
+		softc->unmappedio = 1;
+	}
+	/*
+	 * If we can do RCVSND_FPDMA_QUEUED commands, we may be able to do
+	 * NCQ trims, if we support trims at all. We also need support from
+	 * the sim do do things properly. Perhaps we should look at log 13
+	 * dword 0 bit 0 and dword 1 bit 0 are set too...
+	 */
+	if (cpi.hba_misc & PIM_NCQ_KLUDGE)
+		softc->flags |= ADA_FLAG_PIM_CAN_NCQ_TRIM;
+	if ((softc->quirks & ADA_Q_NCQ_TRIM_BROKEN) == 0 &&
+	    (softc->flags & ADA_FLAG_PIM_CAN_NCQ_TRIM) != 0 &&
+	    (cgd->ident_data.satacapabilities2 & ATA_SUPPORT_RCVSND_FPDMA_QUEUED) != 0 &&
+	    (softc->flags & ADA_FLAG_CAN_TRIM) != 0)
+		softc->flags |= ADA_FLAG_CAN_NCQ_TRIM;
 	strlcpy(softc->disk->d_descr, cgd->ident_data.model,
 	    MIN(sizeof(softc->disk->d_descr), sizeof(cgd->ident_data.model)));
 	strlcpy(softc->disk->d_ident, cgd->ident_data.serial,
@@ -1320,6 +1525,7 @@ adaregister(struct cam_periph *periph, void *arg)
 	softc->disk->d_fwsectors = softc->params.secs_per_track;
 	softc->disk->d_fwheads = softc->params.heads;
 	ata_disk_firmware_geom_adjust(softc->disk);
+	adasetdeletemethod(softc);
 
 	/*
 	 * Acquire a reference to the periph before we register with GEOM.
@@ -1389,10 +1595,9 @@ adaregister(struct cam_periph *periph, void *arg)
 	return(CAM_REQ_CMP);
 }
 
-static void
-ada_dsmtrim(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
+static int
+ada_dsmtrim_req_create(struct ada_softc *softc, struct bio *bp, struct trim_request *req)
 {
-	struct trim_request *req = &softc->trim_req;
 	uint64_t lastlba = (uint64_t)-1;
 	int c, lastcount = 0, off, ranges = 0;
 
@@ -1402,8 +1607,6 @@ ada_dsmtrim(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
 		uint64_t lba = bp->bio_pblkno;
 		int count = bp->bio_bcount / softc->params.secsize;
 
-		bioq_remove(&softc->trim_queue, bp);
-
 		/* Try to extend the previous range. */
 		if (lba == lastlba) {
 			c = min(count, ATA_DSM_RANGE_MAX - lastcount);
@@ -1439,12 +1642,27 @@ ada_dsmtrim(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
 		}
 		lastlba = lba;
 		TAILQ_INSERT_TAIL(&req->bps, bp, bio_queue);
-		bp = bioq_first(&softc->trim_queue);
-		if (bp == NULL ||
-		    bp->bio_bcount / softc->params.secsize >
-		    (softc->trim_max_ranges - ranges) * ATA_DSM_RANGE_MAX)
+
+		bp = cam_iosched_next_trim(softc->cam_iosched);
+		if (bp == NULL)
 			break;
+		if (bp->bio_bcount / softc->params.secsize >
+		    (softc->trim_max_ranges - ranges) * ATA_DSM_RANGE_MAX) {
+			cam_iosched_put_back_trim(softc->cam_iosched, bp);
+			break;
+		}
 	} while (1);
+
+	return (ranges);
+}
+
+static void
+ada_dsmtrim(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
+{
+	struct trim_request *req = &softc->trim_req;
+	int ranges;
+
+	ranges = ada_dsmtrim_req_create(softc, bp, req);
 	cam_fill_ataio(ataio,
 	    ada_retry_count,
 	    adadone,
@@ -1460,6 +1678,30 @@ ada_dsmtrim(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
 }
 
 static void
+ada_ncq_dsmtrim(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
+{
+	struct trim_request *req = &softc->trim_req;
+	int ranges;
+
+	ranges = ada_dsmtrim_req_create(softc, bp, req);
+	cam_fill_ataio(ataio,
+	    ada_retry_count,
+	    adadone,
+	    CAM_DIR_OUT,
+	    0,
+	    req->data,
+	    ((ranges + ATA_DSM_BLK_RANGES - 1) /
+	    ATA_DSM_BLK_RANGES) * ATA_DSM_BLK_SIZE,
+	    ada_default_timeout * 1000);
+	ata_ncq_cmd(ataio,
+	    ATA_SEND_FPDMA_QUEUED,
+	    0,
+	    (ranges + ATA_DSM_BLK_RANGES - 1) / ATA_DSM_BLK_RANGES);
+	ataio->cmd.sector_count_exp = ATA_SFPDMA_DSM;
+	ataio->cmd.flags |= CAM_ATAIO_AUX_HACK;
+}
+
+static void
 ada_cfaerase(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
 {
 	struct trim_request *req = &softc->trim_req;
@@ -1468,7 +1710,6 @@ ada_cfaerase(struct ada_softc *softc, struct bio *bp, struct ccb_ataio *ataio)
 
 	bzero(req, sizeof(*req));
 	TAILQ_INIT(&req->bps);
-	bioq_remove(&softc->trim_queue, bp);
 	TAILQ_INSERT_TAIL(&req->bps, bp, bio_queue);
 
 	cam_fill_ataio(ataio,
@@ -1499,37 +1740,14 @@ adastart(struct cam_periph *periph, union ccb *start_ccb)
 		struct bio *bp;
 		u_int8_t tag_code;
 
-		/* Run TRIM if not running yet. */
-		if (!softc->trim_running &&
-		    (bp = bioq_first(&softc->trim_queue)) != 0) {
-			if (softc->flags & ADA_FLAG_CAN_TRIM) {
-				ada_dsmtrim(softc, bp, ataio);
-			} else if ((softc->flags & ADA_FLAG_CAN_CFA) &&
-			    !(softc->flags & ADA_FLAG_CAN_48BIT)) {
-				ada_cfaerase(softc, bp, ataio);
-			} else {
-				/* This can happen if DMA was disabled. */
-				bioq_remove(&softc->trim_queue, bp);
-				biofinish(bp, NULL, EOPNOTSUPP);
-				xpt_release_ccb(start_ccb);
-				adaschedule(periph);
-				return;
-			}
-			softc->trim_running = 1;
-			start_ccb->ccb_h.ccb_state = ADA_CCB_TRIM;
-			start_ccb->ccb_h.flags |= CAM_UNLOCKED;
-			goto out;
-		}
-		/* Run regular command. */
-		bp = bioq_first(&softc->bio_queue);
+		bp = cam_iosched_next_bio(softc->cam_iosched);
 		if (bp == NULL) {
 			xpt_release_ccb(start_ccb);
 			break;
 		}
-		bioq_remove(&softc->bio_queue, bp);
 
-		if ((bp->bio_flags & BIO_ORDERED) != 0
-		 || (softc->flags & ADA_FLAG_NEED_OTAG) != 0) {
+		if ((bp->bio_flags & BIO_ORDERED) != 0 ||
+		    (bp->bio_cmd != BIO_DELETE && (softc->flags & ADA_FLAG_NEED_OTAG) != 0)) {
 			softc->flags &= ~ADA_FLAG_NEED_OTAG;
 			softc->flags |= ADA_FLAG_WAS_OTAG;
 			tag_code = 0;
@@ -1659,6 +1877,27 @@ adastart(struct cam_periph *periph, union ccb *start_ccb)
 			}
 			break;
 		}
+		case BIO_DELETE:
+			switch (softc->delete_method) {
+			case ADA_DELETE_NCQ_DSM_TRIM:
+				ada_ncq_dsmtrim(softc, bp, ataio);
+				break;
+			case ADA_DELETE_DSM_TRIM:
+				ada_dsmtrim(softc, bp, ataio);
+				break;
+			case ADA_DELETE_CFA_ERASE:
+				ada_cfaerase(softc, bp, ataio);
+				break;
+			default:
+				biofinish(bp, NULL, EOPNOTSUPP);
+				xpt_release_ccb(start_ccb);
+				adaschedule(periph);
+				return;
+			}
+			start_ccb->ccb_h.ccb_state = ADA_CCB_TRIM;
+			start_ccb->ccb_h.flags |= CAM_UNLOCKED;
+			cam_iosched_submit_trim(softc->cam_iosched);
+			goto out;
 		case BIO_FLUSH:
 			cam_fill_ataio(ataio,
 			    1,
@@ -1742,6 +1981,7 @@ adadone(struct cam_periph *periph, union ccb *done_ccb)
 		int error;
 
 		cam_periph_lock(periph);
+		bp = (struct bio *)done_ccb->ccb_h.ccb_bp;
 		if ((done_ccb->ccb_h.status & CAM_STATUS_MASK) != CAM_REQ_CMP) {
 			error = adaerror(done_ccb, 0, 0);
 			if (error == ERESTART) {
@@ -1755,12 +1995,25 @@ adadone(struct cam_periph *periph, union ccb *done_ccb)
 						 /*reduction*/0,
 						 /*timeout*/0,
 						 /*getcount_only*/0);
+			/*
+			 * If we get an error on an NCQ DSM TRIM, fall back
+			 * to a non-NCQ DSM TRIM forever. Please note that if
+			 * CAN_NCQ_TRIM is set, CAN_TRIM is necessarily set too.
+			 * However, for this one trim, we treat it as advisory
+			 * and return success up the stack.
+			 */
+			if (state == ADA_CCB_TRIM &&
+			    error != 0 &&
+			    (softc->flags & ADA_FLAG_CAN_NCQ_TRIM) != 0) {
+				softc->flags &= ~ADA_FLAG_CAN_NCQ_TRIM;
+				error = 0;
+				adasetdeletemethod(softc);
+			}
 		} else {
 			if ((done_ccb->ccb_h.status & CAM_DEV_QFRZN) != 0)
 				panic("REQ_CMP with QFRZN");
 			error = 0;
 		}
-		bp = (struct bio *)done_ccb->ccb_h.ccb_bp;
 		bp->bio_error = error;
 		if (error != 0) {
 			bp->bio_resid = bp->bio_bcount;
@@ -1776,6 +2029,8 @@ adadone(struct cam_periph *periph, union ccb *done_ccb)
 		softc->outstanding_cmds--;
 		if (softc->outstanding_cmds == 0)
 			softc->flags |= ADA_FLAG_WAS_OTAG;
+
+		cam_iosched_bio_complete(softc->cam_iosched, bp, done_ccb);
 		xpt_release_ccb(done_ccb);
 		if (state == ADA_CCB_TRIM) {
 			TAILQ_HEAD(, bio) queue;
@@ -1793,7 +2048,7 @@ adadone(struct cam_periph *periph, union ccb *done_ccb)
 			 * daschedule again so that we don't stall if there are
 			 * no other I/Os pending apart from BIO_DELETEs.
 			 */
-			softc->trim_running = 0;
+			cam_iosched_trim_done(softc->cam_iosched);
 			adaschedule(periph);
 			cam_periph_unlock(periph);
 			while ((bp1 = TAILQ_FIRST(&queue)) != NULL) {
@@ -1807,6 +2062,7 @@ adadone(struct cam_periph *periph, union ccb *done_ccb)
 				biodone(bp1);
 			}
 		} else {
+			adaschedule(periph);
 			cam_periph_unlock(periph);
 			biodone(bp);
 		}
@@ -1898,6 +2154,31 @@ out:
 static int
 adaerror(union ccb *ccb, u_int32_t cam_flags, u_int32_t sense_flags)
 {
+	struct ada_softc *softc;
+	struct cam_periph *periph;
+
+	periph = xpt_path_periph(ccb->ccb_h.path);
+	softc = (struct ada_softc *)periph->softc;
+
+	switch (ccb->ccb_h.status & CAM_STATUS_MASK) {
+	case CAM_CMD_TIMEOUT:
+#ifdef CAM_IO_STATS
+		softc->timeouts++;
+#endif
+		break;
+	case CAM_REQ_ABORTED:
+	case CAM_REQ_CMP_ERR:
+	case CAM_REQ_TERMIO:
+	case CAM_UNREC_HBA_ERROR:
+	case CAM_DATA_RUN_ERR:
+	case CAM_ATA_STATUS_ERROR:
+#ifdef CAM_IO_STATS
+		softc->errors++;
+#endif
+		break;
+	default:
+		break;
+	}
 
 	return(cam_periph_error(ccb, cam_flags, sense_flags, NULL));
 }
diff --git a/sys/cam/cam_ccb.h b/sys/cam/cam_ccb.h
index 28415ed..c739aa6 100644
--- a/sys/cam/cam_ccb.h
+++ b/sys/cam/cam_ccb.h
@@ -581,6 +581,7 @@ typedef enum {
 } pi_tmflag;
 
 typedef enum {
+	PIM_NCQ_KLUDGE  = 0x200, /* Supports the sata ncq trim kludge */
 	PIM_EXTLUNS	= 0x100,/* 64bit extended LUNs supported */
 	PIM_SCANHILO	= 0x80,	/* Bus scans from high ID to low ID */
 	PIM_NOREMOVE	= 0x40,	/* Removeable devices not included in scan */
diff --git a/sys/cam/cam_xpt.c b/sys/cam/cam_xpt.c
index e811fe6..b426857 100644
--- a/sys/cam/cam_xpt.c
+++ b/sys/cam/cam_xpt.c
@@ -3311,6 +3311,7 @@ xpt_run_devq(struct cam_devq *devq)
 		lock = (mtx_owned(sim->mtx) == 0);
 		if (lock)
 			CAM_SIM_LOCK(sim);
+		work_ccb->ccb_h.qos.sim_data = sbinuptime(); // xxx uintprt_t too small 32bit platforms
 		(*(sim->sim_action))(sim, work_ccb);
 		if (lock)
 			CAM_SIM_UNLOCK(sim);
@@ -4439,6 +4440,8 @@ xpt_done(union ccb *done_ccb)
 	if ((done_ccb->ccb_h.func_code & XPT_FC_QUEUED) == 0)
 		return;
 
+	/* Store the time the ccb was in the sim */
+	done_ccb->ccb_h.qos.sim_data = sbinuptime() - done_ccb->ccb_h.qos.sim_data;
 	hash = (done_ccb->ccb_h.path_id + done_ccb->ccb_h.target_id +
 	    done_ccb->ccb_h.target_lun) % cam_num_doneqs;
 	queue = &cam_doneqs[hash];
@@ -4459,6 +4462,8 @@ xpt_done_direct(union ccb *done_ccb)
 	if ((done_ccb->ccb_h.func_code & XPT_FC_QUEUED) == 0)
 		return;
 
+	/* Store the time the ccb was in the sim */
+	done_ccb->ccb_h.qos.sim_data = sbinuptime() - done_ccb->ccb_h.qos.sim_data;
 	xpt_done_process(&done_ccb->ccb_h);
 }
 
diff --git a/sys/cam/scsi/scsi_da.c b/sys/cam/scsi/scsi_da.c
index 07a6435..907203a 100644
--- a/sys/cam/scsi/scsi_da.c
+++ b/sys/cam/scsi/scsi_da.c
@@ -60,6 +60,7 @@ __FBSDID("$FreeBSD$");
 #include <cam/cam_periph.h>
 #include <cam/cam_xpt_periph.h>
 #include <cam/cam_sim.h>
+#include <cam/cam_iosched.h>
 
 #include <cam/scsi/scsi_message.h>
 
@@ -199,21 +200,19 @@ struct disk_params {
 #define ATA_TRIM_MAX_RANGES	((UNMAP_BUF_SIZE / \
 	(ATA_DSM_RANGE_SIZE * ATA_DSM_BLK_SIZE)) * ATA_DSM_BLK_SIZE)
 
+#define DA_WORK_TUR		(1 << 16)
+
 struct da_softc {
-	struct	 bio_queue_head bio_queue;
-	struct	 bio_queue_head delete_queue;
+	struct   cam_iosched_softc *cam_iosched;
 	struct	 bio_queue_head delete_run_queue;
 	LIST_HEAD(, ccb_hdr) pending_ccbs;
-	int	 tur;			/* TEST UNIT READY should be sent */
 	int	 refcount;		/* Active xpt_action() calls */
 	da_state state;
 	da_flags flags;	
 	da_quirks quirks;
-	int	 sort_io_queue;
 	int	 minimum_cmd_size;
 	int	 error_inject;
 	int	 trim_max_ranges;
-	int	 delete_running;
 	int	 delete_available;	/* Delete methods possibly available */
 	u_int	 maxio;
 	uint32_t		unmap_max_ranges;
@@ -222,6 +221,8 @@ struct da_softc {
 	da_delete_methods	delete_method_pref;
 	da_delete_methods	delete_method;
 	da_delete_func_t	*delete_func;
+	int			unmappedio;
+	int			rotating;
 	struct	 disk_params params;
 	struct	 disk *disk;
 	union	 ccb saved_ccb;
@@ -233,6 +234,13 @@ struct da_softc {
 	uint8_t	 unmap_buf[UNMAP_BUF_SIZE];
 	struct scsi_read_capacity_data_long rcaplong;
 	struct callout		mediapoll_c;
+#ifdef CAM_IO_STATS
+	struct sysctl_ctx_list	sysctl_stats_ctx;
+	struct sysctl_oid	*sysctl_stats_tree;
+	u_int	errors;
+	u_int	timeouts;
+	u_int	invalidations;
+#endif
 };
 
 #define dadeleteflag(softc, delete_method, enable)			\
@@ -1193,6 +1201,7 @@ static	periph_init_t	dainit;
 static	void		daasync(void *callback_arg, u_int32_t code,
 				struct cam_path *path, void *arg);
 static	void		dasysctlinit(void *context, int pending);
+static	int		dasysctlsofttimeout(SYSCTL_HANDLER_ARGS);
 static	int		dacmdsizesysctl(SYSCTL_HANDLER_ARGS);
 static	int		dadeletemethodsysctl(SYSCTL_HANDLER_ARGS);
 static	int		dadeletemaxsysctl(SYSCTL_HANDLER_ARGS);
@@ -1230,6 +1239,10 @@ static timeout_t	damediapoll;
 #define DA_DEFAULT_TIMEOUT 60	/* Timeout in seconds */
 #endif
 
+#ifndef DA_DEFAULT_SOFTTIMEOUT
+#define DA_DEFAULT_SOFTTIMEOUT	0
+#endif
+
 #ifndef	DA_DEFAULT_RETRY
 #define	DA_DEFAULT_RETRY	4
 #endif
@@ -1238,12 +1251,10 @@ static timeout_t	damediapoll;
 #define	DA_DEFAULT_SEND_ORDERED	1
 #endif
 
-#define DA_SIO (softc->sort_io_queue >= 0 ? \
-    softc->sort_io_queue : cam_sort_io_queues)
-
 static int da_poll_period = DA_DEFAULT_POLL_PERIOD;
 static int da_retry_count = DA_DEFAULT_RETRY;
 static int da_default_timeout = DA_DEFAULT_TIMEOUT;
+static sbintime_t da_default_softtimeout = DA_DEFAULT_SOFTTIMEOUT;
 static int da_send_ordered = DA_DEFAULT_SEND_ORDERED;
 
 static SYSCTL_NODE(_kern_cam, OID_AUTO, da, CTLFLAG_RD, 0,
@@ -1257,6 +1268,11 @@ SYSCTL_INT(_kern_cam_da, OID_AUTO, default_timeout, CTLFLAG_RWTUN,
 SYSCTL_INT(_kern_cam_da, OID_AUTO, send_ordered, CTLFLAG_RWTUN,
            &da_send_ordered, 0, "Send Ordered Tags");
 
+SYSCTL_PROC(_kern_cam_da, OID_AUTO, default_softtimeout,
+    CTLTYPE_UINT | CTLFLAG_RW, NULL, 0, dasysctlsofttimeout, "I",
+    "Soft I/O timeout (ms)");
+TUNABLE_LONG("kern.cam.da.default_softtimeout", &da_default_softtimeout);
+
 /*
  * DA_ORDEREDTAG_INTERVAL determines how often, relative
  * to the default timeout, we check to see whether an ordered
@@ -1400,12 +1416,7 @@ daschedule(struct cam_periph *periph)
 	if (softc->state != DA_STATE_NORMAL)
 		return;
 
-	/* Check if we have more work to do. */
-	if (bioq_first(&softc->bio_queue) ||
-	    (!softc->delete_running && bioq_first(&softc->delete_queue)) ||
-	    softc->tur) {
-		xpt_schedule(periph, CAM_PRIORITY_NORMAL);
-	}
+	cam_iosched_schedule(softc->cam_iosched, periph);
 }
 
 /*
@@ -1438,13 +1449,7 @@ dastrategy(struct bio *bp)
 	/*
 	 * Place it in the queue of disk activities for this disk
 	 */
-	if (bp->bio_cmd == BIO_DELETE) {
-		bioq_disksort(&softc->delete_queue, bp);
-	} else if (DA_SIO) {
-		bioq_disksort(&softc->bio_queue, bp);
-	} else {
-		bioq_insert_tail(&softc->bio_queue, bp);
-	}
+	cam_iosched_queue_work(softc->cam_iosched, bp);
 
 	/*
 	 * Schedule ourselves for performing the work.
@@ -1519,7 +1524,7 @@ dadump(void *arg, void *virtual, vm_offset_t physical, off_t offset, size_t leng
 				       /*begin_lba*/0,/* Cover the whole disk */
 				       /*lb_count*/0,
 				       SSD_FULL_SIZE,
-				       5 * 60 * 1000);
+				       5 * 1000);
 		xpt_polled_action((union ccb *)&csio);
 
 		error = cam_periph_error((union ccb *)&csio,
@@ -1599,14 +1604,16 @@ daoninvalidate(struct cam_periph *periph)
 	xpt_register_async(0, daasync, periph, periph->path);
 
 	softc->flags |= DA_FLAG_PACK_INVALID;
+#ifdef CAM_IO_STATS
+	softc->invalidations++;
+#endif
 
 	/*
 	 * Return all queued I/O with ENXIO.
 	 * XXX Handle any transactions queued to the card
 	 *     with XPT_ABORT_CCB.
 	 */
-	bioq_flush(&softc->bio_queue, NULL, ENXIO);
-	bioq_flush(&softc->delete_queue, NULL, ENXIO);
+	cam_iosched_flush(softc->cam_iosched, NULL, ENXIO);
 
 	/*
 	 * Tell GEOM that we've gone away, we'll get a callback when it is
@@ -1624,12 +1631,20 @@ dacleanup(struct cam_periph *periph)
 
 	cam_periph_unlock(periph);
 
+	cam_iosched_fini(softc->cam_iosched);
+
 	/*
 	 * If we can't free the sysctl tree, oh well...
 	 */
-	if ((softc->flags & DA_FLAG_SCTX_INIT) != 0
-	    && sysctl_ctx_free(&softc->sysctl_ctx) != 0) {
-		xpt_print(periph->path, "can't remove sysctl context\n");
+	if ((softc->flags & DA_FLAG_SCTX_INIT) != 0) {
+#ifdef CAM_IO_STATS
+		if (sysctl_ctx_free(&softc->sysctl_stats_ctx) != 0)
+			xpt_print(periph->path,
+			    "can't remove sysctl stats context\n");
+#endif
+		if (sysctl_ctx_free(&softc->sysctl_ctx) != 0)
+			xpt_print(periph->path,
+			    "can't remove sysctl context\n");
 	}
 
 	callout_drain(&softc->mediapoll_c);
@@ -1732,9 +1747,9 @@ daasync(void *callback_arg, u_int32_t code,
 	}
 	case AC_SCSI_AEN:
 		softc = (struct da_softc *)periph->softc;
-		if (!softc->tur) {
+		if (!cam_iosched_has_work_flags(softc->cam_iosched, DA_WORK_TUR)) {
 			if (cam_periph_acquire(periph) == CAM_REQ_CMP) {
-				softc->tur = 1;
+				cam_iosched_set_work_flags(softc->cam_iosched, DA_WORK_TUR);
 				daschedule(periph);
 			}
 		}
@@ -1808,9 +1823,6 @@ dasysctlinit(void *context, int pending)
 		OID_AUTO, "minimum_cmd_size", CTLTYPE_INT | CTLFLAG_RW,
 		&softc->minimum_cmd_size, 0, dacmdsizesysctl, "I",
 		"Minimum CDB size");
-	SYSCTL_ADD_INT(&softc->sysctl_ctx, SYSCTL_CHILDREN(softc->sysctl_tree),
-		OID_AUTO, "sort_io_queue", CTLFLAG_RW, &softc->sort_io_queue, 0,
-		"Sort IO queue to try and optimise disk access patterns");
 
 	SYSCTL_ADD_INT(&softc->sysctl_ctx,
 		       SYSCTL_CHILDREN(softc->sysctl_tree),
@@ -1821,6 +1833,23 @@ dasysctlinit(void *context, int pending)
 		       0,
 		       "error_inject leaf");
 
+	SYSCTL_ADD_INT(&softc->sysctl_ctx,
+		       SYSCTL_CHILDREN(softc->sysctl_tree),
+		       OID_AUTO,
+		       "unmapped_io",
+		       CTLFLAG_RD, 
+		       &softc->unmappedio,
+		       0,
+		       "Unmapped I/O leaf");
+
+	SYSCTL_ADD_INT(&softc->sysctl_ctx,
+		       SYSCTL_CHILDREN(softc->sysctl_tree),
+		       OID_AUTO,
+		       "rotating",
+		       CTLFLAG_RD, 
+		       &softc->rotating,
+		       0,
+		       "Rotating media");
 
 	/*
 	 * Add some addressing info.
@@ -1846,6 +1875,44 @@ dasysctlinit(void *context, int pending)
 			    &softc->wwpn, "World Wide Port Name");
 		}
 	}
+
+#ifdef CAM_IO_STATS
+	/*
+	 * Now add some useful stats.
+	 * XXX These should live in cam_periph and be common to all periphs
+	 */
+	softc->sysctl_stats_tree = SYSCTL_ADD_NODE(&softc->sysctl_stats_ctx,
+	    SYSCTL_CHILDREN(softc->sysctl_tree), OID_AUTO, "stats",
+	    CTLFLAG_RD, 0, "Statistics");
+	SYSCTL_ADD_INT(&softc->sysctl_stats_ctx,
+		       SYSCTL_CHILDREN(softc->sysctl_stats_tree),
+		       OID_AUTO,
+		       "errors",
+		       CTLFLAG_RD,
+		       &softc->errors,
+		       0,
+		       "Transport errors reported by the SIM");
+	SYSCTL_ADD_INT(&softc->sysctl_stats_ctx,
+		       SYSCTL_CHILDREN(softc->sysctl_stats_tree),
+		       OID_AUTO,
+		       "timeouts",
+		       CTLFLAG_RD,
+		       &softc->timeouts,
+		       0,
+		       "Device timeouts reported by the SIM");
+	SYSCTL_ADD_INT(&softc->sysctl_stats_ctx,
+		       SYSCTL_CHILDREN(softc->sysctl_stats_tree),
+		       OID_AUTO,
+		       "pack_invalidations",
+		       CTLFLAG_RD,
+		       &softc->invalidations,
+		       0,
+		       "Device pack invalidations");
+#endif
+
+	cam_iosched_sysctl_init(softc->cam_iosched, &softc->sysctl_ctx,
+	    softc->sysctl_tree);
+
 	cam_periph_release(periph);
 }
 
@@ -1904,6 +1971,26 @@ dacmdsizesysctl(SYSCTL_HANDLER_ARGS)
 	return (0);
 }
 
+static int
+dasysctlsofttimeout(SYSCTL_HANDLER_ARGS)
+{
+	sbintime_t value;
+	int error;
+
+	value = da_default_softtimeout / SBT_1MS;
+
+	error = sysctl_handle_int(oidp, (int *)&value, 0, req);
+	if ((error != 0) || (req->newptr == NULL))
+		return (error);
+
+	/* XXX Should clip this to a reasonable level */
+	if (value > da_default_timeout * 1000)
+		return (EINVAL);
+
+	da_default_softtimeout = value * SBT_1MS;
+	return (0);
+}
+
 static void
 dadeletemethodset(struct da_softc *softc, da_delete_methods delete_method)
 {
@@ -2075,14 +2162,18 @@ daregister(struct cam_periph *periph, void *arg)
 
 	if (softc == NULL) {
 		printf("daregister: Unable to probe new device. "
-		       "Unable to allocate softc\n");				
+		       "Unable to allocate softc\n");
 		return(CAM_REQ_CMP_ERR);
 	}
 
+	if (cam_iosched_init(&softc->cam_iosched, periph) != 0) {
+		printf("daregister: Unable to probe new device. "
+		       "Unable to allocate iosched memory\n");
+		return(CAM_REQ_CMP_ERR);
+	}
+	
 	LIST_INIT(&softc->pending_ccbs);
 	softc->state = DA_STATE_PROBE_RC;
-	bioq_init(&softc->bio_queue);
-	bioq_init(&softc->delete_queue);
 	bioq_init(&softc->delete_run_queue);
 	if (SID_IS_REMOVABLE(&cgd->inq_data))
 		softc->flags |= DA_FLAG_PACK_REMOVABLE;
@@ -2090,7 +2181,7 @@ daregister(struct cam_periph *periph, void *arg)
 	softc->unmap_max_lba = UNMAP_RANGE_MAX;
 	softc->ws_max_blks = WS16_MAX_BLKS;
 	softc->trim_max_ranges = ATA_TRIM_MAX_RANGES;
-	softc->sort_io_queue = -1;
+	softc->rotating = 1;
 
 	periph->softc = softc;
 
@@ -2199,8 +2290,11 @@ daregister(struct cam_periph *periph, void *arg)
 	softc->disk->d_flags = DISKFLAG_DIRECT_COMPLETION;
 	if ((softc->quirks & DA_Q_NO_SYNC_CACHE) == 0)
 		softc->disk->d_flags |= DISKFLAG_CANFLUSHCACHE;
-	if ((cpi.hba_misc & PIM_UNMAPPED) != 0)
+	if ((cpi.hba_misc & PIM_UNMAPPED) != 0) {
+		softc->unmappedio = 1;
 		softc->disk->d_flags |= DISKFLAG_UNMAPPED_BIO;
+		xpt_print(periph->path, "UNMAPPED\n");
+	}
 	cam_strvis(softc->disk->d_descr, cgd->inq_data.vendor,
 	    sizeof(cgd->inq_data.vendor), sizeof(softc->disk->d_descr));
 	strlcat(softc->disk->d_descr, " ", sizeof(softc->disk->d_descr));
@@ -2277,23 +2371,11 @@ skipstate:
 		struct bio *bp;
 		uint8_t tag_code;
 
-		/* Run BIO_DELETE if not running yet. */
-		if (!softc->delete_running &&
-		    (bp = bioq_first(&softc->delete_queue)) != NULL) {
-			if (softc->delete_func != NULL) {
-				softc->delete_func(periph, start_ccb, bp);
-				goto out;
-			} else {
-				bioq_flush(&softc->delete_queue, NULL, 0);
-				/* FALLTHROUGH */
-			}
-		}
-
-		/* Run regular command. */
-		bp = bioq_takefirst(&softc->bio_queue);
+more:
+		bp = cam_iosched_next_bio(softc->cam_iosched);
 		if (bp == NULL) {
-			if (softc->tur) {
-				softc->tur = 0;
+			if (cam_iosched_has_work_flags(softc->cam_iosched, DA_WORK_TUR)) {
+				cam_iosched_clr_work_flags(softc->cam_iosched, DA_WORK_TUR);
 				scsi_test_unit_ready(&start_ccb->csio,
 				     /*retries*/ da_retry_count,
 				     dadone,
@@ -2307,9 +2389,21 @@ skipstate:
 				xpt_release_ccb(start_ccb);
 			break;
 		}
-		if (softc->tur) {
-			softc->tur = 0;
-			cam_periph_release_locked(periph);
+
+		if (bp->bio_cmd == BIO_DELETE) {
+			if (softc->delete_func != NULL) {
+				softc->delete_func(periph, start_ccb, bp);
+				goto out;
+			} else {
+				/* Not sure this is possible, but failsafe by lying and saying "sure, done." */
+				biofinish(bp, NULL, 0);
+				goto more;
+			}
+		}
+
+		if (cam_iosched_has_work_flags(softc->cam_iosched, DA_WORK_TUR)) {
+			cam_iosched_clr_work_flags(softc->cam_iosched, DA_WORK_TUR);
+			cam_periph_release_locked(periph);	/* XXX is this still valid? I think so but unverified */
 		}
 
 		if ((bp->bio_flags & BIO_ORDERED) != 0 ||
@@ -2377,6 +2471,7 @@ skipstate:
 		}
 		start_ccb->ccb_h.ccb_state = DA_CCB_BUFFER_IO;
 		start_ccb->ccb_h.flags |= CAM_UNLOCKED;
+		start_ccb->ccb_h.softtimeout = sbttotv(da_default_softtimeout);
 
 out:
 		LIST_INSERT_HEAD(&softc->pending_ccbs,
@@ -2625,11 +2720,19 @@ da_delete_unmap(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 	 * fewer LBA's than requested.
 	 */
 
-	softc->delete_running = 1;
 	bzero(softc->unmap_buf, sizeof(softc->unmap_buf));
 	bp1 = bp;
 	do {
-		bioq_remove(&softc->delete_queue, bp1);
+		/*
+		 * Note: ada and da are different in how they store the
+		 * pending bp's in a trim. ada stores all of them in the
+		 * trim_req.bps. da stores all but the first one in the
+		 * delete_run_queue. ada then completes all the bps in
+		 * its adadone() loop. da completes all the bps in the
+		 * delete_run_queue in dadone, and relies on the biodone
+		 * after to complete. This should be reconciled since there's
+		 * no real reason to do it differently. XXX
+		 */
 		if (bp1 != bp)
 			bioq_insert_tail(&softc->delete_run_queue, bp1);
 		lba = bp1->bio_pblkno;
@@ -2669,11 +2772,15 @@ da_delete_unmap(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 			lastcount = c;
 		}
 		lastlba = lba;
-		bp1 = bioq_first(&softc->delete_queue);
-		if (bp1 == NULL || ranges >= softc->unmap_max_ranges ||
+		bp1 = cam_iosched_next_trim(softc->cam_iosched);
+		if (bp1 == NULL)
+			break;
+		if (ranges >= softc->unmap_max_ranges ||
 		    totalcount + bp1->bio_bcount /
-		    softc->params.secsize > softc->unmap_max_lba)
+		    softc->params.secsize > softc->unmap_max_lba) {
+			cam_iosched_put_back_trim(softc->cam_iosched, bp1);
 			break;
+		}
 	} while (1);
 	scsi_ulto2b(ranges * 16 + 6, &buf[0]);
 	scsi_ulto2b(ranges * 16, &buf[2]);
@@ -2689,6 +2796,7 @@ da_delete_unmap(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 		   da_default_timeout * 1000);
 	ccb->ccb_h.ccb_state = DA_CCB_DELETE;
 	ccb->ccb_h.flags |= CAM_UNLOCKED;
+	cam_iosched_submit_trim(softc->cam_iosched);
 }
 
 static void
@@ -2703,12 +2811,10 @@ da_delete_trim(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 	uint32_t lastcount = 0, c, requestcount;
 	int ranges = 0, off, block_count;
 
-	softc->delete_running = 1;
 	bzero(softc->unmap_buf, sizeof(softc->unmap_buf));
 	bp1 = bp;
 	do {
-		bioq_remove(&softc->delete_queue, bp1);
-		if (bp1 != bp)
+		if (bp1 != bp)//XXX imp XXX
 			bioq_insert_tail(&softc->delete_run_queue, bp1);
 		lba = bp1->bio_pblkno;
 		count = bp1->bio_bcount / softc->params.secsize;
@@ -2752,10 +2858,14 @@ da_delete_trim(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 			}
 		}
 		lastlba = lba;
-		bp1 = bioq_first(&softc->delete_queue);
-		if (bp1 == NULL || bp1->bio_bcount / softc->params.secsize >
-		    (softc->trim_max_ranges - ranges) * ATA_DSM_RANGE_MAX)
+		bp1 = cam_iosched_next_trim(softc->cam_iosched);
+		if (bp1 == NULL)
+			break;
+		if (bp1->bio_bcount / softc->params.secsize >
+		    (softc->trim_max_ranges - ranges) * ATA_DSM_RANGE_MAX) {
+			cam_iosched_put_back_trim(softc->cam_iosched, bp1);
 			break;
+		}
 	} while (1);
 
 	block_count = (ranges + ATA_DSM_BLK_RANGES - 1) / ATA_DSM_BLK_RANGES;
@@ -2770,6 +2880,7 @@ da_delete_trim(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 		      da_default_timeout * 1000);
 	ccb->ccb_h.ccb_state = DA_CCB_DELETE;
 	ccb->ccb_h.flags |= CAM_UNLOCKED;
+	cam_iosched_submit_trim(softc->cam_iosched);
 }
 
 /*
@@ -2788,13 +2899,11 @@ da_delete_ws(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 
 	softc = (struct da_softc *)periph->softc;
 	ws_max_blks = softc->disk->d_delmaxsize / softc->params.secsize;
-	softc->delete_running = 1;
 	lba = bp->bio_pblkno;
 	count = 0;
 	bp1 = bp;
 	do {
-		bioq_remove(&softc->delete_queue, bp1);
-		if (bp1 != bp)
+		if (bp1 != bp)//XXX imp XXX
 			bioq_insert_tail(&softc->delete_run_queue, bp1);
 		count += bp1->bio_bcount / softc->params.secsize;
 		if (count > ws_max_blks) {
@@ -2805,11 +2914,15 @@ da_delete_ws(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 			count = omin(count, ws_max_blks);
 			break;
 		}
-		bp1 = bioq_first(&softc->delete_queue);
-		if (bp1 == NULL || lba + count != bp1->bio_pblkno ||
+		bp1 = cam_iosched_next_trim(softc->cam_iosched);
+		if (bp1 == NULL)
+			break;
+		if (lba + count != bp1->bio_pblkno ||
 		    count + bp1->bio_bcount /
-		    softc->params.secsize > ws_max_blks)
+		    softc->params.secsize > ws_max_blks) {
+			cam_iosched_put_back_trim(softc->cam_iosched, bp1);
 			break;
+		}
 	} while (1);
 
 	scsi_write_same(&ccb->csio,
@@ -2827,6 +2940,7 @@ da_delete_ws(struct cam_periph *periph, union ccb *ccb, struct bio *bp)
 			da_default_timeout * 1000);
 	ccb->ccb_h.ccb_state = DA_CCB_DELETE;
 	ccb->ccb_h.flags |= CAM_UNLOCKED;
+	cam_iosched_submit_trim(softc->cam_iosched);
 }
 
 static int
@@ -2870,8 +2984,8 @@ cmd6workaround(union ccb *ccb)
 				  da_delete_method_desc[softc->delete_method]);
 
 		while ((bp = bioq_takefirst(&softc->delete_run_queue)) != NULL)
-			bioq_disksort(&softc->delete_queue, bp);
-		bioq_disksort(&softc->delete_queue,
+			cam_iosched_queue_work(softc->cam_iosched, bp);
+		cam_iosched_queue_work(softc->cam_iosched,
 		    (struct bio *)ccb->ccb_h.ccb_bp);
 		ccb->ccb_h.ccb_bp = NULL;
 		return (0);
@@ -2998,9 +3112,12 @@ dadone(struct cam_periph *periph, union ccb *done_ccb)
 					xpt_print(periph->path,
 					    "Invalidating pack\n");
 					softc->flags |= DA_FLAG_PACK_INVALID;
+#ifdef CAM_IO_STATS
+					softc->invalidations++;
+#endif
 					queued_error = ENXIO;
 				}
-				bioq_flush(&softc->bio_queue, NULL,
+				cam_iosched_flush(softc->cam_iosched, NULL,
 					   queued_error);
 				if (bp != NULL) {
 					bp->bio_error = error;
@@ -3043,6 +3160,7 @@ dadone(struct cam_periph *periph, union ccb *done_ccb)
 		if (LIST_EMPTY(&softc->pending_ccbs))
 			softc->flags |= DA_FLAG_WAS_OTAG;
 
+		cam_iosched_bio_complete(softc->cam_iosched, bp, done_ccb);
 		xpt_release_ccb(done_ccb);
 		if (state == DA_CCB_DELETE) {
 			TAILQ_HEAD(, bio) queue;
@@ -3060,7 +3178,7 @@ dadone(struct cam_periph *periph, union ccb *done_ccb)
 			 * and call daschedule again so that we don't stall if
 			 * there are no other I/Os pending apart from BIO_DELETEs.
 			 */
-			softc->delete_running = 0;
+			cam_iosched_trim_done(softc->cam_iosched);
 			daschedule(periph);
 			cam_periph_unlock(periph);
 			while ((bp1 = TAILQ_FIRST(&queue)) != NULL) {
@@ -3073,8 +3191,10 @@ dadone(struct cam_periph *periph, union ccb *done_ccb)
 					bp1->bio_resid = 0;
 				biodone(bp1);
 			}
-		} else
+		} else {
+			daschedule(periph);
 			cam_periph_unlock(periph);
+		}
 		if (bp != NULL)
 			biodone(bp);
 		return;
@@ -3459,7 +3579,8 @@ dadone(struct cam_periph *periph, union ccb *done_ccb)
 				scsi_2btoul(bdc->medium_rotation_rate);
 			if (softc->disk->d_rotation_rate ==
 			    SVPD_BDC_RATE_NON_ROTATING) {
-				softc->sort_io_queue = 0;
+				cam_iosched_set_sort_queue(softc->cam_iosched, 0);
+				softc->rotating = 0;
 			}
 			if (softc->disk->d_rotation_rate != old_rate) {
 				disk_attr_changed(softc->disk,
@@ -3521,9 +3642,9 @@ dadone(struct cam_periph *periph, union ccb *done_ccb)
 			    ata_params->media_rotation_rate;
 			if (softc->disk->d_rotation_rate ==
 			    ATA_RATE_NON_ROTATING) {
-				softc->sort_io_queue = 0;
+				cam_iosched_set_sort_queue(softc->cam_iosched, 0);
+				softc->rotating = 0;
 			}
-
 			if (softc->disk->d_rotation_rate != old_rate) {
 				disk_attr_changed(softc->disk,
 				    "GEOM::rotation_rate", M_NOWAIT);
@@ -3652,6 +3773,25 @@ daerror(union ccb *ccb, u_int32_t cam_flags, u_int32_t sense_flags)
 	if (error == ERESTART)
 		return (ERESTART);
 
+	switch (ccb->ccb_h.status & CAM_STATUS_MASK) {
+	case CAM_CMD_TIMEOUT:
+#ifdef CAM_IO_STATS
+		softc->timeouts++;
+#endif
+		break;
+	case CAM_REQ_ABORTED:
+	case CAM_REQ_CMP_ERR:
+	case CAM_REQ_TERMIO:
+	case CAM_UNREC_HBA_ERROR:
+	case CAM_DATA_RUN_ERR:
+#ifdef CAM_IO_STATS
+		softc->errors++;
+#endif
+		break;
+	default:
+		break;
+	}
+
 	/*
 	 * XXX
 	 * Until we have a better way of doing pack validation,
@@ -3671,9 +3811,10 @@ damediapoll(void *arg)
 	struct cam_periph *periph = arg;
 	struct da_softc *softc = periph->softc;
 
-	if (!softc->tur && LIST_EMPTY(&softc->pending_ccbs)) {
+	if (!cam_iosched_has_work_flags(softc->cam_iosched, DA_WORK_TUR) &&
+	    LIST_EMPTY(&softc->pending_ccbs)) {
 		if (cam_periph_acquire(periph) == CAM_REQ_CMP) {
-			softc->tur = 1;
+			cam_iosched_set_work_flags(softc->cam_iosched, DA_WORK_TUR);
 			daschedule(periph);
 		}
 	}