struct hrtaskq_ops {
	int		(*now)(void *);
	void		(*add)(void *, int);
	void		(*del)(void *);
};

struct hrtaskq {
	const struct hrtaskq_ops *
				 hrtq_ops;
	void			*hrtq_cookie;

	struct task_heap	 hrtq_worklist;
	struct mutex		 hrtq_mtx;
};

void
hrtaskq_init(struct hrtaskq *hrtq, const struct hrtaskq_ops *ops,
    void *cookie, int ipl)
{
	hrtq->hrtq_ops = ops;
	hrtq->hrtq_cookie = cookie;
	HEAP_INIT(task_heap, &hrtq->hrtq_worklist);
	mtx_init(&hrtq->hrtq_mtx, ipl);
}

int
hrtaskq_add(struct hrtaskq *hrtq, struct task *t, int nsec)
{
	int rv = 0;
	int diff;
	int now;

	/* check range? */

	now = (*hrtq->hrtq_ops->now)(hrtq->hrtq_cookie);

	mtx_enter(&hrtq->hrtq_mtx);
	if (task_pending(t))
		HEAP_REMOVE(task_heap, &hrtq->hrtq_worklist, t);
	else {
		SET(t->t_flags, TASK_ONQUEUE);
		rv = 1;
	}

	t->t_deadline = now + nsec;
	HEAP_INSERT(task_heap, &hrtq->hrtq_worklist, t);

	t = HEAP_FIRST(task_heap, &hrtq->hrtq_worklist);
	diff = t->t_deadline - now;
	mtx_leave(&hrtq->hrtq_mtx);

	/* check diff is positive? */

	(*hrtq->hrtq_ops->add)(hrtq->hrtq_cookie, diff);

	return (rv);
}

int
hrtaskq_del(struct hrtaskq *hrtq, struct task *t)
{
	int rv = 1;
	int diff;

	if (!task_pending(t))
		return (0);

	mtx_enter(&hrtq->hrtq_mtx);

	if (task_pending(t)) {
		CLR(t->t_flags, TASK_ONQUEUE);
		HEAP_REMOVE(task_heap, &hrtq->hrtq_worklist, t);

		/* figure out the new deadline */
		t = HEAP_FIRST(task_heap, &hrtq->hrtq_worklist);
		if (t != NULL)
			diff = t->t_deadline;
	} else
		rv = 0;

	mtx_leave(&hrtq->hrtq_mtx);

	if (rv == 1) {
		if (t == NULL)
			(*hrtq->hrtq_ops->del)(hrtq->hrtq_cookie);
		else {
			diff -= (*hrtq->hrtq_ops->now)(hrtq->hrtq_cookie);
			(*hrtq->hrtq_ops->add)(hrtq->hrtq_cookie, diff);
		}
	}

	return (rv);
}

void
hrtaskq_run(struct hrtaskq *hrtq, int now)
{
	struct task *next, work;
	int diff = 0;

	mtx_enter(&hrtq->hrtq_mtx);

	while ((next = taskq_extract(&hrtq->hrtq_worklist, now)) != NULL) {
		CLR(next->t_flags, TASK_ONQUEUE);
		work = *next; /* copy to caller to avoid races */
		mtx_leave(&hrtq->hrtq_mtx);

		(*work.t_func)(work.t_arg);

		mtx_enter(&hrtq->hrtq_mtx);
	}

	next = HEAP_FIRST(task_heap, &hrtq->hrtq_worklist);
	if (next != NULL)
		diff = next->t_deadline - now;

	mtx_leave(&hrtq->hrtq_mtx);

	if (next != NULL)
		(*hrtq->hrtq_ops->add)(hrtq->hrtq_cookie, diff);
}