Optimise the AP startup code to reduce the startup delay.

app/config.c

@@ -87,7 +87,7 @@ cpu_mode_t      cpu_mode = PAR;
 
 error_mode_t    error_mode = ERROR_MODE_NONE;
 
-bool            enable_pcpu[MAX_PCPUS];
+cpu_state_t     pcpu_state[MAX_PCPUS];
 
 bool            enable_temperature = false;
 bool            enable_trace       = false;
@@ -505,12 +505,12 @@ static void error_mode_menu(void)
     clear_screen_region(POP_REGION);
 }
 
-static bool set_all_cpus(bool enabled, int display_offset)
+static bool set_all_cpus(cpu_state_t state, int display_offset)
 {
     clear_popup_row(POP_R+16);
     for (int i = 1; i < num_pcpus; i++) {
-        enable_pcpu[i] = enabled;
+        pcpu_state[i] = state;
-        display_enabled(POP_R+12, i - display_offset, enabled);
+        display_enabled(POP_R+12, i - display_offset, state == CPU_STATE_ENABLED);
     }
     return true;
 }
@@ -524,7 +524,7 @@ static bool add_or_remove_cpu(bool add, int display_offset)
         display_error_message(POP_R+16, "Invalid CPU number");
         return false;
     }
-    enable_pcpu[n] = add;
+    pcpu_state[n] = add ? CPU_STATE_ENABLED : CPU_STATE_DISABLED;
     display_enabled(POP_R+12, n - display_offset, add);
     clear_popup_row(POP_R+16);
     return true;
@@ -545,7 +545,7 @@ static bool add_cpu_range(int display_offset)
         return false;
     }
     for (int i = n1; i <= n2; i++) {
-        enable_pcpu[i] = true;
+        pcpu_state[i] = CPU_STATE_ENABLED;
         display_enabled(POP_R+12, i - display_offset, true);
     }
     clear_popup_row(POP_R+16);
@@ -560,7 +560,7 @@ static void display_cpu_selection(int display_offset)
         printc(POP_R+12, POP_LM, 'B');
     }
     for (int i = 1; i < num_pcpus; i++) {
-        display_enabled(POP_R+12, i - display_offset, enable_pcpu[i]);
+        display_enabled(POP_R+12, i - display_offset, pcpu_state[i] == CPU_STATE_ENABLED);
     }
 }
 
@@ -642,7 +642,7 @@ void config_init(void)
     error_mode = ERROR_MODE_ADDRESS;
 
     for (int i = 0; i < MAX_PCPUS; i++) {
-        enable_pcpu[i] = true;
+        pcpu_state[i] = CPU_STATE_ENABLED;
     }
 
     enable_temperature = !no_temperature;

app/config.h

@@ -34,7 +34,7 @@ extern cpu_mode_t   cpu_mode;
 
 extern error_mode_t error_mode;
 
-extern bool         enable_pcpu[MAX_PCPUS];
+extern cpu_state_t  pcpu_state[MAX_PCPUS];
 
 extern bool         enable_temperature;
 extern bool         enable_trace;

app/display.h

@@ -150,6 +150,9 @@
 #define display_notice(str) \
     prints(ROW_MESSAGE_T + 6, (SCREEN_WIDTH - strlen(str)) / 2, str)
 
+#define display_notice_with_args(length, ...) \
+    printf(ROW_MESSAGE_T + 6, (SCREEN_WIDTH - length) / 2, __VA_ARGS__)
+
 #define clear_footer_message() \
     { \
         set_background_colour(WHITE); \

app/main.c

@@ -167,7 +167,7 @@ static void global_init(void)
 
     num_vcpus = 0;
     for (int i = 0; i < num_pcpus; i++) {
-        if (enable_pcpu[i]) {
+        if (pcpu_state[i] == CPU_STATE_ENABLED) {
             pcpu_num_to_vcpu_num[i] = num_vcpus;
             num_vcpus++;
         }
@@ -204,8 +204,10 @@ static void global_init(void)
     start_mutex   = smp_alloc_mutex();
     error_mutex   = smp_alloc_mutex();
 
-    if (smp_start(enable_pcpu) != SMP_ERR_NONE) {
+    int failed = smp_start(pcpu_state);
-        display_notice("Failed to start other CPUs. Press any key to reboot...");
+    if (failed) {
+        const char *message = "Failed to start CPU core %i. Press any key to reboot...";
+        display_notice_with_args(strlen(message), message, failed);
         while (get_key() == 0) { }
         reboot();
     }
@@ -380,7 +382,7 @@ void main(void)
             init_state = 1;
             global_init();
         } else {
-            smp_set_ap_booted(my_pcpu);
+            pcpu_state[my_pcpu] = CPU_STATE_RUNNING;
         }
     } else {
         // Release the lock taken in run_at().

system/smp.c

@@ -1,5 +1,5 @@
 // SPDX-License-Identifier: GPL-2.0
-// Copyright (C) 2020 Martin Whitaker.
+// Copyright (C) 2020-2022 Martin Whitaker.
 //
 // Derived from an extract of memtest86+ smp.c:
 //
@@ -240,8 +240,6 @@ static int8_t           apic_id_to_pcpu_num[MAX_APIC_IDS];
 
 static uint8_t          pcpu_num_to_apic_id[MAX_PCPUS];
 
-static volatile bool    cpu_started[MAX_PCPUS];
-
 static uintptr_t        smp_heap_page = 0;
 
 static uintptr_t        alloc_addr = 0;
@@ -275,7 +273,7 @@ static uint32_t apic_read(unsigned reg)
     return apic[reg][0];
 }
 
-static void send_ipi(unsigned apic_id, unsigned trigger, unsigned level, unsigned mode, uint8_t vector)
+static bool send_ipi(unsigned apic_id, unsigned trigger, unsigned level, unsigned mode, uint8_t vector)
 {
     uint32_t v;
 
@@ -289,6 +287,14 @@ static void send_ipi(unsigned apic_id, unsigned trigger, unsigned level, unsigne
     v |= mode           << APIC_ICRLO_DELMODE_OFFSET;
     v |= vector;
     apic_write(APICR_ICRLO, v);
+
+    for (int time = 0; time < 100; time++) {
+        usleep(1);
+        if (~apic_read(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) {
+            return true;
+        }
+    }
+    return false;
 }
 
 static int checksum(const void *data, int length)
@@ -605,55 +611,33 @@ static bool find_cpus_in_rsdp(void)
     return false;
 }
 
-static smp_error_t start_cpu(int pcpu_num)
+static bool start_cpu(int pcpu_num)
 {
+    // This implements the universal algorithm described in section B.4 of the Intel Multiprocessor specification.
+
     int apic_id = pcpu_num_to_apic_id[pcpu_num];
 
-    // Clear the APIC ESR register.
-    apic_write(APICR_ESR, 0);
-    apic_read(APICR_ESR);
-
     // Pulse the INIT IPI.
-    send_ipi(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
+    apic_write(APICR_ESR, 0);
-    usleep(100000);
+    if (!send_ipi(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0)) {
-    send_ipi(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);
+        return false;
+    }
+    if (!send_ipi(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0)) {
+        return false;
+    }
 
+    usleep(10000);
+
+    // Send two STARTUP_IPIs.
     for (int num_sipi = 0; num_sipi < 2; num_sipi++) {
         apic_write(APICR_ESR, 0);
-
+        if (!send_ipi(apic_id, 0, 0, APIC_DELMODE_STARTUP, AP_TRAMPOLINE_PAGE)) {
-        send_ipi(apic_id, 0, 0, APIC_DELMODE_STARTUP, AP_TRAMPOLINE_PAGE);
+            return false;
-
-        bool send_pending;
-        int timeout = 0;
-        do {
-            usleep(10);
-            timeout++;
-            send_pending = (apic_read(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
-        } while (send_pending && timeout < 1000);
-
-        if (send_pending) {
-            return SMP_ERR_STARTUP_IPI_NOT_SENT;
-        }
-
-        usleep(100000);
-
-        uint32_t error = apic_read(APICR_ESR) & 0xef;
-        if (error) {
-            return SMP_ERR_STARTUP_IPI_ERROR + error;
         }
+        usleep(200);
     }
 
-    int timeout = 0;
+    return true;
-    do {
-        usleep(10);
-        timeout++;
-    } while (!cpu_started[pcpu_num] && timeout < 100000);
-
-    if (!cpu_started[pcpu_num]) {
-        return SMP_ERR_BOOT_TIMEOUT;
-    }
-
-    return SMP_ERR_NONE;
 }
 
 //------------------------------------------------------------------------------
@@ -667,8 +651,7 @@ void smp_init(bool smp_enable)
     }
 
     for (int i = 0; i < MAX_PCPUS; i++) {
-        pcpu_num_to_apic_id[i]  = 0;
+        pcpu_num_to_apic_id[i] = 0;
-        cpu_started[i] = false;
     }
 
     num_pcpus = 1;
@@ -693,25 +676,32 @@ void smp_init(bool smp_enable)
     alloc_addr = HEAP_BASE_ADDR + ap_trampoline_size;
 }
 
-smp_error_t smp_start(bool enable_pcpu[MAX_PCPUS])
+int smp_start(cpu_state_t pcpu_state[MAX_PCPUS])
 {
-    enable_pcpu[0] = true;  // we don't support disabling the boot CPU
+    int pcpu_num;
 
-    for (int i = 1; i < num_pcpus; i++) {
+    pcpu_state[0] = CPU_STATE_RUNNING;  // we don't support disabling the boot CPU
-        if (enable_pcpu[i]) {
+
-            smp_error_t error = start_cpu(i);
+    for (pcpu_num = 1; pcpu_num < num_pcpus; pcpu_num++) {
-            if (error != SMP_ERR_NONE) {
+        if (pcpu_state[pcpu_num] == CPU_STATE_ENABLED) {
-                return error;
+            if (!start_cpu(pcpu_num)) {
+                return pcpu_num;
             }
         }
     }
 
-    return SMP_ERR_NONE;
+    int timeout = 100000;
-}
+    while (timeout > 0) {
-
+        for (pcpu_num = 1; pcpu_num < num_pcpus; pcpu_num++) {
-void smp_set_ap_booted(int pcpu_num)
+            if (pcpu_state[pcpu_num] == CPU_STATE_ENABLED) break;
-{
+        }
-    cpu_started[pcpu_num] = true;
+        if (pcpu_num == num_pcpus) {
+            return 0;
+        }
+        usleep(10);
+        timeout--;
+    }
+    return pcpu_num;
 }
 
 int smp_my_pcpu_num(void)

system/smp.h

@@ -16,23 +16,23 @@
 #include "spinlock.h"
 
 /*
- * The maximum number of active physical CPUs. There must be room in
+ * The maximum number of active CPU cores. In the current implementation this
- * low memory for the program and all the CPU stacks.
+ * is limited to 256 both by the number of available APIC IDs and the need to
+ * fit both the program and the CPU stacks in low memory.
  */
-#define MAX_PCPUS   256
+#define MAX_PCPUS       256
 
 /*
- * An error code returned by smp_start().
+ * The current state of a CPU core.
  */
-typedef enum {
+typedef enum  __attribute__ ((packed)) {
-    SMP_ERR_NONE                    = 0,
+    CPU_STATE_DISABLED  = 0,
-    SMP_ERR_BOOT_TIMEOUT            = 1,
+    CPU_STATE_ENABLED   = 1,
-    SMP_ERR_STARTUP_IPI_NOT_SENT    = 2,
+    CPU_STATE_RUNNING   = 2
-    SMP_ERR_STARTUP_IPI_ERROR       = 0x100 // error code will be added to this
+} cpu_state_t;
-} smp_error_t;
 
 /*
- * The number of available physical CPUs. Initially this is 1, but may
+ * The number of available physical CPU cores. Initially this is 1, but may
  * increase after calling smp_init().
  */
 extern int num_pcpus;
@@ -52,14 +52,10 @@ extern uintptr_t rsdp_addr;
 void smp_init(bool smp_enable);
 
 /*
- * Starts the selected APs.
+ * Starts the APs listed as enabled in pcpu_state. Returns 0 on success
+ * or the index number of the lowest-numbered AP that failed to start.
  */
-smp_error_t smp_start(bool enable_pcpu[MAX_PCPUS]);
+int smp_start(cpu_state_t pcpu_state[MAX_PCPUS]);
-
-/*
- * Signals that an AP has booted.
- */
-void smp_set_ap_booted(int pcpu_num);
 
 /*
  * Returns the ordinal number of the calling PCPU.