#include <kernel.h>
#include <mem.h>
#include <smp.h>

static uint8_t checksum(void *start, size_t n) {
	uint8_t sum = 0;
	uint8_t *bytes;
	int i;

	bytes = start;
	for (i=0; i<n; i++) {
		sum += bytes[i];
	}
	return sum;
}

#if 0
static void *scanfordword(paddr_t start, paddr_t max, size_t by, uint32_t val) {
	while (start <= max - by) {
		if (*(uint32_t *)(PHY2VIRTP(start)) == val) {
			return PHY2VIRTP(start);
		}
		start += by;
	}
	return 0;
}
#endif

/* TODO this could probably be a kernel thread, and just power up
 * processors as though they were hot-plugged.  Except that the
 * local apic on the BSP needs to be configured
 */
void init_smp(void) {
	uint16_t *bda;
	uint32_t mpsig = 0x5f504d5f;
	uint32_t tabsig = 0x504d4350;
	uintptr_t mem;
	struct mpfp *mp = 0;
	struct mpconfig *config = 0;
	int i;

	/* search for base table */
	
	/* search first KB of the extended bios data area */
	bda = PHY2VIRTP(0x40E);
	mem = (uintptr_t)((*bda) << 4);

	for (i=0;i <= 1024 - 16 ;i+=16) {
		mp = PHY2VIRTP(mem+i);
		if ( mp->signature == mpsig && checksum(mp, 16) == 0) {
			goto found;
		}
	}

	/* search last kb of base mem, 639-640k */
	mem = KB(639);
	for (i=0;i<1024-16;i+=16) {
		mp = PHY2VIRTP(mem+i);
		if ( mp->signature == mpsig && checksum(mp, 16) == 0) {
			goto found;
		}
	}

	/* search bios rom address space, f0000 - fffff */
	mem = 0xf0000;
	for (i=0;i<KB(64)-16;i+=16) {
		mp = PHY2VIRTP(mem+i);
		if ( mp->signature == mpsig && checksum(mp, 16) == 0) {
			goto found;
		}
	}

	printk("mpft not found\n");
	return;
found:
	printk("found mpft at %llx: features1 %x, features2 %x\n", (uintptr_t)mp,
		       	(uint32_t)mp->features1, (uint32_t)mp->features2);
	config = PHY2VIRTP(mp->mpconfigptr);
	if (config->signature != tabsig) {
		printk("tabsig bad\n");
		return;
	}
#if 0
	if (checksum(config, (size_t)config->tablelength) != 0) {
		printk("config table checksum bad\n");
		return;
	}
#endif

	vaddr_t entry = (vaddr_t)config + sizeof *config;
	printk("mpct local apic: %x, entries: %x, len = %x\n", config->localapicaddr, config->entrycount, config->tablelength);
	int procnum = 0;
	for (i=0;i<config->entrycount;i++) {
		uint8_t etype;
		struct processorentry *proc;
		struct ioapic *io;

		etype = *(uint8_t *)entry;
		switch (etype) {
			case 0: /* processor */
				proc = (struct processorentry *)entry;
				printk("processor %x, apic = %x:%s%s\n", procnum,
					       	(uint32_t)proc->apicid,
					       	proc->enabled ? " enabled" : "",
					       	proc->bootstrap ? " bsp" : ""
					);
				procnum++;
				entry += 20;
				break;
			case 1: /* bus */
				//printk("mp bus\n");
				entry += 8;
				break;
			case 2: /* io apic */
				io = (struct ioapic *)entry;
				if (io->enabled) {
					printk("ioapic %x %llx\n", io->id, io->address);
				} else {
					printk("ioapic %x disabledx\n", io->id);
				}
				entry += 8;
				break;
			default:
				//printk(" type: %x\n", etype);
				entry += 8;
				break;
		}
	}
}