struct usb_taskq {
	TAILQ_HEAD(, usb_task) tasks;
	kmutex_t lock;
	kcondvar_t cv;
	struct lwp *task_thread_lwp;
	const char *name;
	struct usb_task *current;
	int flags;
#define	USB_TASKQ_DONE		0x01
#define	USB_TASKQ_REQUEUED	0x02
};

static struct usb_taskq usb_taskq[USB_NUM_TASKQS];

void
usb_add_task(usbd_device_handle dev, struct usb_task *task, int queue)
{
	struct usb_taskq *taskq;
	int queue0;

	KASSERT(0 <= queue);
	KASSERT(queue < USB_NUM_TASKS);

	taskq = &usb_taskq[queue];
	mutex_enter(&taskq->lock);
	queue0 = atomic_cas_uint(&task->queue, USB_NUM_TASKQS, queue);
	if (queue0 == USB_NUM_TASKQS) {
		TAILQ_INSERT_TAIL(&taskq->tasks, task, next);
		cv_signal(&taskq->cv);
	} else {
		KASSERT(queue0 == queue);
		if ((taskq->current == task) &&
		    !ISSET(taskq->flags, USB_TASKQ_REQUEUED)) {
			taskq->flags |= USB_TASKQ_REQUEUED;
			TAILQ_INSERT_TAIL(&taskq->tasks, task, next);
		}
	}
	mutex_exit(&taskq->lock);
}

void
usb_rem_task(usbd_device_handle dv, struct usb_task *task)
{
	struct usb_taskq *taskq;
	unsigned queue;

	queue = task->queue;
	if (queue == USB_NUM_TASKQS)
		return;
	taskq = &usb_taskq[queue];
	mutex_enter(&taskq->lock);
	if (task->queue == USB_NUM_TASKQS)
		goto out;
	KASSERT(task->queue == queue);
	if (taskq->current == task)
		goto out;
	TAILQ_REMOVE(&taskq->tasks, task, next);
out:	mutex_exit(&taskq->lock);
}

bool
usb_rem_task_sync(usbd_device_handle dv, struct usb_task *task,
    kmutex_t *interlock)
{
	struct usb_taskq *taskq;
	unsigned queue;

	ASSERT_SLEEPABLE();

	queue = task->queue;
	if (queue == USB_NUM_TASKQS)
		return false;	/* not running, didn't cancel */
	taskq = &usb_taskq[queue];
	mutex_enter(&taskq->lock);
	if (task->queue == USB_NUM_TASKQS) {
		mutex_exit(&taskq->lock);
		return false;	/* already ran, didn't cancel */
	}
	KASSERT(task->queue == queue);
	if (taskq->current == task) {
		mutex_exit(interlock);
		while ((taskq->current == task) &&
		    !ISSET(taskq->flags, USB_TASKQ_DONE))
			cv_wait(&taskq->cv, &taskq->lock);
		mutex_exit(&taskq->lock);
		mutex_enter(interlock);
		return false;	/* came in too late to cancel */
	}
	TAILQ_REMOVE(&taskq->tasks, task, next);
	mutex_exit(&taskq->lock);
	return true;		/* cancelled it */
}

static void
usb_task_done_locked(struct usb_taskq *taskq, struct usb_task *task)
{

	KASSERT(mutex_owned(&taskq->lock));
	KASSERT(taskq->current == task);
	KASSERT(!ISSET(taskq->flags, USB_TASKQ_DONE));
	KASSERT(task->queue == taskq - usb_taskqs);

	if (ISSET(taskq->flags, USB_TASKQ_REQUEUED)) {
		taskq->flags &= ~USB_TASKQ_REQUEUED;
	} else {
		const unsigned queue0 =
		    atomic_swap_uint(&task->queue, USB_NUM_TASKQS);

		KASSERT(queue0 == taskq - usb_taskqs);
		cv_broadcast(&taskq->cv);
	}
	taskq->flags |= USB_TASKQ_DONE;
}

void
usb_task_done(struct usb_task *task)
{
	struct usb_taskq *taskq;
	unsigned queue;

	queue = task->queue;
	KASSERT(queue != USB_NUM_TASKQS);
	taskq = &usb_taskqs[queue];
	mutex_enter(&taskq->lock);
	KASSERT(task->queue == queue);
	KASSERT(taskq->current == task);
	KASSERT(!ISSET(taskq->flags, USB_TASKQ_DONE));
	usb_task_done_locked(taskq, task);
	mutex_exit(&taskq->lock);
}

void
usb_task_thread(void *arg)
{
	struct usb_task *task;
	struct usb_taskq *taskq;
	bool mpsafe;

	taskq = arg;
	DPRINTF(("usb_task_thread: start taskq %s\n", taskq->name));

	mutex_enter(&taskq->lock);
	for (;;) {
		while (TAILQ_EMPTY(&taskq->tasks))
			cv_wait(&taskq->cv, &taskq->lock);
		DPRINTFN(2,("usb_task_thread: woke up task=%p\n", task));
		mpsafe = ISSET(task->flags, USB_TASKQ_MPSAFE);
		TAILQ_REMOVE(&taskq->tasks, task, next);
		taskq->current = task;
		mutex_exit(&taskq->lock);

		if (!mpsafe)
			KERNEL_LOCK(1, curlwp);
		task->fun(task->arg);
		if (!mpsafe)
			KERNEL_UNLOCK_ONE(curlwp);

		mutex_enter(&taskq->lock);
		if (!ISSET(taskq->flags, USB_TASKQ_DONE))
			usb_task_done_locked(taskq, task);
		KASSERT(ISSET(taskq->flags, USB_TASKQ_DONE));
		taskq->flags &= ~(USB_TASKQ_DONE | USB_TASKQ_REQUEUED);
	}
	mutex_exit(&taskq->lock);
}