1 files changed, 49 insertions, 46 deletions

diff --git a/bsps/i386/pc386/start/smp-imps.c b/bsps/i386/pc386/start/smp-imps.c
index 0543b17ec5..763ac0afc7 100644
--- a/bsps/i386/pc386/start/smp-imps.c
+++ b/bsps/i386/pc386/start/smp-imps.c
@@ -83,6 +83,10 @@
 #include <assert.h>
 
 extern void _pc386_delay(void);
+extern uint32_t* gdtdesc;
+
+static int lapic_dummy = 0;
+unsigned imps_lapic_addr = ((unsigned)(&lapic_dummy)) - LAPIC_ID;
 
 /* #define KERNEL_PRINT(_format)       printk(_format) */
 
@@ -219,12 +223,14 @@ get_checksum(unsigned start, int length)
 /*
  *  APIC ICR write and status check function.
  */
-static int
+int
 send_ipi(unsigned int dst, unsigned int v)
 {
-  int to, send_status;
+  int to, send_status, apicid;
 
-  IMPS_LAPIC_WRITE(LAPIC_ICR+0x10, (dst << 24));
+  apicid = imps_cpu_apic_map[dst];
+
+  IMPS_LAPIC_WRITE(LAPIC_ICR+0x10, (apicid << 24));
   IMPS_LAPIC_WRITE(LAPIC_ICR, v);
 
   /* Wait for send to finish */
@@ -247,9 +253,11 @@ static int
 boot_cpu(imps_processor *proc)
 {
   int apicid = proc->apic_id, success = 1;
+  int cpuid;
   unsigned bootaddr;
   unsigned bios_reset_vector = PHYS_TO_VIRTUAL(BIOS_RESET_VECTOR);
 
+  cpuid = imps_apic_cpu_map[apicid];
   /*
    * Copy boot code for secondary CPUs here.  Find it in between
    * "patch_code_start" and "patch_code_end" symbols.  The other CPUs
@@ -258,10 +266,10 @@ boot_cpu(imps_processor *proc)
    * under the 1MB boundary.
    */
 
-  uint32_t *reset;
+  volatile uint32_t *reset;
 
   bootaddr = (512-64)*1024;
-  reset= (uint32_t *)bootaddr;
+  reset= (volatile uint32_t *)bootaddr;
 
   memcpy(
     (char *) bootaddr,
@@ -269,9 +277,14 @@ boot_cpu(imps_processor *proc)
     (size_t)_binary_appstart_bin_size
   );
 
+  /* Pass start function, stack region and gdtdescr to AP
+   * see startAP.S for location */
   reset[1] = (uint32_t)secondary_cpu_initialize;
-  reset[2] = (uint32_t)_Per_CPU_Get_by_index(apicid)->interrupt_stack_high;
-
+  reset[2] = (uint32_t)_Per_CPU_Get_by_index(cpuid)->interrupt_stack_high;
+  memcpy(
+  	(char*) &reset[3],
+  	&gdtdesc,
+  	6);
   /*
    *  Generic CPU startup sequence starts here.
    */
@@ -286,13 +299,13 @@ boot_cpu(imps_processor *proc)
 
   /* assert INIT IPI */
   send_ipi(
-    apicid,
+    cpuid,
     LAPIC_ICR_TM_LEVEL | LAPIC_ICR_LEVELASSERT | LAPIC_ICR_DM_INIT
   );
   UDELAY(10000);
 
   /* de-assert INIT IPI */
-  send_ipi(apicid, LAPIC_ICR_TM_LEVEL | LAPIC_ICR_DM_INIT);
+  send_ipi(cpuid, LAPIC_ICR_TM_LEVEL | LAPIC_ICR_DM_INIT);
 
   UDELAY(10000);
 
@@ -303,12 +316,17 @@ boot_cpu(imps_processor *proc)
   if (proc->apic_ver >= APIC_VER_NEW) {
     int i;
     for (i = 1; i <= 2; i++) {
-      send_ipi(apicid, LAPIC_ICR_DM_SIPI | ((bootaddr >> 12) & 0xFF));
+      send_ipi(cpuid, LAPIC_ICR_DM_SIPI | ((bootaddr >> 12) & 0xFF));
       UDELAY(1000);
     }
   }
 
   /*
+   *  Wait until AP is in protected mode before starting the next AP
+   */
+  while (reset[2] != 0);
+
+  /*
    *  Generic CPU startup sequence ends here, the rest is cleanup.
    */
 
@@ -320,8 +338,6 @@ boot_cpu(imps_processor *proc)
   CMOS_WRITE_BYTE(CMOS_RESET_CODE, 0);
   *((volatile unsigned *) bios_reset_vector) = 0;
 
-  printk("\n");
-
   return success;
 }
 
@@ -342,13 +358,19 @@ add_processor(imps_processor *proc)
     printk("#0  BootStrap Processor (BSP)\n");
     return;
   }
+  /* Setup the apic/cpu maps before booting the APs
+   * otherwise calls to _Get_current_processor can deliver
+   * wrong values if the BSP gets interrupted
+   */
+  imps_cpu_apic_map[imps_num_cpus] = apicid;
+  imps_apic_cpu_map[apicid] = imps_num_cpus;
   if (boot_cpu(proc)) {
 
     /*  XXXXX  add OS-specific setup for secondary CPUs here */
 
-    imps_cpu_apic_map[imps_num_cpus] = apicid;
-    imps_apic_cpu_map[apicid] = imps_num_cpus;
+    /* AP booted successfully, increase number of available cores */
     imps_num_cpus++;
+    printk("#%d  Application Processor (AP)\n", imps_apic_cpu_map[apicid]);
   }
 }
 
@@ -683,7 +705,7 @@ imps_force(int ncpus)
  *
  *  Function finished.
  */
-static int
+int
 imps_probe(void)
 {
   /*
@@ -750,10 +772,21 @@ static void bsp_inter_processor_interrupt(void *arg)
 
   smp_apic_ack();
 
+  /*
+   * Disallow nesting.
+   */
+   __asm__ __volatile__("cli");
+
   _SMP_Inter_processor_interrupt_handler(_Per_CPU_Get());
+
+   /*
+   * Allow nesting.
+   */
+   __asm__ __volatile__("sti");
 }
 
-static void ipi_install_irq(void)
+void
+ipi_install_irq(void)
 {
   rtems_status_code status;
 
@@ -787,33 +820,3 @@ static void secondary_cpu_initialize(void)
 
   _SMP_Start_multitasking_on_secondary_processor( _Per_CPU_Get() );
 }
-
-uint32_t _CPU_SMP_Initialize( void )
-{
-  /* XXX need to deal with finding too many cores */
-
-  return (uint32_t) imps_probe();
-}
-
-void _CPU_SMP_Prepare_start_multitasking( void )
-{
-  /* Do nothing */
-}
-
-bool _CPU_SMP_Start_processor( uint32_t cpu_index )
-{
-  (void) cpu_index;
-
-  return true;
-}
-
-void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
-{
-  if ( cpu_count > 1 )
-    ipi_install_irq();
-}
-
-void _CPU_SMP_Send_interrupt( uint32_t target_processor_index )
-{
-  send_ipi( target_processor_index, 0x30 );
-}