[SeaBIOS] [PATCH v2 2/4] smp: refactor present CPU APIC ID detection and counting

[Igor Mammedov]

From: Kevin O'Connor

Signed-off-by: "Kevin O'Connor" 
Signed-off-by: Igor Mammedov 
---
v2:
  * s/count_cpu/apic_id_init/
  * call apic_id_init() after sending SIPI,
    it will be needed for switching BSP into x2APIC mode
---
 src/fw/smp.c | 33 +++++++++++++++++++--------------
 1 file changed, 19 insertions(+), 14 deletions(-)

diff --git a/src/fw/smp.c b/src/fw/smp.c
index 579acdb..f2cc827 100644
--- a/src/fw/smp.c
+++ b/src/fw/smp.c
@@ -46,27 +46,34 @@ int apic_id_is_present(u8 apic_id)
     return !!(FoundAPICIDs[apic_id/32] & (1ul << (apic_id % 32)));
 }

+static int
+apic_id_init(void)
+{
+    CountCPUs++;
+
+    // Track found apic id for use in legacy internal bios tables
+    u32 eax, ebx, ecx, cpuid_features;
+    cpuid(1, &eax, &ebx, &ecx, &cpuid_features);
+    u8 apic_id = ebx>>24;
+    FoundAPICIDs[apic_id/32] |= 1 << (apic_id % 32);
+
+    return apic_id;
+}
+
 void VISIBLE32FLAT
 handle_smp(void)
 {
     if (!CONFIG_QEMU)
         return;

-    // Detect apic_id
-    u32 eax, ebx, ecx, cpuid_features;
-    cpuid(1, &eax, &ebx, &ecx, &cpuid_features);
-    u8 apic_id = ebx>>24;
+    // Track this CPU and detect the apic_id
+    int apic_id = apic_id_init();
     dprintf(DEBUG_HDL_smp, "handle_smp: apic_id=%d\n", apic_id);

// MTRR setup
     int i;
     for (i=0; i<smp_mtrr_count; i++)
         wrmsr(smp_mtrr[i].index, smp_mtrr[i].val);
-
-    // Set bit on FoundAPICIDs
-    FoundAPICIDs[apic_id/32] |= (1 << (apic_id % 32));
-
-    CountCPUs++;
 }

// Atomic lock for shared stack across processors.
@@ -91,11 +98,6 @@ smp_setup(void)
         return;
     }

-    // mark the BSP initial APIC ID as found, too:
-    u8 apic_id = ebx>>24;
-    FoundAPICIDs[apic_id/32] |= (1 << (apic_id % 32));
-    CountCPUs = 1;
-
     // Setup jump trampoline to counter code.
     u64 old = *(u64*)BUILD_AP_BOOT_ADDR;
     // ljmpw $SEG_BIOS, $(entry_smp - BUILD_BIOS_ADDR)
@@ -123,6 +125,9 @@ smp_setup(void)
     u32 sipi_vector = BUILD_AP_BOOT_ADDR >> 12;
     writel(APIC_ICR_LOW, 0x000C4600 | sipi_vector);

+    // Put BSP into APIC ID bitmap and CPUs counter
+    apic_id_init();
+
     // Wait for other CPUs to process the SIPI.
     u8 cmos_smp_count = rtc_read(CMOS_BIOS_SMP_COUNT) + 1;
     while (cmos_smp_count != CountCPUs)