/*
 * This file implements the CPU setup and control functions, in particular the
 * IPI messaging system.  Each CPU has a message queue (IPIQ) which can be
 * written by other CPUs.  Each message describes a function that shoudl be run
 * on the target CPU. When signalled via an IPI, a CPU will check it's queue for
 * work and execute anything outstanding.  Sending a message to an IPIQ will
 * wait for the IPI to be received.  If the message is synchronous (IPIQ_SYNC)
 * then the call will also wait for the function to return on the target.  If
 * the message is asynchronous (IPIQ_ASYNC) then it will return immediately and
 * the call may be completed at any time, with the target performing any
 * necessary clean-up.
 */

#include <nucleus/cpu.h>
#include <nucleus/lib.h>
#include <nucleus/task.h>
#include "desc.h"

/* Structure for a IPI Message Queue */
struct IPIQueue {
	struct IPIMessage *msg[32];
	uint32_t slots;
	int working;
};

/* Structure for an IPI Message */
struct IPIMessage {
	enum IPIQFlag flags;
	ipiq_func_t func;
	void *arg;
};

void send_ipi(cpu_t target, uint8_t num);

Processor __seg_gs *const cpu = 0;
Processor *cpus[MAX_CPUS];
uintptr_t stacks[MAX_CPUS];

/* Per-CPU setup */
void
cpu_load(void)
{
	/* Initialise SSE */
	asm volatile(
		"mov %cr0, %eax;"
		"andl $0xfffffffb, %eax;"
		"orl $0x2, %eax;"
		"mov %eax, %cr0;"
		"mov %cr4, %eax;"
		"orl $0x600, %eax;"
		"mov %eax, %cr4;"
	);

	/* Initialise CR4.PGE */
	asm volatile(
		"mov %cr4, %eax;"
		"orl $0x10, %eax;"
		"mov %eax, %cr4;"
	);

	/* Tables */
	cpu_load_idt();
	cpu_load_gdt();

	/* Processor structure */
	cpu_t id = CPUID;
	cpus[id] = kmalloc(sizeof(Processor));
	set_gs_base((uintptr_t) cpus[id]);
	cpu->self = cpus[id];
	cpu->id = id;
	cpu->scheduler = new(&schedulerType);
	cpu->ipiq = kmalloc(sizeof(struct IPIQueue));

	asm volatile("sti");
}

/* IPI Queue handler */
void
ipiq_handler(struct InterruptFrame *frame)
{
	struct IPIMessage *msg;
	struct IPIQueue *queue = cpu->ipiq;

	if (queue->working || !queue->slots)
		return;

	enter_critical_section();
	queue->working++;

	/* Dispatch all messages that are pending */
	uint8_t slot;
	for (slot = 0; slot < 32; slot++) {
		msg = queue->msg[slot];
		/* Check the bitset for used slots */
		if (!(queue->slots & (1 << slot)))
			continue;
		queue->slots &= ~(1 << slot);
		msg->func(msg->arg);
		if (msg->flags & IPIQ_SYNC)
			msg->flags |= IPIQ_DONE;
		else
			kfree(msg);
	}

	queue->working--;
	exit_critical_section();
}

/* Request a CPU run a function */
void
send_ipiq(cpu_t targid, ipiq_func_t func, void *arg, enum IPIQFlag flags)
{
	enter_critical_section();
	if (__builtin_expect((targid == cpu->id), 0)) {
		func(arg);
		goto end;
	}

	/* Construct IPI message */
	struct IPIMessage *msg = kmalloc(sizeof(struct IPIMessage));
	msg->func = func;
	msg->arg = arg;
	msg->flags = flags & ~IPIQ_DONE;

	/* Find slot and send */
	uint8_t slot;
	struct IPIQueue *queue = cpus[targid]->ipiq;
find_slot:
	for (slot = 0; slot < 32; slot++) {
		/*
		 * Search for a free slot by testing and setting each bit in the
		 * bitset until one is changed.  One bit per slot minimises the
		 * contention and guarantees synchronisation when acted on
		 * atomically. Only try to set one bit at a time, then test that
		 * bit, none of the others.
		 */
		if (!((__atomic_fetch_xor(&queue->slots, 1 << slot,
		    __ATOMIC_ACQUIRE) >> slot) & 1))
			break;
	}
	if (slot == 32)
		goto find_slot;
	queue->msg[slot] = msg;
	if (!queue->working)
		send_ipi(targid, 0);

	/* Wait for completion */
	if (flags & IPIQ_SYNC) {
		while (!(msg->flags & IPIQ_DONE));
		kfree(msg);
	}

end:
	exit_critical_section();
}