Nucleus

diff --git a/kernel/cpu.c b/kernel/cpu.c
new file mode 100644
index 0000000..be56047
--- /dev/null
+++ b/kernel/cpu.c
@@ -0,0 +1,154 @@
+/*
+ * 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/memory.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];
+
+/* 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();
+}