smp: Fix INIT/STARTUP IPI sequence

TESTED: works in qemu
TESTED: works hardware with AMD cpu
Message-ID: <20240207050158.1640853-5-damien@zamaudio.com>

4 files changed, 110 insertions, 40 deletions

diff --git a/i386/i386/mp_desc.c b/i386/i386/mp_desc.c
index a5f6b8f6..61a7607b 100644
--- a/i386/i386/mp_desc.c
+++ b/i386/i386/mp_desc.c
@@ -292,17 +292,22 @@ cpu_ap_main()
 kern_return_t
 cpu_start(int cpu)
 {
+    int err;
+
     assert(machine_slot[cpu].running != TRUE);
 
     uint16_t apic_id = apic_get_cpu_apic_id(cpu);
 
-    printf("Trying to enable: %d\n", apic_id);
-
-    smp_startup_cpu(apic_id, apboot_addr);
+    printf("Trying to enable: %d at 0x%lx\n", apic_id, apboot_addr);
 
-    printf("Started cpu %d (lapic id %04x)\n", cpu, apic_id);
+    err = smp_startup_cpu(apic_id, apboot_addr);
 
-    return KERN_SUCCESS;
+    if (!err) {
+        printf("Started cpu %d (lapic id %04x)\n", cpu, apic_id);
+        return KERN_SUCCESS;
+    }
+    printf("FATAL: Cannot init AP %d\n", cpu);
+    for (;;);
 }
 
 void
diff --git a/i386/i386/smp.c b/i386/i386/smp.c
index 87f59913..05e9de67 100644
--- a/i386/i386/smp.c
+++ b/i386/i386/smp.c
@@ -18,10 +18,14 @@
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111, USA. */
 
+#include <string.h>
 #include <i386/apic.h>
 #include <i386/smp.h>
 #include <i386/cpu.h>
+#include <i386/pio.h>
+#include <i386/vm_param.h>
 #include <i386at/idt.h>
+#include <i386at/cram.h>
 #include <i386at/acpi_parse_apic.h>
 #include <kern/printf.h>
 #include <mach/machine.h>
@@ -75,59 +79,112 @@ void smp_pmap_update(unsigned apic_id)
     smp_send_ipi(apic_id, CALL_PMAP_UPDATE);
 }
 
-/* See Intel IA32/64 Software Developer's Manual 3A Section 8.4.4.1 */
-void smp_startup_cpu(unsigned apic_id, unsigned vector)
+static void
+wait_for_ipi(void)
 {
-    /* Clear APIC errors */
-    lapic->error_status.r = 0;
+    /* This could have a timeout, but if the IPI
+     * is never delivered, its a disaster anyway */
+    while (lapic->icr_low.delivery_status == SEND_PENDING) {
+	cpu_pause();
+    }
+}
+
+static int
+smp_send_ipi_init(int apic_id)
+{
+    int err;
 
-    printf("Sending IPIs to APIC ID %u...", apic_id);
+    lapic->error_status.r = 0;
 
-    /* Assert INIT IPI */
-    apic_send_ipi(NO_SHORTHAND, INIT, PHYSICAL, ASSERT, LEVEL, 0, apic_id);
+    /* Assert INIT IPI:
+     *
+     * This is EDGE triggered to match the deassert
+     */
+    apic_send_ipi(NO_SHORTHAND, INIT, PHYSICAL, ASSERT, EDGE, 0, apic_id);
 
     /* Wait for delivery */
-    do {
-        cpu_pause();
-    } while(lapic->icr_low.delivery_status == SEND_PENDING);
+    wait_for_ipi();
+    hpet_mdelay(10);
 
-    /* Deassert INIT IPI */
-    apic_send_ipi(NO_SHORTHAND, INIT, PHYSICAL, DE_ASSERT, LEVEL, 0, apic_id);
+    /* Deassert INIT IPI:
+     *
+     * NB: This must be an EDGE triggered deassert signal.
+     * A LEVEL triggered deassert is only supported on very old hardware
+     * that does not support STARTUP IPIs at all, and instead jump
+     * via a warm reset vector.
+     */
+    apic_send_ipi(NO_SHORTHAND, INIT, PHYSICAL, DE_ASSERT, EDGE, 0, apic_id);
 
     /* Wait for delivery */
-    do {
-        cpu_pause();
-    } while(lapic->icr_low.delivery_status == SEND_PENDING);
+    wait_for_ipi();
 
-    /* Wait 10 msec */
-    hpet_mdelay(10);
+    err = lapic->error_status.r;
+    if (err) {
+        printf("ESR error upon INIT 0x%x\n", err);
+    }
+    return 0;
+}
 
-    /* Clear APIC errors */
-    lapic->error_status.r = 0;
+static int
+smp_send_ipi_startup(int apic_id, int vector)
+{
+    int err;
 
-    /* First StartUp IPI */
-    apic_send_ipi(NO_SHORTHAND, STARTUP, PHYSICAL, ASSERT, LEVEL, vector >> 12, apic_id);
+    lapic->error_status.r = 0;
 
-    /* Wait 200 usec */
-    hpet_udelay(200);
+    /* StartUp IPI:
+     *
+     * Have not seen any documentation for trigger mode for this IPI
+     * but it seems to work with EDGE.  (AMD BKDG FAM16h document specifies dont care)
+     */
+    apic_send_ipi(NO_SHORTHAND, STARTUP, PHYSICAL, ASSERT, EDGE, vector, apic_id);
 
     /* Wait for delivery */
-    do {
-        cpu_pause();
-    } while(lapic->icr_low.delivery_status == SEND_PENDING);
+    wait_for_ipi();
 
-    /* Second StartUp IPI */
-    apic_send_ipi(NO_SHORTHAND, STARTUP, PHYSICAL, ASSERT, LEVEL, vector >> 12, apic_id);
+    err = lapic->error_status.r;
+    if (err) {
+        printf("ESR error upon STARTUP 0x%x\n", err);
+    }
+    return 0;
+}
 
-    /* Wait 200 usec */
+/* See Intel IA32/64 Software Developer's Manual 3A Section 8.4.4.1 */
+int smp_startup_cpu(unsigned apic_id, phys_addr_t start_eip)
+{
+#if 0
+    /* This block goes with a legacy method of INIT that only works with
+     * old hardware that does not support SIPIs.
+     * Must use INIT DEASSERT LEVEL triggered IPI to use this block.
+     * (At least one AMD FCH does not support this IPI mode,
+     * See AMD BKDG FAM16h document # 48751 page 461).
+     */
+
+    /* Tell CMOS to warm reset through through 40:67 */
+    outb(CMOS_ADDR, CMOS_SHUTDOWN);
+    outb(CMOS_DATA, CM_JMP_467);
+
+    /* Set warm reset vector to point to AP startup code */
+    uint16_t dword[2];
+    dword[0] = 0;
+    dword[1] = start_eip >> 4;
+    memcpy((uint8_t *)phystokv(0x467), dword, 4);
+#endif
+
+    /* Local cache flush */
+    asm("wbinvd":::"memory");
+
+    printf("Sending IPIs to APIC ID %u...\n", apic_id);
+
+    smp_send_ipi_init(apic_id);
+    hpet_mdelay(10);
+    smp_send_ipi_startup(apic_id, start_eip >> STARTUP_VECTOR_SHIFT);
+    hpet_udelay(200);
+    smp_send_ipi_startup(apic_id, start_eip >> STARTUP_VECTOR_SHIFT);
     hpet_udelay(200);
-
-    /* Wait for delivery */
-    do {
-        cpu_pause();
-    } while(lapic->icr_low.delivery_status == SEND_PENDING);
 
     printf("done\n");
+    return 0;
 }
 
 /*
diff --git a/i386/i386/smp.h b/i386/i386/smp.h
index 784936ea..73d273ef 100644
--- a/i386/i386/smp.h
+++ b/i386/i386/smp.h
@@ -21,11 +21,14 @@
 #ifndef _SMP_H_
 #define _SMP_H_
 
+#include <mach/machine/vm_types.h>
+
 int smp_init(void);
 void smp_remote_ast(unsigned apic_id);
 void smp_pmap_update(unsigned apic_id);
-void smp_startup_cpu(unsigned apic_id, unsigned vector);
+int smp_startup_cpu(unsigned apic_id, phys_addr_t start_eip);
 
 #define cpu_pause() asm volatile ("pause" : : : "memory")
+#define STARTUP_VECTOR_SHIFT	(20 - 8)
 
 #endif
diff --git a/i386/i386at/cram.h b/i386/i386at/cram.h
index 8a3a6ec9..ac40cf13 100644
--- a/i386/i386at/cram.h
+++ b/i386/i386at/cram.h
@@ -71,6 +71,11 @@ WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
 /* Addresses, related masks, and potential results */
 
+#define CMOS_SHUTDOWN	0xf
+#define CM_NORM_RST	0x0
+#define CM_LOAD_SYS	0x4
+#define CM_JMP_467	0xa
+
 #define CMOS_EB		0x14		/* read Equipment Byte */
 #define CM_SCRMSK	0x30		/* mask for EB query to get screen */
 #define CM_EGA_VGA	0x00		/* "not CGA or MONO" */