1 files changed, 52 insertions, 3 deletions

diff --git a/sys/cam/cam_queue.h b/sys/cam/cam_queue.h
index 9b27bae..0bb4491 100644
--- a/sys/cam/cam_queue.h
+++ b/sys/cam/cam_queue.h
@@ -57,6 +57,8 @@ SLIST_HEAD(ccb_hdr_slist, ccb_hdr);
 
 struct cam_ccbq {
 	struct	camq queue;
+	struct ccb_hdr_tailq	queue_extra_head;
+	int	queue_extra_entries;
 	int	devq_openings;
 	int	devq_allocating;
 	int	dev_openings;
@@ -177,7 +179,7 @@ cam_ccbq_release_opening(struct cam_ccbq *ccbq);
 static __inline int
 cam_ccbq_pending_ccb_count(struct cam_ccbq *ccbq)
 {
-	return (ccbq->queue.entries);
+	return (ccbq->queue.entries + ccbq->queue_extra_entries);
 }
 
 static __inline void
@@ -190,14 +192,61 @@ cam_ccbq_take_opening(struct cam_ccbq *ccbq)
 static __inline void
 cam_ccbq_insert_ccb(struct cam_ccbq *ccbq, union ccb *new_ccb)
 {
+	struct ccb_hdr *old_ccb;
+	struct camq *queue = &ccbq->queue;
+
 	ccbq->held--;
-	camq_insert(&ccbq->queue, &new_ccb->ccb_h.pinfo);
+
+	/*
+	 * If queue is already full, try to resize.
+	 * If resize fail, push CCB with lowest priority out to the TAILQ.
+	 */
+	if (queue->entries == queue->array_size &&
+	    camq_resize(&ccbq->queue, queue->array_size * 2) != CAM_REQ_CMP) {
+		old_ccb = (struct ccb_hdr *)camq_remove(queue, queue->entries);
+		TAILQ_INSERT_HEAD(&ccbq->queue_extra_head, old_ccb,
+		    xpt_links.tqe);
+		old_ccb->pinfo.index = CAM_EXTRAQ_INDEX;
+		ccbq->queue_extra_entries++;
+	}
+
+	camq_insert(queue, &new_ccb->ccb_h.pinfo);
 }
 
 static __inline void
 cam_ccbq_remove_ccb(struct cam_ccbq *ccbq, union ccb *ccb)
 {
-	camq_remove(&ccbq->queue, ccb->ccb_h.pinfo.index);
+	struct ccb_hdr *cccb, *bccb;
+	struct camq *queue = &ccbq->queue;
+
+	/* If the CCB is on the TAILQ, remove it from there. */
+	if (ccb->ccb_h.pinfo.index == CAM_EXTRAQ_INDEX) {
+		TAILQ_REMOVE(&ccbq->queue_extra_head, &ccb->ccb_h,
+		    xpt_links.tqe);
+		ccb->ccb_h.pinfo.index = CAM_UNQUEUED_INDEX;
+		ccbq->queue_extra_entries--;
+		return;
+	}
+
+	camq_remove(queue, ccb->ccb_h.pinfo.index);
+
+	/*
+	 * If there are some CCBs on TAILQ, find the best one and move it
+	 * to the emptied space in the queue.
+	 */
+	bccb = TAILQ_FIRST(&ccbq->queue_extra_head);
+	if (bccb == NULL)
+		return;
+	TAILQ_FOREACH(cccb, &ccbq->queue_extra_head, xpt_links.tqe) {
+		if (bccb->pinfo.priority > cccb->pinfo.priority ||
+		    (bccb->pinfo.priority == cccb->pinfo.priority &&
+		     GENERATIONCMP(bccb->pinfo.generation, >,
+		      cccb->pinfo.generation)))
+		        bccb = cccb;
+	}
+	TAILQ_REMOVE(&ccbq->queue_extra_head, bccb, xpt_links.tqe);
+	ccbq->queue_extra_entries--;
+	camq_insert(queue, &bccb->pinfo);
 }
 
 static __inline union ccb *