2011-08-01 Jennifer Averett <Jennifer.Averett@OARcorp.com>

	PR 1802
	* shared/irq/irq_asm.S, shared/irq/irq_init.c, shared/smp/smp-imps.c,
	shared/smp/smp-imps.h: Add SMP support for i386.
	* shared/smp/getcpuid.c: New file.

6 files changed, 358 insertions, 141 deletions

diff --git a/c/src/lib/libbsp/i386/ChangeLog b/c/src/lib/libbsp/i386/ChangeLog
index 0a8ce44670..c093bca7f2 100644
--- a/c/src/lib/libbsp/i386/ChangeLog
+++ b/c/src/lib/libbsp/i386/ChangeLog
@@ -1,3 +1,10 @@
+2011-08-01	Jennifer Averett <Jennifer.Averett@OARcorp.com>
+
+	PR 1802
+	* shared/irq/irq_asm.S, shared/irq/irq_init.c, shared/smp/smp-imps.c,
+	shared/smp/smp-imps.h: Add SMP support for i386.
+	* shared/smp/getcpuid.c: New file.
+
 2011-07-18	Joel Sherrill <joel.sherrill@oarcorp.com>
 
 	* shared/irq/apic.h, shared/smp/smp-imps.c, shared/smp/smp-imps.h:
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq_asm.S b/c/src/lib/libbsp/i386/shared/irq/irq_asm.S
index edd7cea6cb..e644e7587c 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq_asm.S
+++ b/c/src/lib/libbsp/i386/shared/irq/irq_asm.S
@@ -32,7 +32,7 @@
 #define EBP_OFF 12       /* code restoring ebp/esp relies on */
 #define ESP_OFF 16       /* esp being on top of ebp!         */
 #ifdef __SSE__
-/* need to be on 16 byte boundary for SSE */
+/* need to be on 16 byte boundary for SSE, add 12 to do that */
 #define FRM_SIZ (20+12+512)
 #define SSE_OFF 32
 #else
@@ -46,17 +46,9 @@ SYM (_ISR_Handler):
 	 *  Before this was point is reached the vectors unique
 	 *  entry point did the following:
 	 *
-	 *     1. saved scratch registers registers eax edx ecx"
+	 *     1. saved scratch registers registers eax edx ecx
 	 *     2. put the vector number in ecx.
 	 *
-	 * BEGINNING OF ESTABLISH SEGMENTS
-	 *
-	 *  WARNING: If an interrupt can occur when the segments are
-	 *           not correct, then this is where we should establish
-	 *           the segments.  In addition to establishing the
-	 *           segments, it may be necessary to establish a stack
-	 *           in the current data area on the outermost interrupt.
-	 *
 	 *  NOTE:  If the previous values of the segment registers are
 	 *         pushed, do not forget to adjust SAVED_REGS.
 	 *
@@ -104,9 +96,48 @@ SYM (_ISR_Handler):
 	ldmxcsr ARG_OFF(esp)            /* clean-slate MXCSR              */
 #endif
 
+.check_stack_switch:
+	movl      esp, ebp                  /* ebp = previous stack pointer */
+#if defined(RTEMS_SMP) && defined(BSP_HAS_SMP)
+	movl     $SYM(_Per_CPU_Information_p), ebx
+	call     SYM(bsp_smp_processor_id)
+	mov      (ebx,eax,4), ebx
+        pushl    ecx
+        call     SYM(_ISR_SMP_Enter)
+        popl     ecx
+        cmpl     $0, eax
+        jne      .i8259
+        movl     PER_CPU_INTERRUPT_STACK_HIGH(ebx), esp
+       
+#else
+	movl     $SYM(_Per_CPU_Information), ebx
+
+	/*
+         *  Is this the outermost interrupt?
+	 *  Switch stacks if necessary
+	 */
+	cmpl      $0, PER_CPU_ISR_NEST_LEVEL(ebx)
+	jne       nested                    /* No, then continue */
+	movl      PER_CPU_INTERRUPT_STACK_HIGH(ebx), esp
+
+	/*
+	 *  We want to insure that the old stack pointer is in ebp
+	 *  By saving it on every interrupt, all we have to do is
+	 *  movl ebp->esp near the end of every interrupt.
+	 */
+
+nested:
+	incl      PER_CPU_ISR_NEST_LEVEL(ebx)  /* one nest level deeper */
+	incl      SYM (_Thread_Dispatch_disable_level) /* disable multitasking */
+#endif
+        /*
+         *  i8259 Management
+         */
+
+.i8259:
         /* Do not disable any 8259 interrupts if this isn't from one */
 	cmp       ecx, 16               /* is this a PIC IRQ? */
-	jge       .check_stack_switch
+	jge       .end_of_i8259
 
 	/*
 	 * acknowledge the interrupt
@@ -136,44 +167,7 @@ SYM (_ISR_Handler):
 	outb      $PIC_SLAVE_COMMAND_IO_PORT
 .master:
 	outb      $PIC_MASTER_COMMAND_IO_PORT
-
-	/*
-	 *  Now switch stacks if necessary
-	 */
-
-PUBLIC (ISR_STOP)
-ISR_STOP:
-.check_stack_switch:
-	movl      esp, ebp                  /* ebp = previous stack pointer */
-#if defined(RTEMS_SMP) && defined(BSP_HAS_SMP)
-	movl     $SYM(_Per_CPU_Information_p), ebx
-	call     SYM(bsp_smp_processor_id)
-	mov      (ebx,eax,4), ebx
-#else
-	movl     $SYM(_Per_CPU_Information), ebx
-#endif
-
-	/* is this the outermost interrupt? */
-	cmpl      $0, PER_CPU_ISR_NEST_LEVEL(ebx)
-	jne       nested                    /* No, then continue */
-	movl      PER_CPU_INTERRUPT_STACK_HIGH(ebx), esp
-
-	/*
-	 *  We want to insure that the old stack pointer is in ebp
-	 *  By saving it on every interrupt, all we have to do is
-	 *  movl ebp->esp near the end of every interrupt.
-	 */
-
-nested:
-	incl      PER_CPU_ISR_NEST_LEVEL(ebx)  /* one nest level deeper */
-	incl      SYM (_Thread_Dispatch_disable_level) /* disable multitasking */
-
-        /*
-         * GCC versions starting with 4.3 no longer place the cld
-         * instruction before string operations.  We  need to ensure
-         * it is set correctly for ISR handlers.
-         */
-        cld
+.end_of_i8259:
 
 	/*
 	 * re-enable interrupts at processor level as the current
@@ -216,8 +210,14 @@ nested:
 	outb	  $PIC_MASTER_IMR_IO_PORT
 	movb	  ah, al
 	outb	  $PIC_SLAVE_IMR_IO_PORT
-
 .dont_restore_i8259:
+
+
+#if defined(RTEMS_SMP) && defined(BSP_HAS_SMP)
+        call    SYM(_ISR_SMP_Exit)
+	testl	eax, eax
+	je	.exit
+#else
 	decl      PER_CPU_ISR_NEST_LEVEL(ebx)  /* one less ISR nest level */
 	                                    /* If interrupts are nested, */
 	                                    /*   then dispatching is disabled */
@@ -231,6 +231,7 @@ nested:
 	                                    /* Is task switch necessary? */
 	jne       .schedule                 /* Yes, then call the scheduler */
 	jmp       .exit                     /* No, exit */
+#endif
 
 .schedule:
 	/*
diff --git a/c/src/lib/libbsp/i386/shared/irq/irq_init.c b/c/src/lib/libbsp/i386/shared/irq/irq_init.c
index 798d57a35b..b2b9b92a78 100644
--- a/c/src/lib/libbsp/i386/shared/irq/irq_init.c
+++ b/c/src/lib/libbsp/i386/shared/irq/irq_init.c
@@ -6,6 +6,9 @@
  *  Copyright (c) 2009 embedded brains GmbH
  *  CopyRight (C) 1998 valette@crf.canon.fr
  *
+ *  COPYRIGHT (c) 2011.
+ *  On-Line Applications Research Corporation (OAR).
+ *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
  *  http://www.rtems.com/license/LICENSE.
diff --git a/c/src/lib/libbsp/i386/shared/smp/getcpuid.c b/c/src/lib/libbsp/i386/shared/smp/getcpuid.c
new file mode 100644
index 0000000000..59af658127
--- /dev/null
+++ b/c/src/lib/libbsp/i386/shared/smp/getcpuid.c
@@ -0,0 +1,22 @@
+/*
+ *  COPYRIGHT (c) 2011.
+ *  On-Line Applications Research Corporation (OAR).
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *  $Id$
+ */
+
+#include <bsp/apic.h>
+#include <bsp/smp-imps.h>
+
+static int lapic_dummy = 0;
+unsigned imps_lapic_addr = ((unsigned)(&lapic_dummy)) - LAPIC_ID;
+
+int  bsp_smp_processor_id(void)
+{
+  return APIC_ID(IMPS_LAPIC_READ(LAPIC_ID));
+}
+
diff --git a/c/src/lib/libbsp/i386/shared/smp/smp-imps.c b/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
index f07ab48e45..f1be625a31 100644
--- a/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
+++ b/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
@@ -47,6 +47,16 @@
 #define _SMP_IMPS_C
 
 /*
+ *  Includes here
+ */
+#if 0
+#define IMPS_DEBUG
+#endif
+
+#include <bsp/apic.h>
+#include <bsp/smp-imps.h>
+
+/*
  *  XXXXX  The following absolutely must be defined!!!
  *
  *  The "KERNEL_PRINT" could be made a null macro with no danger, of
@@ -63,17 +73,69 @@
 #define UDELAY(x)             /* delay roughly at least "x" microsecs */
 #define TEST_BOOTED(x)        /* test bootaddr x to see if CPU started */
 #define READ_MSR_LO(x)        /* Read MSR low function */
-#endif
+#else
+#include <string.h>
+#include <unistd.h>
+#include <rtems.h>
+#include <rtems/bspsmp.h>
+#include <rtems/bspIo.h>
+#include <libcpu/cpu.h>
+
+extern void _pc386_delay(void);
+
+/* #define KERNEL_PRINT(_format)       printk(_format) */
+
+static void CMOS_WRITE_BYTE(
+  unsigned int  offset,
+  unsigned char value
+)
+{
+  if ( offset < 128 ) {
+    outport_byte( 0x70, offset );
+    outport_byte( 0x71, value );
+  } else {
+    outport_byte( 0x72, offset );
+    outport_byte( 0x73, value );
+  }
+}
 
-/*
- *  Includes here
- */
+static unsigned char CMOS_READ_BYTE(
+  unsigned int  offset
+)
+{
+  unsigned char value;
+  if ( offset < 128 ) {
+    outport_byte( 0x70, offset );
+    inport_byte( 0x71, value );
+  } else {
+    outport_byte( 0x72, offset );
+    inport_byte( 0x73, value );
+  }
+  return value;
+}
 
-#define IMPS_DEBUG
+#define PHYS_TO_VIRTUAL(_x)    _x
+#define VIRTUAL_TO_PHYS(_x)    _x
+static void UDELAY(int x)
+{ int _i = x;
+  while ( _i-- )
+    _pc386_delay();
+}
+ 
+#define READ_MSR_LO(_x) \
+  (unsigned int)(read_msr(_x) & 0xffffffff)
 
-#include "apic.h"
-#include "smp-imps.h"
+#define TEST_BOOTED(_cpu) \
+  (_Per_CPU_Information[_cpu].state == RTEMS_BSP_SMP_CPU_INITIALIZED)
 
+static inline unsigned long long read_msr(unsigned int msr)
+{
+  unsigned long long value;
+ 
+  asm volatile("rdmsr" : "=A" (value) : "c" (msr));
+  return value;
+}
+#endif
 
 /*
  *  Defines that are here so as not to be in the global header file.
@@ -92,7 +154,6 @@
 
 #define DEF_ENTRIES  23
 
-static int lapic_dummy = 0;
 static struct {
   imps_processor proc[2];
   imps_bus bus[2];
@@ -132,10 +193,12 @@ static struct {
 volatile int imps_release_cpus = 0;
 int imps_enabled = 0;
 int imps_num_cpus = 1;
-unsigned imps_lapic_addr = ((unsigned)(&lapic_dummy)) - LAPIC_ID;
 unsigned char imps_cpu_apic_map[IMPS_MAX_CPUS];
 unsigned char imps_apic_cpu_map[IMPS_MAX_CPUS];
 
+/* now defined in getcpuid.c */
+extern unsigned imps_lapic_addr;
+
 /*
  *  MPS checksum function
  *
@@ -180,10 +243,10 @@ send_ipi(unsigned int dst, unsigned int v)
  *  This must be modified to perform whatever OS-specific initialization
  *  that is required.
  */
-static int
+int
 boot_cpu(imps_processor *proc)
 {
-  int apicid = proc->apic_id, success = 1, to;
+  int apicid = proc->apic_id, success = 1;
   unsigned bootaddr, accept_status;
   unsigned bios_reset_vector = PHYS_TO_VIRTUAL(BIOS_RESET_VECTOR);
 
@@ -195,10 +258,19 @@ boot_cpu(imps_processor *proc)
    * under the 1MB boundary.
    */
 
-  extern char patch_code_start[];
-  extern char patch_code_end[];
+  uint32_t *reset;
+
   bootaddr = (512-64)*1024;
-  memcpy((char *)bootaddr, patch_code_start, patch_code_end - patch_code_start);
+  reset= (uint32_t *)bootaddr;
+
+  memcpy(
+    (char *) bootaddr,
+    _binary_appstart_bin_start,
+    (size_t)_binary_appstart_bin_size
+  );
+
+  reset[1] = (uint32_t)rtems_smp_secondary_cpu_initialize;
+  reset[2] = (uint32_t)_Per_CPU_Information[apicid].interrupt_stack_high;
 
   /*
    *  Generic CPU startup sequence starts here.
@@ -214,8 +286,9 @@ boot_cpu(imps_processor *proc)
 
   /* assert INIT IPI */
   send_ipi(
-    apicid, LAPIC_ICR_TM_LEVEL | LAPIC_ICR_LEVELASSERT | LAPIC_ICR_DM_INIT);
-
+    apicid,
+    LAPIC_ICR_TM_LEVEL | LAPIC_ICR_LEVELASSERT | LAPIC_ICR_DM_INIT
+  );
   UDELAY(10000);
 
   /* de-assert INIT IPI */
@@ -238,14 +311,17 @@ boot_cpu(imps_processor *proc)
   /*
    *  Check to see if other processor has started.
    */
-  to = 0;
-  while (!TEST_BOOTED(bootaddr) && to++ < 100)
-    UDELAY(10000);
-  if (to >= 100) {
-    KERNEL_PRINT(("CPU Not Responding, DISABLED"));
+  bsp_smp_wait_for(
+    (volatile unsigned int *)&_Per_CPU_Information[imps_num_cpus].state,
+    RTEMS_BSP_SMP_CPU_INITIALIZED,
+    1600
+  );
+  if ( _Per_CPU_Information[imps_num_cpus].state ==
+        RTEMS_BSP_SMP_CPU_INITIALIZED )
+    printk("#%d  Application Processor (AP)", imps_num_cpus);
+  else {
+    printk("CPU Not Responding, DISABLED");
     success = 0;
-  } else {
-    KERNEL_PRINT(("#%d  Application Processor (AP)", imps_num_cpus));
   }
 
   /*
@@ -260,7 +336,7 @@ boot_cpu(imps_processor *proc)
   CMOS_WRITE_BYTE(CMOS_RESET_CODE, 0);
   *((volatile unsigned *) bios_reset_vector) = 0;
 
-  KERNEL_PRINT(("\n"));
+  printk("\n");
 
   return success;
 }
@@ -273,14 +349,13 @@ add_processor(imps_processor *proc)
 {
   int apicid = proc->apic_id;
 
-  KERNEL_PRINT(("  Processor [APIC id %d ver %d]:  ",
-          apicid, proc->apic_ver));
+  printk("  Processor [APIC id %d ver %d]: ", apicid, proc->apic_ver);
   if (!(proc->flags & IMPS_FLAG_ENABLED)) {
-    KERNEL_PRINT(("DISABLED\n"));
+    printk("DISABLED\n");
     return;
   }
   if (proc->flags & (IMPS_CPUFLAG_BOOT)) {
-    KERNEL_PRINT(("#0  BootStrap Processor (BSP)\n"));
+    printk("#0  BootStrap Processor (BSP)\n");
     return;
   }
   if (boot_cpu(proc)) {
@@ -301,7 +376,7 @@ add_bus(imps_bus *bus)
 
   memcpy(str, bus->bus_type, 6);
   str[6] = 0;
-  KERNEL_PRINT(("  Bus id %d is %s\n", bus->id, str));
+  printk("  Bus id %d is %s\n", bus->id, str);
 
   /*  XXXXX  add OS-specific code here */
 }
@@ -309,13 +384,13 @@ add_bus(imps_bus *bus)
 static void
 add_ioapic(imps_ioapic *ioapic)
 {
-  KERNEL_PRINT(("  I/O APIC id %d ver %d, address: 0x%x  ",
-          ioapic->id, ioapic->ver, ioapic->addr));
+  printk("  I/O APIC id %d ver %d, address: 0x%x  ",
+          ioapic->id, ioapic->ver, ioapic->addr);
   if (!(ioapic->flags & IMPS_FLAG_ENABLED)) {
-    KERNEL_PRINT(("DISABLED\n"));
+    printk("DISABLED\n");
     return;
   }
-  KERNEL_PRINT(("\n"));
+  printk("\n");
 
   /*  XXXXX  add OS-specific code here */
 }
@@ -326,7 +401,10 @@ imps_read_config_table(unsigned start, int count)
   while (count-- > 0) {
     switch (*((unsigned char *)start)) {
     case IMPS_BCT_PROCESSOR:
-      add_processor((imps_processor *)start);
+      if ( imps_num_cpus < rtems_configuration_smp_maximum_processors ) {
+	add_processor((imps_processor *)start);
+      } else 
+        imps_num_cpus++;
       start += 12;  /* 20 total */
       break;
     case IMPS_BCT_BUS:
@@ -350,6 +428,15 @@ imps_read_config_table(unsigned start, int count)
     }
     start += 8;
   }
+  if ( imps_num_cpus > rtems_configuration_smp_maximum_processors ) {
+    printk(
+      "WARNING!! Found more CPUs (%d) than configured for (%d)!!\n",
+      imps_num_cpus - 1,
+      rtems_configuration_smp_maximum_processors
+    );
+    imps_num_cpus = rtems_configuration_smp_maximum_processors;
+    return;
+  }
 }
 
 static int
@@ -360,8 +447,8 @@ imps_bad_bios(imps_fps *fps_ptr)
     = (imps_cth *) PHYS_TO_VIRTUAL(fps_ptr->cth_ptr);
 
   if (fps_ptr->feature_info[0] > IMPS_FPS_DEFAULT_MAX) {
-    KERNEL_PRINT(("    Invalid MP System Configuration type %d\n",
-            fps_ptr->feature_info[0]));
+    printk("    Invalid MP System Configuration type %d\n",
+            fps_ptr->feature_info[0]);
     return 1;
   }
 
@@ -369,17 +456,17 @@ imps_bad_bios(imps_fps *fps_ptr)
     sum = get_checksum((unsigned)local_cth_ptr,
                                    local_cth_ptr->base_length);
     if (local_cth_ptr->sig != IMPS_CTH_SIGNATURE || sum) {
-      KERNEL_PRINT(
-        ("    Bad MP Config Table sig 0x%x and/or checksum 0x%x\n",
+      printk(
+        "    Bad MP Config Table sig 0x%x and/or checksum 0x%x\n",
         (unsigned)(fps_ptr->cth_ptr),
-        sum)
+        sum
       );
       return 1;
     }
     if (local_cth_ptr->spec_rev != fps_ptr->spec_rev) {
-      KERNEL_PRINT(
-        ("    Bad MP Config Table sub-revision # %d\n",
-        local_cth_ptr->spec_rev)
+      printk(
+        "    Bad MP Config Table sub-revision # %d\n",
+        local_cth_ptr->spec_rev
       );
       return 1;
     }
@@ -389,12 +476,12 @@ imps_bad_bios(imps_fps *fps_ptr)
               local_cth_ptr->extended_length)
              + local_cth_ptr->extended_checksum) & 0xFF;
       if (sum) {
-        KERNEL_PRINT(("    Bad Extended MP Config Table checksum 0x%x\n", sum));
+        printk("    Bad Extended MP Config Table checksum 0x%x\n", sum);
         return 1;
       }
     }
   } else if (!fps_ptr->feature_info[0]) {
-    KERNEL_PRINT(("    Missing configuration information\n"));
+    printk("    Missing configuration information\n");
     return 1;
   }
 
@@ -410,15 +497,15 @@ imps_read_bios(imps_fps *fps_ptr)
     = (imps_cth *)PHYS_TO_VIRTUAL(fps_ptr->cth_ptr);
   char *str_ptr;
 
-  KERNEL_PRINT(("Intel MultiProcessor Spec 1.%d BIOS support detected\n",
-          fps_ptr->spec_rev));
+  printk("Intel MultiProcessor Spec 1.%d BIOS support detected\n",
+          fps_ptr->spec_rev);
 
   /*
    *  Do all checking of errors which would definitely
    *  lead to failure of the SMP boot here.
    */
   if (imps_bad_bios(fps_ptr)) {
-    KERNEL_PRINT(("    Disabling MPS support\n"));
+    printk("    Disabling MPS support\n");
     return;
   }
 
@@ -432,10 +519,10 @@ imps_read_bios(imps_fps *fps_ptr)
   } else {
     imps_lapic_addr = LAPIC_ADDR_DEFAULT;
   }
-  KERNEL_PRINT(("    APIC config: \"%s mode\"    Local APIC address: 0x%x\n",
-          str_ptr, imps_lapic_addr));
+  printk("    APIC config: \"%s mode\"    Local APIC address: 0x%x\n",
+          str_ptr, imps_lapic_addr);
   if (imps_lapic_addr != (READ_MSR_LO(0x1b) & 0xFFFFF000)) {
-    KERNEL_PRINT(("Inconsistent Local APIC address, Disabling SMP support\n"));
+    printk("Inconsistent Local APIC address, Disabling SMP support\n");
     return;
   }
   imps_lapic_addr = PHYS_TO_VIRTUAL(imps_lapic_addr);
@@ -455,7 +542,7 @@ imps_read_bios(imps_fps *fps_ptr)
     str1[8] = 0;
     memcpy(str2, local_cth_ptr->prod_id, 12);
     str2[12] = 0;
-    KERNEL_PRINT(("  OEM id: %s  Product id: %s\n", str1, str2));
+    printk("  OEM id: %s  Product id: %s\n", str1, str2);
     cth_start = ((unsigned) local_cth_ptr) + sizeof(imps_cth);
     cth_count = local_cth_ptr->entry_count;
   } else {
@@ -518,8 +605,7 @@ imps_read_bios(imps_fps *fps_ptr)
 static int
 imps_scan(unsigned start, unsigned length)
 {
-  IMPS_DEBUG_PRINT(("Scanning from 0x%x for %d bytes\n",
-        start, length));
+  printk("Scanning from 0x%x for %d bytes\n", start, length);
 
   while (length > 0) {
     imps_fps *fps_ptr = (imps_fps *) PHYS_TO_VIRTUAL(start);
@@ -528,7 +614,7 @@ imps_scan(unsigned start, unsigned length)
      && fps_ptr->length == 1
      && (fps_ptr->spec_rev == 1 || fps_ptr->spec_rev == 4)
      && !get_checksum(start, 16)) {
-      IMPS_DEBUG_PRINT(("Found MP Floating Structure Pointer at %x\n", start));
+      printk("Found MP Floating Structure Pointer at %x\n", start);
       imps_read_bios(fps_ptr);
       return 1;
     }
@@ -540,6 +626,7 @@ imps_scan(unsigned start, unsigned length)
   return 0;
 }
 
+#if !defined(__rtems__)
 /*
  *  This is the primary function to "force" SMP support, with
  *  the assumption that you have consecutively numbered APIC ids.
@@ -550,7 +637,7 @@ imps_force(int ncpus)
   int apicid, i;
   imps_processor p;
 
-  KERNEL_PRINT(("Intel MultiProcessor \"Force\" Support\n"));
+  printk("Intel MultiProcessor \"Force\" Support\n");
 
   imps_lapic_addr = (READ_MSR_LO(0x1b) & 0xFFFFF000);
   imps_lapic_addr = PHYS_TO_VIRTUAL(imps_lapic_addr);
@@ -580,6 +667,7 @@ imps_force(int ncpus)
 
   return imps_num_cpus;
 }
+#endif
 
 /*
  *  This is the primary function for probing for MPS compatible hardware
@@ -661,3 +749,116 @@ imps_probe(void)
   return 0;
 }
 
+/*
+ *  RTEMS SMP BSP Support
+ */
+void smp_apic_ack(void)
+{
+  (void) IMPS_LAPIC_READ(LAPIC_SPIV);  /* dummy read */
+  IMPS_LAPIC_WRITE(LAPIC_EOI, 0 );     /* ACK the interrupt */
+}
+
+rtems_isr ap_ipi_isr(
+  rtems_vector_number vector
+)
+{
+  smp_apic_ack();
+
+  rtems_smp_process_interrupt();
+}
+
+#include <rtems/irq.h>
+
+static rtems_irq_connect_data apIPIIrqData = {
+  16,
+  (void *)ap_ipi_isr,
+  0,
+  NULL,            /* On */
+  NULL,            /* Off */
+  NULL,            /* IsOn */
+};
+
+extern void bsp_reset(void);
+void ipi_install_irq(void)
+{
+  if (!BSP_install_rtems_irq_handler (&apIPIIrqData)) {
+    printk("Unable to initialize IPI\n");
+    bsp_reset();
+  }
+}
+
+#ifdef __SSE__
+extern void enable_sse(void);
+#endif
+
+/* pc386 specific initialization */
+void bsp_smp_secondary_cpu_initialize(int cpu)
+{
+  int apicid;
+
+  asm volatile( "lidt IDT_Descriptor" );
+
+  apicid = IMPS_LAPIC_READ(LAPIC_SPIV);
+  IMPS_LAPIC_WRITE(LAPIC_SPIV, apicid|LAPIC_SPIV_ENABLE_APIC);
+
+#ifdef __SSE__
+  enable_sse();
+#endif
+}
+
+#include <rtems/bspsmp.h>
+int bsp_smp_initialize(
+  int maximum
+)
+{
+  int cores;
+  /* XXX need to deal with finding too many cores */
+
+  cores = imps_probe();
+
+  if ( cores > 1 )
+    ipi_install_irq();
+  return cores;
+}
+
+void bsp_smp_interrupt_cpu(
+  int cpu
+)
+{
+  send_ipi( cpu, 0x30 );
+}
+
+void bsp_smp_broadcast_interrupt(void)
+{
+  /* Single broadcast interrupt */
+  send_ipi( 0, LAPIC_ICR_DS_ALLEX | 0x30 );
+}
+
+void bsp_smp_wait_for(
+  volatile unsigned int *address,
+  unsigned int           desired,
+  int                    maximum_usecs
+)
+{
+  int iterations;
+  volatile int i;
+  volatile unsigned int *p = (volatile unsigned int *)address;
+
+  for (iterations=0 ;  iterations < maximum_usecs ; iterations++ ) {
+    if ( *p == desired )
+      break;
+    #ifdef __SSE3__
+      __builtin_ia32_monitor( (const void *)address, 0, 0 );
+      if ( *p == desired )
+        break;
+      __builtin_ia32_mwait( 0, 0 );
+    #endif
+
+    /*
+     *  Until i386 ms delay does not depend upon the clock we
+     *  will use this less sophisticated delay. 
+     */
+    for(i=5000; i>0; i--)
+      ;
+  }
+}
diff --git a/c/src/lib/libbsp/i386/shared/smp/smp-imps.h b/c/src/lib/libbsp/i386/shared/smp/smp-imps.h
index 809f6a29ec..394f147438 100644
--- a/c/src/lib/libbsp/i386/shared/smp/smp-imps.h
+++ b/c/src/lib/libbsp/i386/shared/smp/smp-imps.h
@@ -37,6 +37,22 @@
  *  Intel literature center.
  */
 
+/*
+ *  This file is based upon code by Eric Boleyn as documented above.  
+ *  RTEMS support was added and minimal other changes were made.  
+ *  This should make it easier to compare this file with the original 
+ *  version.
+ *
+ *  COPYRIGHT (c) 2011.
+ *  On-Line Applications Research Corporation (OAR).
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *  $Id$
+ */
+
 #ifndef _SMP_IMPS_H
 #define _SMP_IMPS_H
 
@@ -194,47 +210,14 @@ struct imps_interrupt
  */
 
 /*
- *  "imps_enabled" is non-zero if the probe sequence found IMPS
- *  information and was successful.
- */
-extern int imps_enabled;
-
-/*
- *  This contains the local APIC hardware address.
- */
-extern unsigned imps_lapic_addr;
-
-/*
- *  This represents the number of CPUs found.
- */
-extern int imps_num_cpus;
-
-/*
  *  These map from virtual cpu numbers to APIC id's and back.
  */
 extern unsigned char imps_cpu_apic_map[IMPS_MAX_CPUS];
 extern unsigned char imps_apic_cpu_map[IMPS_MAX_CPUS];
 
-/*
- *  This is the primary function for probing for Intel MPS 1.1/1.4
- *  compatible hardware and BIOS information.  While probing the CPUs
- *  information returned from the BIOS, this also starts up each CPU
- *  and gets it ready for use.
- *
- *  Call this during the early stages of OS startup, before memory can
- *  be messed up.
- *
- *  Returns N if IMPS information was found (for number of CPUs started)
- *  and is valid, else 0.
- */
-int imps_probe(void);
-
-/*
- *  This one is used as a "force" function.  Give it the number of CPUs
- *  to start, and it will assume a certain number and try it.
- */
-int imps_force(int ncpus);
-
+/* base address of application processor reset code at 0x70000 */
+extern char _binary_appstart_bin_start[];
+extern char _binary_appstart_bin_size[];
 
 /*
  *  Defines that use variables