smp: restore MSRs on S3 resume1.9-stable

Currently the MTRRs and MSR_IA32_FEATURE_CONTROL are not restored on S3
resume.  Because these have to be applied to all processors, SMP setup
has to be added to S3 resume.

There are two differences between the boot and resume paths.  First,
romfile_* is not usable in the resume paths so we separate out the
remaining common code to a new smp_scan function.  Second, smp_msr has
to be walked on the BSP as well, so we extract that out of handle_smp
and into a new function smp_write_msrs.  Then, resume can call
smp_write_msrs on the BSP followed by smp_scan to initialize the APs.

Reported-by: Laszlo Ersek <lersek@redhat.com>
Signed-off-by: Paolo Bonzini <pbonzini@redhat.com>
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
(cherry picked from commit 54e3a88609da074aaae2f04e592026ebf82169dc)

3 files changed, 38 insertions, 14 deletions

diff --git a/src/fw/smp.c b/src/fw/smp.c
index 6e706e4..719d51d 100644
--- a/src/fw/smp.c
+++ b/src/fw/smp.c
@@ -36,6 +36,15 @@ wrmsr_smp(u32 index, u64 val)
     smp_msr_count++;
 }
 
+static void
+smp_write_msrs(void)
+{
+    // MTRR and MSR_IA32_FEATURE_CONTROL setup
+    int i;
+    for (i=0; i<smp_msr_count; i++)
+        wrmsr(smp_msr[i].index, smp_msr[i].val);
+}
+
 u32 MaxCountCPUs;
 static u32 CountCPUs;
 // 256 bits for the found APIC IDs
@@ -58,10 +67,7 @@ handle_smp(void)
     u8 apic_id = ebx>>24;
     dprintf(DEBUG_HDL_smp, "handle_smp: apic_id=%d\n", apic_id);
 
-    // MTRR and MSR_IA32_FEATURE_CONTROL setup
-    int i;
-    for (i=0; i<smp_msr_count; i++)
-        wrmsr(smp_msr[i].index, smp_msr[i].val);
+    smp_write_msrs();
 
     // Set bit on FoundAPICIDs
     FoundAPICIDs[apic_id/32] |= (1 << (apic_id % 32));
@@ -74,12 +80,9 @@ u32 SMPLock __VISIBLE;
 u32 SMPStack __VISIBLE;
 
 // find and initialize the CPUs by launching a SIPI to them
-void
-smp_setup(void)
+static void
+smp_scan(void)
 {
-    if (!CONFIG_QEMU)
-        return;
-
     ASSERT32FLAT();
     u32 eax, ebx, ecx, cpuid_features;
     cpuid(1, &eax, &ebx, &ecx, &cpuid_features);
@@ -87,7 +90,6 @@ smp_setup(void)
         // No apic - only the main cpu is present.
         dprintf(1, "No apic - only the main cpu is present.\n");
         CountCPUs= 1;
-        MaxCountCPUs = 1;
         return;
     }
 
@@ -141,10 +143,30 @@ smp_setup(void)
     // Restore memory.
     *(u64*)BUILD_AP_BOOT_ADDR = old;
 
-    MaxCountCPUs = romfile_loadint("etc/max-cpus", 0);
-    if (!MaxCountCPUs || MaxCountCPUs < CountCPUs)
-        MaxCountCPUs = CountCPUs;
-
     dprintf(1, "Found %d cpu(s) max supported %d cpu(s)\n", CountCPUs,
             MaxCountCPUs);
 }
+
+void
+smp_setup(void)
+{
+    if (!CONFIG_QEMU)
+        return;
+
+    MaxCountCPUs = romfile_loadint("etc/max-cpus", 0);
+    u8 cmos_smp_count = rtc_read(CMOS_BIOS_SMP_COUNT) + 1;
+    if (MaxCountCPUs < cmos_smp_count)
+        MaxCountCPUs = cmos_smp_count;
+
+    smp_scan();
+}
+
+void
+smp_resume(void)
+{
+    if (!CONFIG_QEMU)
+        return;
+
+    smp_write_msrs();
+    smp_scan();
+}
diff --git a/src/resume.c b/src/resume.c
index a5465d8..3a26f84 100644
--- a/src/resume.c
+++ b/src/resume.c
@@ -97,6 +97,7 @@ s3_resume(void)
 
     pic_setup();
     smm_setup();
+    smp_resume();
 
     pci_resume();
 
diff --git a/src/util.h b/src/util.h
index cba3359..ba9c420 100644
--- a/src/util.h
+++ b/src/util.h
@@ -132,6 +132,7 @@ void smm_setup(void);
 extern u32 MaxCountCPUs;
 void wrmsr_smp(u32 index, u64 val);
 void smp_setup(void);
+void smp_resume(void);
 int apic_id_is_present(u8 apic_id);
 
 // hw/dma.c