Nucleus

/*
 * This file contains code dealing with the Advanced Programmable Interrupt
 * Controller system.  It should provide a drop-in replacement for the standard
 * PIC system, abstracting it in the same way.  It should also be able to handle
 * a non-SMP system, and fall back on the PIC.
 */

#include <stddef.h>
#include <stdint.h>
#include <nucleus/cpu.h>
#include <nucleus/io.h>
#include <nucleus/lib.h>
#include <nucleus/memory.h>
#include <nucleus/task.h>
#include "acpi.h"

void cpu_load(void);
extern void ap_trampoline(void);

/* Entry types */
enum EntryType {
	ENTRY_LAPIC,
	ENTRY_IOAPIC,
	ENTRY_IOAPIC_ISO,
	ENTRY_IOAPIC_NMI,
	ENTRY_LAPIC_NMI,
	ENTRY_LAPIC_ADDR,
	ENTRY_X2APIC = 9,
};

/* Generic Entry */
struct EntryHeader {
	uint8_t type, length;
};

/* Local APIC */
struct LAPICEntry {
	uint8_t type, length;
	uint8_t procId, id;
	uint32_t flags;
};

/* I/O APIC */
struct IOAPICEntry {
	uint8_t type, length;
	uint8_t id, reserved;
	uint32_t addr, gsiBase;
};

/* IOAPIC Interrupt Source Override */
struct ISOEntry {
	uint8_t type, length;
	uint8_t bus, irq;
	uint8_t gsi, flags;
};

int apic = 0;
size_t ncpus = 1;
uintptr_t lapicPtr, ioapicPtr;
cpu_t lapicIds[MAX_CPUS], lapicNums[MAX_CPUS];
extern uintptr_t stacks[];

/* Enable APIC */
static void
enable_apic(void)
{
	LAPIC(0xF0) = 0x1FF;
	LAPIC(0x80) = 0;
}

/* Read from the IOAPIC */
static uint32_t
read_io_apic(uint32_t reg)
{
	uint32_t volatile *ioapic = (uint32_t volatile *) ioapicPtr;
	ioapic[0] = (reg & 0xFF);
	return ioapic[4];
}

/* Write to the IOAPIC */
static void
write_io_apic(uint32_t reg, uint32_t value)
{
	uint32_t volatile *ioapic = (uint32_t volatile *) ioapicPtr;
	ioapic[0] = (reg & 0xFF);
	ioapic[4] = value;
}

/* Delay in gaps of 100 microseconds */
static void
delay(uint32_t time)
{
	float calc = (1193182 / 10000) * time;
	uint32_t div = (uint32_t) calc;
	outb(0x43, 0x30);
	outb(0x40, div & 0xFF);
	outb(0x40, (div >> 8) & 0xFF);
	do outb(0x43, 0xE2); while (!(inb(0x40) & (1 << 7)));
}

/* Start the APIC timer */
static void
apic_start_timer(void)
{
	/* Wait for this processor's "turn" */
	static cpu_t c = 0;
	while (c != cpu->id);

	/* Start LAPIC countdown from -1 */
	LAPIC(0x3E0) = 3;
	LAPIC(0x380) = 0xFFFFFFFF;
	delay(10);

	/* Stop after PIT fires, count LAPIC ticks */
	LAPIC(0x320) = 0x10000;
	uint32_t ticks = 0xFFFFFFFF - LAPIC(0x390);

	/* Tick every 1ms */
	LAPIC(0x320) = 0x20000 | 32;
	LAPIC(0x3E0) = 3;
	LAPIC(0x380) = ticks;

	/* Signal next processor */
	c++;
}

/* Initialise SMP */
void
init_apic(struct SDTHeader *header)
{
	apic = 1;
	ncpus = 0;

	uint8_t overrides[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};

	/* Iterate the entries */
	lapicPtr = *(uint32_t *) (header + 1);
	set_page(lapicPtr, PAGE_ADDR(lapicPtr) |
	         PTE_PRESENT | PTE_WRITE | PTE_GLOBAL);
	flush_tlb(lapicPtr);
	struct EntryHeader *entry = (void *) (header + 1) + 8;
	struct LAPICEntry *lapic;
	struct IOAPICEntry *ioapic;
	struct ISOEntry *iso;
	while ((void *) entry < (((void *) header) + header->length)) {
		switch (entry->type) {
		case ENTRY_LAPIC:
			if (ncpus >= MAX_CPUS)
				break;
			lapic = (void *) entry;
			if (!(lapic->flags & 1))
				break;
			lapicNums[lapic->id] = ncpus;
			lapicIds[ncpus] = lapic->id;
			ncpus++;
			break;
		case ENTRY_IOAPIC:
			ioapic = (void *) entry;
			ioapicPtr = ioapic->addr;
			break;
		case ENTRY_IOAPIC_ISO:
			iso = (void *) entry;
			overrides[iso->irq] = iso->gsi;
			break;
		}
		entry = (void *) entry + entry->length;
	}
	set_page(ioapicPtr, PAGE_ADDR(ioapicPtr) |
	         PTE_PRESENT | PTE_WRITE | PTE_GLOBAL);
	flush_tlb(ioapicPtr);

	/* Send INT#1 to IRQ#33 on CPU#0 */
	write_io_apic(0x10 + (2 * overrides[1] + 0), 33);
	write_io_apic(0x10 + (2 * overrides[1] + 1), lapicIds[0]);

	/* Initialise APs */
	uint32_t i, j;
	uintptr_t apTrampoline = 0x1000, stack;
	memcpy((void *) apTrampoline, &ap_trampoline, PAGE_SIZE);
	*((uint32_t *) (apTrampoline + 0xFFC)) = (uintptr_t) stacks;
	for (i = 1; i < ncpus; i++) {
		/* Give each processor a separate stack */
		stacks[i] = alloc_frame() + PAGE_SIZE;
//		*((uint32_t *) (apTrampoline + 0xC00) + i) = stacks[i];
		/* Send INIT IPI */
		LAPIC(0x280) = 0;
		LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
		             | (lapicIds[i] << 24);
		LAPIC(0x300) = (LAPIC(0x300) & 0xFFF00000)
		             | 0xC500;
		do asm("pause":::"memory"); while (LAPIC(0x300) & (1 << 12));
		LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
		             | (lapicIds[i] << 24);
		LAPIC(0x300) = (LAPIC(0x300) & 0xFFF00000)
		             | 0x8500;
		do asm("pause":::"memory"); while (LAPIC(0x300) & (1 << 12));
		delay(100);
		/* Send STARTUP IPI (twice) */
		for (j = 0; j < 2; j++) {
			LAPIC(0x280) = 0;
			LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
			             | (lapicIds[i] << 24);
			LAPIC(0x300) = (LAPIC(0x300) & 0xFFF0F800)
			             | (0x600 + ((apTrampoline >> 12) & 0xFF));
			delay(2);
			do
				asm("pause":::"memory");
			while (LAPIC(0x300) & (1 << 12));
		}
	}

	/* Enable APIC */
	enable_apic();
	/* Disable PIC */
	outb(0x21, 0xFF);
	outb(0xA1, 0xFF);

	apic_start_timer();
}

/* AP initialisation routine */
_Noreturn void
ap_startup(void)
{
	/* Set up CPU environment */
	enable_apic();
	cpu_load();
	cpu_load_paging();
	apic_start_timer();

	/* Start running tasks */
	schedule();
	__builtin_unreachable();
}

/* Send an IPI */
void
send_ipi(cpu_t target, uint8_t num)
{
	LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
	             | (lapicIds[target] << 24);
	LAPIC(0x300) = (LAPIC(0x300) & 0xFFF32000)
	             | (0x5000 + num + 48);
	do asm("pause":::"memory"); while (LAPIC(0x300) & (1 << 12));
}