Index: kernel/arch/ia32/src/smp/mps.c
===================================================================
--- kernel/arch/ia32/src/smp/mps.c	(revision d99c1d2ba8c7b2c687d430f2e9fd237046606545)
+++ kernel/arch/ia32/src/smp/mps.c	(revision 9110ff070e5e3abb5381cb99117b41599fabdcb9)
@@ -27,5 +27,5 @@
  */
 
-/** @addtogroup ia32	
+/** @addtogroup ia32
  * @{
  */
@@ -52,47 +52,70 @@
  */
 
-#define	FS_SIGNATURE	0x5f504d5f
-#define CT_SIGNATURE 	0x504d4350
-
-static int mps_fs_check(uint8_t *base);
-static int mps_ct_check(void);
-
-static int configure_via_ct(void);
-static int configure_via_default(uint8_t n);
-
-static int ct_processor_entry(struct __processor_entry *pr);
-static void ct_bus_entry(struct __bus_entry *bus);
-static void ct_io_apic_entry(struct __io_apic_entry *ioa);
-static void ct_io_intr_entry(struct __io_intr_entry *iointr);
-static void ct_l_intr_entry(struct __l_intr_entry *lintr);
-
-static void ct_extended_entries(void);
+#define FS_SIGNATURE  0x5f504d5f
+#define CT_SIGNATURE  0x504d4350
 
 static struct mps_fs *fs;
 static struct mps_ct *ct;
 
-struct __processor_entry *processor_entries = NULL;
-struct __bus_entry *bus_entries = NULL;
-struct __io_apic_entry *io_apic_entries = NULL;
-struct __io_intr_entry *io_intr_entries = NULL;
-struct __l_intr_entry *l_intr_entries = NULL;
-
-unsigned int processor_entry_cnt = 0;
-unsigned int bus_entry_cnt = 0;
-unsigned int io_apic_entry_cnt = 0;
-unsigned int io_intr_entry_cnt = 0;
-unsigned int l_intr_entry_cnt = 0;
-
-/*
- * Implementation of IA-32 SMP configuration interface.
- */
-static size_t get_cpu_count(void);
-static bool is_cpu_enabled(size_t i);
-static bool is_bsp(size_t i);
-static uint8_t get_cpu_apic_id(size_t i);
-static int mps_irq_to_pin(unsigned int irq);
-
+static struct __processor_entry *processor_entries = NULL;
+static struct __bus_entry *bus_entries = NULL;
+static struct __io_apic_entry *io_apic_entries = NULL;
+static struct __io_intr_entry *io_intr_entries = NULL;
+static struct __l_intr_entry *l_intr_entries = NULL;
+
+static size_t io_apic_cnt = 0;
+
+static size_t processor_entry_cnt = 0;
+static size_t bus_entry_cnt = 0;
+static size_t io_apic_entry_cnt = 0;
+static size_t io_intr_entry_cnt = 0;
+static size_t l_intr_entry_cnt = 0;
+
+static uint8_t get_cpu_apic_id(size_t i)
+{
+	ASSERT(i < processor_entry_cnt);
+	
+	return processor_entries[i].l_apic_id;
+}
+
+static bool is_cpu_enabled(size_t i)
+{
+	ASSERT(i < processor_entry_cnt);
+	
+	/*
+	 * FIXME: The current local APIC driver limits usable
+	 * APIC IDs to 8.
+	 *
+	 */
+	if (get_cpu_apic_id(i) > 7)
+		return false;
+	
+	return (bool) ((processor_entries[i].cpu_flags & 0x01) == 0x01);
+}
+
+static bool is_bsp(size_t i)
+{
+	ASSERT(i < processor_entry_cnt);
+	
+	return (bool) ((processor_entries[i].cpu_flags & 0x02) == 0x02);
+}
+
+static int mps_irq_to_pin(unsigned int irq)
+{
+	size_t i;
+	
+	for (i = 0; i < io_intr_entry_cnt; i++) {
+		if (io_intr_entries[i].src_bus_irq == irq &&
+		    io_intr_entries[i].intr_type == 0)
+			return io_intr_entries[i].dst_io_apic_pin;
+	}
+	
+	return -1;
+}
+
+/** Implementation of IA-32 SMP configuration interface.
+ *
+ */
 struct smp_config_operations mps_config_operations = {
-	.cpu_count = get_cpu_count,
 	.cpu_enabled = is_cpu_enabled,
 	.cpu_bootstrap = is_bsp,
@@ -101,32 +124,8 @@
 };
 
-size_t get_cpu_count(void)
-{
-	return processor_entry_cnt;
-}
-
-bool is_cpu_enabled(size_t i)
-{
-	ASSERT(i < processor_entry_cnt);
-	return (bool) ((processor_entries[i].cpu_flags & 0x01) == 0x01);
-}
-
-bool is_bsp(size_t i)
-{
-	ASSERT(i < processor_entry_cnt);
-	return (bool) ((processor_entries[i].cpu_flags & 0x02) == 0x02);
-}
-
-uint8_t get_cpu_apic_id(size_t i)
-{
-	ASSERT(i < processor_entry_cnt);
-	return processor_entries[i].l_apic_id;
-}
-
-
-/*
- * Used to check the integrity of the MP Floating Structure.
- */
-int mps_fs_check(uint8_t *base)
+/** Check the integrity of the MP Floating Structure.
+ *
+ */
+static bool mps_fs_check(uint8_t *base)
 {
 	unsigned int i;
@@ -136,112 +135,240 @@
 		sum = (uint8_t) (sum + base[i]);
 	
-	return !sum;
-}
-
-/*
- * Used to check the integrity of the MP Configuration Table.
- */
-int mps_ct_check(void)
+	return (sum == 0);
+}
+
+/** Check the integrity of the MP Configuration Table.
+ *
+ */
+static bool mps_ct_check(void)
 {
 	uint8_t *base = (uint8_t *) ct;
 	uint8_t *ext = base + ct->base_table_length;
 	uint8_t sum;
-	int i;	
-	
-	/* count the checksum for the base table */
-	for (i = 0,sum = 0; i < ct->base_table_length; i++)
+	uint16_t i;
+	
+	/* Compute the checksum for the base table */
+	for (i = 0, sum = 0; i < ct->base_table_length; i++)
 		sum = (uint8_t) (sum + base[i]);
-		
+	
 	if (sum)
-		return 0;
-		
-	/* count the checksum for the extended table */
+		return false;
+	
+	/* Compute the checksum for the extended table */
 	for (i = 0, sum = 0; i < ct->ext_table_length; i++)
 		sum = (uint8_t) (sum + ext[i]);
-		
-	return sum == ct->ext_table_checksum;
-}
-
-void mps_init(void)
-{
-	uint8_t *addr[2] = { NULL, (uint8_t *) PA2KA(0xf0000) };
-	unsigned int i, j, length[2] = { 1024, 64 * 1024 };
-	
-
+	
+	return (sum == ct->ext_table_checksum);
+}
+
+static void ct_processor_entry(struct __processor_entry *pr)
+{
 	/*
-	 * Find MP Floating Pointer Structure
-	 * 1a. search first 1K of EBDA
-	 * 1b. if EBDA is undefined, search last 1K of base memory
-	 *  2. search 64K starting at 0xf0000
+	 * Ignore processors which are not marked enabled.
 	 */
-
-	addr[0] = (uint8_t *) PA2KA(ebda ? ebda : 639 * 1024);
-	for (i = 0; i < 2; i++) {
-		for (j = 0; j < length[i]; j += 16) {
-			if (*((uint32_t *) &addr[i][j]) ==
-			    FS_SIGNATURE && mps_fs_check(&addr[i][j])) {
-				fs = (struct mps_fs *) &addr[i][j];
-				goto fs_found;
-			}
+	if ((pr->cpu_flags & (1 << 0)) == 0)
+		return;
+	
+	apic_id_mask |= (1 << pr->l_apic_id);
+}
+
+static void ct_bus_entry(struct __bus_entry *bus __attribute__((unused)))
+{
+#ifdef MPSCT_VERBOSE
+	char buf[7];
+	
+	memcpy((void *) buf, (void *) bus->bus_type, 6);
+	buf[6] = 0;
+	
+	printf("MPS: bus=%" PRIu8 " (%s)\n", bus->bus_id, buf);
+#endif
+}
+
+static void ct_io_apic_entry(struct __io_apic_entry *ioa)
+{
+	/* This I/O APIC is marked unusable */
+	if ((ioa->io_apic_flags & 1) == 0)
+		return;
+	
+	if (io_apic_cnt++ > 0) {
+		/*
+		 * Multiple I/O APICs are currently not supported.
+		 */
+		return;
+	}
+	
+	io_apic = (uint32_t *) (uintptr_t) ioa->io_apic;
+}
+
+static void ct_io_intr_entry(struct __io_intr_entry *iointr
+    __attribute__((unused)))
+{
+#ifdef MPSCT_VERBOSE
+	printf("MPS: ");
+	
+	switch (iointr->intr_type) {
+	case 0:
+		printf("INT");
+		break;
+	case 1:
+		printf("NMI");
+		break;
+	case 2:
+		printf("SMI");
+		break;
+	case 3:
+		printf("ExtINT");
+		break;
+	}
+	
+	printf(", ");
+	
+	switch (iointr->poel & 3) {
+	case 0:
+		printf("bus-like");
+		break;
+	case 1:
+		printf("active high");
+		break;
+	case 2:
+		printf("reserved");
+		break;
+	case 3:
+		printf("active low");
+		break;
+	}
+	
+	printf(", ");
+	
+	switch ((iointr->poel >> 2) & 3) {
+	case 0:
+		printf("bus-like");
+		break;
+	case 1:
+		printf("edge-triggered");
+		break;
+	case 2:
+		printf("reserved");
+		break;
+	case 3:
+		printf("level-triggered");
+		break;
+	}
+	
+	printf(", bus=%" PRIu8 " irq=%" PRIu8 " io_apic=%" PRIu8" pin=%"
+	    PRIu8 "\n", iointr->src_bus_id, iointr->src_bus_irq,
+	    iointr->dst_io_apic_id, iointr->dst_io_apic_pin);
+#endif
+}
+
+static void ct_l_intr_entry(struct __l_intr_entry *lintr
+    __attribute__((unused)))
+{
+#ifdef MPSCT_VERBOSE
+	printf("MPS: ");
+	
+	switch (lintr->intr_type) {
+	case 0:
+		printf("INT");
+		break;
+	case 1:
+		printf("NMI");
+		break;
+	case 2:
+		printf("SMI");
+		break;
+	case 3:
+		printf("ExtINT");
+		break;
+	}
+	
+	printf(", ");
+	
+	switch (lintr->poel & 3) {
+	case 0:
+		printf("bus-like");
+		break;
+	case 1:
+		printf("active high");
+		break;
+	case 2:
+		printf("reserved");
+		break;
+	case 3:
+		printf("active low");
+		break;
+	}
+	
+	printf(", ");
+	
+	switch ((lintr->poel >> 2) & 3) {
+	case 0:
+		printf("bus-like");
+		break;
+	case 1:
+		printf("edge-triggered");
+		break;
+	case 2:
+		printf("reserved");
+		break;
+	case 3:
+		printf("level-triggered");
+		break;
+	}
+	
+	printf(", bus=%" PRIu8 " irq=%" PRIu8 " l_apic=%" PRIu8" pin=%"
+	    PRIu8 "\n", lintr->src_bus_id, lintr->src_bus_irq,
+	    lintr->dst_l_apic_id, lintr->dst_l_apic_pin);
+#endif
+}
+
+static void ct_extended_entries(void)
+{
+	uint8_t *ext = (uint8_t *) ct + ct->base_table_length;
+	uint8_t *cur;
+	
+	for (cur = ext; cur < ext + ct->ext_table_length;
+	    cur += cur[CT_EXT_ENTRY_LEN]) {
+		switch (cur[CT_EXT_ENTRY_TYPE]) {
+		default:
+			printf("MPS: Skipping MP Configuration Table extended "
+			    "entry type %" PRIu8 "\n", cur[CT_EXT_ENTRY_TYPE]);
 		}
 	}
-
-	return;
-	
-fs_found:
-	printf("%p: MPS Floating Pointer Structure\n", fs);
-
-	if (fs->config_type == 0 && fs->configuration_table) {
-		if (fs->mpfib2 >> 7) {
-			printf("%s: PIC mode not supported\n", __func__);
-			return;
-		}
-
-		ct = (struct mps_ct *)PA2KA((uintptr_t)fs->configuration_table);
-		config.cpu_count = configure_via_ct();
-	} 
-	else
-		config.cpu_count = configure_via_default(fs->config_type);
-
-	return;
-}
-
-int configure_via_ct(void)
-{
-	uint8_t *cur;
-	unsigned int i, cnt;
-		
+}
+
+static void configure_via_ct(void)
+{
 	if (ct->signature != CT_SIGNATURE) {
-		printf("%s: bad ct->signature\n", __func__);
-		return 1;
-	}
+		printf("MPS: Wrong ct->signature\n");
+		return;
+	}
+	
 	if (!mps_ct_check()) {
-		printf("%s: bad ct checksum\n", __func__);
-		return 1;
-	}
+		printf("MPS: Wrong ct checksum\n");
+		return;
+	}
+	
 	if (ct->oem_table) {
-		printf("%s: ct->oem_table not supported\n", __func__);
-		return 1;
-	}
-	
-	l_apic = (uint32_t *)(uintptr_t)ct->l_apic;
-
-	cnt = 0;
-	cur = &ct->base_table[0];
+		printf("MPS: ct->oem_table not supported\n");
+		return;
+	}
+	
+	l_apic = (uint32_t *) (uintptr_t) ct->l_apic;
+	
+	uint8_t *cur = &ct->base_table[0];
+	uint16_t i;
+	
 	for (i = 0; i < ct->entry_count; i++) {
 		switch (*cur) {
-		/* Processor entry */
-		case 0:	
+		case 0:  /* Processor entry */
 			processor_entries = processor_entries ?
 			    processor_entries :
 			    (struct __processor_entry *) cur;
 			processor_entry_cnt++;
-			cnt += ct_processor_entry((struct __processor_entry *)
-			    cur);
+			ct_processor_entry((struct __processor_entry *) cur);
 			cur += 20;
 			break;
-
-		/* Bus entry */
-		case 1:
+		case 1:  /* Bus entry */
 			bus_entries = bus_entries ?
 			    bus_entries : (struct __bus_entry *) cur;
@@ -250,16 +377,12 @@
 			cur += 8;
 			break;
-				
-		/* I/O Apic */
-		case 2:
+		case 2:  /* I/O APIC */
 			io_apic_entries = io_apic_entries ?
 			    io_apic_entries : (struct __io_apic_entry *) cur;
-				io_apic_entry_cnt++;
+			io_apic_entry_cnt++;
 			ct_io_apic_entry((struct __io_apic_entry *) cur);
 			cur += 8;
 			break;
-				
-		/* I/O Interrupt Assignment */
-		case 3:
+		case 3:  /* I/O Interrupt Assignment */
 			io_intr_entries = io_intr_entries ?
 			    io_intr_entries : (struct __io_intr_entry *) cur;
@@ -268,7 +391,5 @@
 			cur += 8;
 			break;
-
-		/* Local Interrupt Assignment */
-		case 4:
+		case 4:  /* Local Interrupt Assignment */
 			l_intr_entries = l_intr_entries ?
 			    l_intr_entries : (struct __l_intr_entry *) cur;
@@ -277,12 +398,10 @@
 			cur += 8;
 			break;
-
 		default:
 			/*
 			 * Something is wrong. Fallback to UP mode.
 			 */
-
-			printf("%s: ct badness\n", __func__);
-			return 1;
+			printf("MPS: ct badness %" PRIu8 "\n", *cur);
+			return;
 		}
 	}
@@ -292,196 +411,57 @@
 	 */
 	ct_extended_entries();
-	return cnt;
-}
-
-int configure_via_default(uint8_t n __attribute__((unused)))
+}
+
+static void configure_via_default(uint8_t n __attribute__((unused)))
 {
 	/*
 	 * Not yet implemented.
 	 */
-	printf("%s: not supported\n", __func__);
-	return 1;
-}
-
-
-int ct_processor_entry(struct __processor_entry *pr __attribute__((unused)))
-{
+	printf("MPS: Default configuration not supported\n");
+}
+
+void mps_init(void)
+{
+	uint8_t *addr[2] = { NULL, (uint8_t *) PA2KA(0xf0000) };
+	unsigned int i;
+	unsigned int j;
+	unsigned int length[2] = { 1024, 64 * 1024 };
+	
 	/*
-	 * Ignore processors which are not marked enabled.
+	 * Find MP Floating Pointer Structure
+	 *  1a. search first 1K of EBDA
+	 *  1b. if EBDA is undefined, search last 1K of base memory
+	 *  2.  search 64K starting at 0xf0000
 	 */
-	if ((pr->cpu_flags & (1 << 0)) == 0)
-		return 0;
-	
-	apic_id_mask |= (1 << pr->l_apic_id); 
-	return 1;
-}
-
-void ct_bus_entry(struct __bus_entry *bus __attribute__((unused)))
-{
-#ifdef MPSCT_VERBOSE
-	char buf[7];
-	memcpy((void *) buf, (void *) bus->bus_type, 6);
-	buf[6] = 0;
-	printf("bus%d: %s\n", bus->bus_id, buf);
-#endif
-}
-
-void ct_io_apic_entry(struct __io_apic_entry *ioa)
-{
-	static unsigned int io_apic_count = 0;
-
-	/* this ioapic is marked unusable */
-	if ((ioa->io_apic_flags & 1) == 0)
-		return;
-	
-	if (io_apic_count++ > 0) {
-		/*
-		 * Multiple IO APIC's are currently not supported.
-		 */
-		return;
-	}
-	
-	io_apic = (uint32_t *)(uintptr_t)ioa->io_apic;
-}
-
-//#define MPSCT_VERBOSE
-void ct_io_intr_entry(struct __io_intr_entry *iointr __attribute__((unused)))
-{
-#ifdef MPSCT_VERBOSE
-	switch (iointr->intr_type) {
-	case 0:
-		printf("INT");
-		break;
-	case 1:
-		printf("NMI");
-		break;
-	case 2:
-		printf("SMI");
-		break;
-	case 3:
-		printf("ExtINT");
-		break;
-	}
-	putchar(',');
-	switch (iointr->poel & 3) {
-	case 0:
-		printf("bus-like");
-		break;
-	case 1:
-		printf("active high");
-		break;
-	case 2:
-		printf("reserved");
-		break;
-	case 3:
-		printf("active low");
-		break;
-	}
-	putchar(',');
-	switch ((iointr->poel >> 2) & 3) {
-	case 0:
-		printf("bus-like");
-		break;
-	case 1:
-		printf("edge-triggered");
-		break;
-	case 2:
-		printf("reserved");
-		break;
-	case 3:
-		printf("level-triggered");
-		break;
-	}
-	putchar(',');
-	printf("bus%d,irq%d", iointr->src_bus_id, iointr->src_bus_irq);
-	putchar(',');
-	printf("io_apic%d,pin%d", iointr->dst_io_apic_id,
-	    iointr->dst_io_apic_pin);
-	putchar('\n');	
-#endif
-}
-
-void ct_l_intr_entry(struct __l_intr_entry *lintr __attribute__((unused)))
-{
-#ifdef MPSCT_VERBOSE
-	switch (lintr->intr_type) {
-	case 0:
-	    printf("INT");
-	    break;
-	case 1:
-	    printf("NMI");
-	    break;
-	case 2:
-	    printf("SMI");
-	    break;
-	case 3:
-	    printf("ExtINT");
-	    break;
-	}
-	putchar(',');
-	switch (lintr->poel & 3) {
-	case 0:
-	    printf("bus-like");
-	    break;
-	case 1:
-	    printf("active high");
-	    break;
-	case 2:
-	    printf("reserved");
-	    break;
-	case 3:
-	    printf("active low");
-	    break;
-	}
-	putchar(',');
-	switch ((lintr->poel >> 2) & 3) {
-	case 0:
-	    printf("bus-like");
-	    break;
-	case 1:
-	    printf("edge-triggered");
-	    break;
-	case 2:
-	    printf("reserved");
-	    break;
-	case 3:
-	    printf("level-triggered");
-	    break;
-	}
-	putchar(',');
-	printf("bus%d,irq%d", lintr->src_bus_id, lintr->src_bus_irq);
-	putchar(',');
-	printf("l_apic%d,pin%d", lintr->dst_l_apic_id, lintr->dst_l_apic_pin);
-	putchar('\n');
-#endif
-}
-
-void ct_extended_entries(void)
-{
-	uint8_t *ext = (uint8_t *) ct + ct->base_table_length;
-	uint8_t *cur;
-
-	for (cur = ext; cur < ext + ct->ext_table_length;
-	    cur += cur[CT_EXT_ENTRY_LEN]) {
-		switch (cur[CT_EXT_ENTRY_TYPE]) {
-		default:
-			printf("%p: skipping MP Configuration Table extended "
-			    "entry type %d\n", cur, cur[CT_EXT_ENTRY_TYPE]);
-			break;
+	
+	addr[0] = (uint8_t *) PA2KA(ebda ? ebda : 639 * 1024);
+	for (i = 0; i < 2; i++) {
+		for (j = 0; j < length[i]; j += 16) {
+			if ((*((uint32_t *) &addr[i][j]) ==
+			    FS_SIGNATURE) && (mps_fs_check(&addr[i][j]))) {
+				fs = (struct mps_fs *) &addr[i][j];
+				goto fs_found;
+			}
 		}
 	}
-}
-
-int mps_irq_to_pin(unsigned int irq)
-{
-	unsigned int i;
-	
-	for (i = 0; i < io_intr_entry_cnt; i++) {
-		if (io_intr_entries[i].src_bus_irq == irq &&
-		    io_intr_entries[i].intr_type == 0)
-			return io_intr_entries[i].dst_io_apic_pin;
-	}
-	
-	return -1;
+	
+	return;
+	
+fs_found:
+	printf("%p: MPS Floating Pointer Structure\n", fs);
+	
+	if ((fs->config_type == 0) && (fs->configuration_table)) {
+		if (fs->mpfib2 >> 7) {
+			printf("MPS: PIC mode not supported\n");
+			return;
+		}
+		
+		ct = (struct mps_ct *) PA2KA((uintptr_t) fs->configuration_table);
+		configure_via_ct();
+	} else
+		configure_via_default(fs->config_type);
+	
+	if (processor_entry_cnt > 0)
+		config.cpu_count = processor_entry_cnt;
 }
 
