Orion

/*
 * This file contains code dealing with the Advanced Programmable Interrupt
 * Controller system.  It should proved 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 <stdint.h>
#include "proc.h"
#include "../mem/heap.h"
#include "../mem/mem.h"
#include "../task/task.h"
#include "../io.h"
#include "../screen.h"
#include "../spinlock.h"

/* RSDP Descriptor */
typedef struct RSDPDescriptor {
	char signature[8];
	uint8_t checksum;
	char oem[6];
	uint8_t revision;
	uint32_t rsdt;
} __attribute__((packed)) RSDPDescriptor;

/* System Descriptor Table Header */
typedef struct SDTHeader {
	char signature[4];
	uint32_t length;
	uint8_t revision;
	uint8_t checksum;
	char oem[6];
	char oemTable[8];
	uint32_t oemRevision;
	uint32_t creator;
	uint32_t creatorRevision;
} SDTHeader;

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

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

uint8_t numCores;
uint8_t bspId;
uint32_t lapicPtr, ioapicPtr;
uint8_t lapicIds[MAX_CPUS], lapicNums[MAX_CPUS];
Spinlock timerLock;

/* 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 divisor = (uint32_t) calc;
	outb(0x43, 0x30);
	outb(0x40, divisor & 0xFF);
	outb(0x40, (divisor >> 8) & 0xFF);
	do outb(0x43, 0xE2); while (!(inb(0x40) & (1 << 7)));
}

/* Start the APIC timer */
static void
apic_start_timer(void)
{
	/* Don't run while another timer is calibrating */
	acquire(&timerLock);

	/* 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;

	release(&timerLock);
}

/* Initialise SMP (RSDT method) */
void
init_multicore(void *ebda)
{
	numCores = 1;
	memset(lapicIds, 0, MAX_CPUS);
	memset(lapicNums, 0, MAX_CPUS);

	init_lock(&timerLock);

	/* Search for SMP tables, start from EBDA start */
	char *signature = (char *) ebda;
	uint8_t found;
	while (signature < (char *) 0x100000) {
		/* RSDT */
		if (!memcmp(signature, "RSD PTR ", 8)) {
			found = 1;
			break;
		}
		signature++;
	}
	/* Fall back to PIC */
	if (!found)
		return;
	/* RSDT checksum */
	uint32_t check = 0, i, j;
	for (i = 0; i < sizeof(RSDPDescriptor); i++)
		check += signature[i];
	if (check % 0x100)
		return;

	uint8_t *ptr, *ptr2;
	uint8_t *rsdt, len;
	uint8_t overrides[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
	IOAPICOverride *iso;

	/* Find APIC table */
	kprintf("Searching for APIC table");
	asm volatile("mov $1, %%eax; cpuid; shrl $24, %%ebx" : "=b" (bspId));
	numCores = 0;
	rsdt = (uint8_t *) ((RSDPDescriptor *) signature)->rsdt;
	len = *((uint32_t *) (rsdt + 4));
	for (ptr2 = rsdt + 36; ptr2 < rsdt + len; ptr2 += 4) {
		ptr = *((uint8_t **) ptr2);
		if (!memcmp(ptr, "APIC", 4))
			break;
	}
	if (memcmp(ptr, "APIC", 4))
		return;

	/* Read APIC table */
	lapicPtr = *((uint32_t *) (ptr + 0x24));
	ptr2 = ptr + *((uint32_t *) (ptr + 4));
	for (ptr += 44; ptr < ptr2; ptr += ptr[1]) {
		/* Entries */
		switch (ptr[0]) {
		case 0: /* Processor Local APIC */
			if (numCores >= MAX_CPUS)
				continue;
			if (ptr[4] & 1) {
				lapicNums[ptr[3]] = numCores;
				lapicIds[numCores] = ptr[3];
				numCores++;
			}
			break;
		case 1: /* IOAPIC */
			ioapicPtr = *((uint32_t *) (ptr + 4));
			break;
		case 2: /* IOAPIC ISO */
			iso = (void *) ptr;
			overrides[iso->irq] = iso->gsi;
			kprintf("  IRQ#%.2d -> GSI#%.2d",
			        iso->irq, iso->gsi);
			break;
		}
	}

	/* 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), bspId);

	kprintf("Detected %d CPU%c", numCores, (numCores>1)?'s':'\0');

	/* Initialise APs */
	uintptr_t apTrampoline = 0x2000;
	memcpy((void *) apTrampoline, &ap_trampoline, 0x1000);
	for (i = 0; i < numCores; i++) {
		if (lapicIds[i] == bspId)
			continue;
		/* Give each CPU a separate stack */
		*((uint32_t *) (apTrampoline + 0xFF0)) = 0x7A000 - (i * 0x2000);
		/* 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)
			             | (0x0600 + ((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 */
void
ap_startup(void)
{
//	enable_apic();
	cpu_load();
//	apic_start_timer();
	current = NULL;
	kprintf("CPU#%d idling", CPUID);
	while (1) asm volatile("hlt");
}