#include <socks/sched.h>
#include <socks/percpu.h>
#include <socks/cpu.h>

#define PRIO_MASK(p) (((uint32_t)1) << (p))
#define FIRST_PRIO(m) (m > 0 ? (PRIO_MAX - __builtin_clz(m) - 1) : -1)

void rq_init(struct runqueue *rq)
{
	memset(rq, 0x00, sizeof *rq);
	rq->rq_lock = SPIN_LOCK_INIT;
}

struct thread *rq_dequeue(struct runqueue *rq)
{
	int prio = FIRST_PRIO(rq->rq_readybits);
	if (prio == -1) {
		return NULL;
	}

	struct queue *q = &rq->rq_queues[prio];
	struct queue_entry *qe = queue_pop_front(q);
	if (!qe) {
		rq->rq_readybits &= ~PRIO_MASK(prio);
		return NULL;
	}

	struct thread *thr = QUEUE_CONTAINER(struct thread, tr_rqentry, qe);

	if (rq->rq_nthreads > 0) {
		rq->rq_nthreads--;
	}

	if (queue_empty(q)) {
		rq->rq_readybits &= ~PRIO_MASK(prio);
	}

	return thr;
}

void rq_enqueue(struct runqueue *rq, struct thread *thr)
{
	int prio = thread_priority(thr);
	if (prio < 0 || prio > PRIO_MAX) {
		return;
	}

	struct queue *q = &rq->rq_queues[prio];
	queue_push_back(q, &thr->tr_rqentry);
	rq->rq_nthreads++;

	rq->rq_readybits |= PRIO_MASK(thread_priority(thr));
	thr->tr_rq = rq;
}

struct runqueue *cpu_rq(unsigned int cpu)
{
	struct cpu_data *cpu_data = get_cpu(cpu);
	struct runqueue *rq = &cpu_data->c_rq;
	put_cpu(cpu_data);

	return rq;
}