smp: restore MSRs on S3 resume

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)
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 @@
     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 @@
     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 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 @@
         // 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 @@
     // 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 @@
 
     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 @@
 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