#define dbg_restore_debug_regs()
 #endif /* ! CONFIG_KGDB */
 
-static void wait_for_master_cpu(int cpu)
-{
-       /*
-        * wait for ACK from master CPU before continuing
-        * with AP initialization
-        */
-       WARN_ON(cpumask_test_and_set_cpu(cpu, cpu_initialized_mask));
-       while (!cpumask_test_cpu(cpu, cpu_callout_mask))
-               cpu_relax();
-}
-
 /*
  * cpu_init() initializes state that is per-CPU. Some data is already
  * initialized (naturally) in the bootstrap process, such as the GDT
        struct task_struct *me;
        struct tss_struct *t;
        unsigned long v;
-       int cpu = stack_smp_processor_id();
+       int cpu;
        int i;
 
-       wait_for_master_cpu(cpu);
-
        /*
         * Load microcode on this cpu if a valid microcode is available.
         * This is early microcode loading procedure.
         */
        load_ucode_ap();
 
+       cpu = stack_smp_processor_id();
        t = &per_cpu(init_tss, cpu);
        oist = &per_cpu(orig_ist, cpu);
 
 
        me = current;
 
+       if (cpumask_test_and_set_cpu(cpu, cpu_initialized_mask))
+               panic("CPU#%d already initialized!\n", cpu);
+
        pr_debug("Initializing CPU#%d\n", cpu);
 
        clear_in_cr4(X86_CR4_VME|X86_CR4_PVI|X86_CR4_TSD|X86_CR4_DE);
        struct tss_struct *t = &per_cpu(init_tss, cpu);
        struct thread_struct *thread = &curr->thread;
 
-       wait_for_master_cpu(cpu);
-
        show_ucode_info_early();
 
+       if (cpumask_test_and_set_cpu(cpu, cpu_initialized_mask)) {
+               printk(KERN_WARNING "CPU#%d already initialized!\n", cpu);
+               for (;;)
+                       local_irq_enable();
+       }
+
        printk(KERN_INFO "Initializing CPU#%d\n", cpu);
 
        if (cpu_has_vme || cpu_has_tsc || cpu_has_de)
 
 static void smp_callin(void)
 {
        int cpuid, phys_id;
+       unsigned long timeout;
 
        /*
         * If waken up by an INIT in an 82489DX configuration
         * (This works even if the APIC is not enabled.)
         */
        phys_id = read_apic_id();
+       if (cpumask_test_cpu(cpuid, cpu_callin_mask)) {
+               panic("%s: phys CPU#%d, CPU#%d already present??\n", __func__,
+                                       phys_id, cpuid);
+       }
+       pr_debug("CPU#%d (phys ID: %d) waiting for CALLOUT\n", cpuid, phys_id);
+
+       /*
+        * STARTUP IPIs are fragile beasts as they might sometimes
+        * trigger some glue motherboard logic. Complete APIC bus
+        * silence for 1 second, this overestimates the time the
+        * boot CPU is spending to send the up to 2 STARTUP IPIs
+        * by a factor of two. This should be enough.
+        */
+
+       /*
+        * Waiting 2s total for startup (udelay is not yet working)
+        */
+       timeout = jiffies + 2*HZ;
+       while (time_before(jiffies, timeout)) {
+               /*
+                * Has the boot CPU finished it's STARTUP sequence?
+                */
+               if (cpumask_test_cpu(cpuid, cpu_callout_mask))
+                       break;
+               cpu_relax();
+       }
+
+       if (!time_before(jiffies, timeout)) {
+               panic("%s: CPU%d started up but did not get a callout!\n",
+                     __func__, cpuid);
+       }
 
        /*
         * the boot CPU has finished the init stage and is spinning
        unsigned long start_ip = real_mode_header->trampoline_start;
 
        unsigned long boot_error = 0;
+       int timeout;
        int cpu0_nmi_registered = 0;
-       unsigned long timeout;
 
        /* Just in case we booted with a single CPU. */
        alternatives_enable_smp();
                }
        }
 
-       /*
-        * AP might wait on cpu_callout_mask in cpu_init() with
-        * cpu_initialized_mask set if previous attempt to online
-        * it timed-out. Clear cpu_initialized_mask so that after
-        * INIT/SIPI it could start with a clean state.
-        */
-       cpumask_clear_cpu(cpu, cpu_initialized_mask);
-       smp_mb();
-
        /*
         * Wake up a CPU in difference cases:
         * - Use the method in the APIC driver if it's defined
                boot_error = wakeup_cpu_via_init_nmi(cpu, start_ip, apicid,
                                                     &cpu0_nmi_registered);
 
-
        if (!boot_error) {
                /*
-                * Wait 10s total for a response from AP
+                * allow APs to start initializing.
                 */
-               boot_error = -1;
-               timeout = jiffies + 10*HZ;
-               while (time_before(jiffies, timeout)) {
-                       if (cpumask_test_cpu(cpu, cpu_initialized_mask)) {
-                               /*
-                                * Tell AP to proceed with initialization
-                                */
-                               cpumask_set_cpu(cpu, cpu_callout_mask);
-                               boot_error = 0;
-                               break;
-                       }
-                       udelay(100);
-                       schedule();
-               }
-       }
+               pr_debug("Before Callout %d\n", cpu);
+               cpumask_set_cpu(cpu, cpu_callout_mask);
+               pr_debug("After Callout %d\n", cpu);
 
-       if (!boot_error) {
                /*
-                * Wait till AP completes initial initialization
+                * Wait 5s total for a response
                 */
-               while (!cpumask_test_cpu(cpu, cpu_callin_mask)) {
+               for (timeout = 0; timeout < 50000; timeout++) {
+                       if (cpumask_test_cpu(cpu, cpu_callin_mask))
+                               break;  /* It has booted */
+                       udelay(100);
                        /*
                         * Allow other tasks to run while we wait for the
                         * AP to come online. This also gives a chance
                         * for the MTRR work(triggered by the AP coming online)
                         * to be completed in the stop machine context.
                         */
-                       udelay(100);
                        schedule();
                }
+
+               if (cpumask_test_cpu(cpu, cpu_callin_mask)) {
+                       print_cpu_msr(&cpu_data(cpu));
+                       pr_debug("CPU%d: has booted.\n", cpu);
+               } else {
+                       boot_error = 1;
+                       if (*trampoline_status == 0xA5A5A5A5)
+                               /* trampoline started but...? */
+                               pr_err("CPU%d: Stuck ??\n", cpu);
+                       else
+                               /* trampoline code not run */
+                               pr_err("CPU%d: Not responding\n", cpu);
+                       if (apic->inquire_remote_apic)
+                               apic->inquire_remote_apic(apicid);
+               }
+       }
+
+       if (boot_error) {
+               /* Try to put things back the way they were before ... */
+               numa_remove_cpu(cpu); /* was set by numa_add_cpu */
+
+               /* was set by do_boot_cpu() */
+               cpumask_clear_cpu(cpu, cpu_callout_mask);
+
+               /* was set by cpu_init() */
+               cpumask_clear_cpu(cpu, cpu_initialized_mask);
        }
 
        /* mark "stuck" area as not stuck */