x86/HVM: fix ID handling of x2APIC emulation

- properly change ID when switching into x2APIC mode (instead of
  mimicking necessary behavior in hvm_x2apic_msr_read())
- correctly (meaningfully) set LDR (so far it ended up being 1 on all
  vCPU-s)
- even if we don't support more than 128 vCPU-s in a HVM guest for now,
  we should properly handle IDs as 32-bit values (i.e. not ignore the
  top 24 bits)
- with that, properly do cluster ID and bit mask check in
  vlapic_match_logical_addr()
- slightly adjust other parameter types of vlapic_match_dest() and
  vlapic_lowest_prio() (and related local variable ones)

Signed-off-by: Jan Beulich <jbeulich@xxxxxxxx>
---
v2: Some changes broken out to separate patch. Correct ID and
    LDR after domain restore (if necessary); as stated previously the
    only compatibility problem this creates is when migrating a VM _to_
    an unfixed (i.e. old) hypervisor, a scenario which supposedly isn't
    supported. This post-migration fixup involves introducing
    arch_domain_unpause(), needed here to fix up state after all
    intended state setting was done (any suggestion as to how else to
    accomplish this is very welcome).

--- a/xen/arch/x86/hvm/vlapic.c
+++ b/xen/arch/x86/hvm/vlapic.c
@@ -173,18 +173,17 @@ uint32_t vlapic_set_ppr(struct vlapic *v
    return ppr;
 }
 
-static int vlapic_match_logical_addr(struct vlapic *vlapic, uint8_t mda)
+static int vlapic_match_logical_addr(struct vlapic *vlapic, uint32_t mda)
 {
     int result = 0;
-    uint32_t logical_id;
+    uint32_t logical_id = vlapic_get_reg(vlapic, APIC_LDR);
 
     if ( vlapic_x2apic_mode(vlapic) )
-    {
-        logical_id = vlapic_get_reg(vlapic, APIC_LDR);
-        return !!(logical_id & mda);
-    }
+        return ((logical_id >> 16) == (mda >> 16)) &&
+               (uint16_t)(logical_id & mda);
 
-    logical_id = GET_xAPIC_LOGICAL_ID(vlapic_get_reg(vlapic, APIC_LDR));
+    logical_id = GET_xAPIC_LOGICAL_ID(logical_id);
+    mda = (uint8_t)mda;
 
     switch ( vlapic_get_reg(vlapic, APIC_DFR) )
     {
@@ -207,8 +206,8 @@ static int vlapic_match_logical_addr(str
 }
 
 bool_t vlapic_match_dest(
-    struct vlapic *target, struct vlapic *source,
-    int short_hand, uint8_t dest, uint8_t dest_mode)
+    struct vlapic *target, const struct vlapic *source,
+    int short_hand, uint32_t dest, bool_t dest_mode)
 {
     HVM_DBG_LOG(DBG_LEVEL_VLAPIC, "target %p, source %p, dest %#x, "
                 "dest_mode %#x, short_hand %#x",
@@ -219,7 +218,8 @@ bool_t vlapic_match_dest(
     case APIC_DEST_NOSHORT:
         if ( dest_mode )
             return vlapic_match_logical_addr(target, dest);
-        return ((dest == 0xFF) || (dest == VLAPIC_ID(target)));
+        return (dest == _VLAPIC_ID(target, 0xffffffff)) ||
+               (dest == VLAPIC_ID(target));
 
     case APIC_DEST_SELF:
         return (target == source);
@@ -286,7 +286,7 @@ static void vlapic_init_sipi_action(unsi
     uint32_t icr = vcpu_vlapic(origin)->init_sipi.icr;
     uint32_t dest = vcpu_vlapic(origin)->init_sipi.dest;
     uint32_t short_hand = icr & APIC_SHORT_MASK;
-    uint32_t dest_mode  = !!(icr & APIC_DEST_MASK);
+    bool_t dest_mode  = !!(icr & APIC_DEST_MASK);
     struct vcpu *v;
 
     if ( icr == 0 )
@@ -352,8 +352,8 @@ static void vlapic_accept_irq(struct vcp
 }
 
 struct vlapic *vlapic_lowest_prio(
-    struct domain *d, struct vlapic *source,
-    int short_hand, uint8_t dest, uint8_t dest_mode)
+    struct domain *d, const struct vlapic *source,
+    int short_hand, uint32_t dest, bool_t dest_mode)
 {
     int old = d->arch.hvm_domain.irq.round_robin_prev_vcpu;
     uint32_t ppr, target_ppr = UINT_MAX;
@@ -414,13 +414,11 @@ void vlapic_ipi(
 {
     unsigned int dest;
     unsigned int short_hand = icr_low & APIC_SHORT_MASK;
-    unsigned int dest_mode  = !!(icr_low & APIC_DEST_MASK);
+    bool_t dest_mode = !!(icr_low & APIC_DEST_MASK);
 
     HVM_DBG_LOG(DBG_LEVEL_VLAPIC, "icr = 0x%08x:%08x", icr_high, icr_low);
 
-    dest = (vlapic_x2apic_mode(vlapic)
-            ? icr_high
-            : GET_xAPIC_DEST_FIELD(icr_high));
+    dest = _VLAPIC_ID(vlapic, icr_high);
 
     switch ( icr_low & APIC_MODE_MASK )
     {
@@ -593,10 +591,6 @@ int hvm_x2apic_msr_read(struct vcpu *v, 
     vlapic_read_aligned(vlapic, offset, &low);
     switch ( offset )
     {
-    case APIC_ID:
-        low = GET_xAPIC_ID(low);
-        break;
-
     case APIC_ICR:
         vlapic_read_aligned(vlapic, APIC_ICR2, &high);
         break;
@@ -898,6 +892,15 @@ const struct hvm_mmio_handler vlapic_mmi
     .write_handler = vlapic_write
 };
 
+static void set_x2apic_id(struct vlapic *vlapic)
+{
+    u32 id = vlapic_vcpu(vlapic)->vcpu_id;
+    u32 ldr = ((id & ~0xf) << 12) | (1 << (id & 0xf));
+
+    vlapic_set_reg(vlapic, APIC_ID, id * 2);
+    vlapic_set_reg(vlapic, APIC_LDR, ldr);
+}
+
 bool_t vlapic_msr_set(struct vlapic *vlapic, uint64_t value)
 {
     if ( (vlapic->hw.apic_base_msr ^ value) & MSR_IA32_APICBASE_ENABLE )
@@ -925,11 +928,7 @@ bool_t vlapic_msr_set(struct vlapic *vla
     vlapic->hw.apic_base_msr = value;
 
     if ( vlapic_x2apic_mode(vlapic) )
-    {
-        u32 id = vlapic_get_reg(vlapic, APIC_ID);
-        u32 ldr = ((id & ~0xf) << 16) | (1 << (id & 0xf));
-        vlapic_set_reg(vlapic, APIC_LDR, ldr);
-    }
+        set_x2apic_id(vlapic);
 
     vmx_vlapic_msr_changed(vlapic_vcpu(vlapic));
 
@@ -1216,6 +1215,7 @@ static int lapic_load_hidden(struct doma
         return -EINVAL;
     }
     s = vcpu_vlapic(v);
+    s->loaded = 1;
     
     if ( hvm_load_entry_zeroextend(LAPIC, h, &s->hw) != 0 ) 
         return -EINVAL;
@@ -1244,6 +1244,7 @@ static int lapic_load_regs(struct domain
         return -EINVAL;
     }
     s = vcpu_vlapic(v);
+    s->loaded = 1;
     
     if ( hvm_load_entry(LAPIC_REGS, h, s->regs) != 0 ) 
         return -EINVAL;
@@ -1261,6 +1262,29 @@ HVM_REGISTER_SAVE_RESTORE(LAPIC, lapic_s
 HVM_REGISTER_SAVE_RESTORE(LAPIC_REGS, lapic_save_regs, lapic_load_regs,
                           1, HVMSR_PER_VCPU);
 
+void vlapic_domain_unpause(const struct domain *d)
+{
+    /*
+     * Following lapic_load_hidden()/lapic_load_regs() we may need to
+     * correct ID and LDR when they come from an old, broken hypervisor.
+     */
+    struct vcpu *v;
+
+    for_each_vcpu ( d, v )
+    {
+        struct vlapic *vlapic = vcpu_vlapic(v);
+        u32 id = vlapic_get_reg(vlapic, APIC_ID);
+
+        if ( vlapic->loaded && vlapic_x2apic_mode(vlapic) &&
+             id && GET_xAPIC_ID(id) == v->vcpu_id * 2 &&
+             id == SET_xAPIC_ID(GET_xAPIC_ID(id)) &&
+             vlapic_get_reg(vlapic, APIC_LDR) == 1 )
+            set_x2apic_id(vlapic);
+
+        vlapic->loaded = 0;
+    }
+}
+
 int vlapic_init(struct vcpu *v)
 {
     struct vlapic *vlapic = vcpu_vlapic(v);
--- a/xen/common/domain.c
+++ b/xen/common/domain.c
@@ -949,8 +949,11 @@ void domain_unpause(struct domain *d)
     struct vcpu *v;
 
     if ( atomic_dec_and_test(&d->pause_count) )
+    {
+        arch_domain_unpause(d);
         for_each_vcpu( d, v )
             vcpu_wake(v);
+    }
 }
 
 int __domain_pause_by_systemcontroller(struct domain *d,
--- a/xen/include/asm-arm/domain.h
+++ b/xen/include/asm-arm/domain.h
@@ -241,6 +241,8 @@ struct arch_vcpu
 void vcpu_show_execution_state(struct vcpu *);
 void vcpu_show_registers(const struct vcpu *);
 
+#define arch_domain_unpause(d) ((void)(d))
+
 #endif /* __ASM_DOMAIN_H__ */
 
 /*
--- a/xen/include/asm-x86/domain.h
+++ b/xen/include/asm-x86/domain.h
@@ -499,6 +499,12 @@ void domain_cpuid(struct domain *d,
                   unsigned int  *ecx,
                   unsigned int  *edx);
 
+#define arch_domain_unpause(d) ({  \
+    const struct domain *d_ = (d); \
+    if ( is_hvm_domain(d_) )       \
+        vlapic_domain_unpause(d_); \
+})
+
 #endif /* __ASM_DOMAIN_H__ */
 
 /*
--- a/xen/include/asm-x86/hvm/vlapic.h
+++ b/xen/include/asm-x86/hvm/vlapic.h
@@ -30,8 +30,9 @@
 #define vlapic_vcpu(x)   (container_of((x), struct vcpu, arch.hvm_vcpu.vlapic))
 #define vlapic_domain(x) (vlapic_vcpu(x)->domain)
 
-#define VLAPIC_ID(vlapic)   \
-    (GET_xAPIC_ID(vlapic_get_reg((vlapic), APIC_ID)))
+#define _VLAPIC_ID(vlapic, id) (vlapic_x2apic_mode(vlapic) \
+                                ? (id) : GET_xAPIC_ID(id))
+#define VLAPIC_ID(vlapic) _VLAPIC_ID(vlapic, vlapic_get_reg(vlapic, APIC_ID))
 
 /*
  * APIC can be disabled in two ways:
@@ -70,6 +71,7 @@
 struct vlapic {
     struct hvm_hw_lapic      hw;
     struct hvm_hw_lapic_regs *regs;
+    bool_t                   loaded;
     struct periodic_time     pt;
     s_time_t                 timer_last_update;
     struct page_info         *regs_page;
@@ -123,11 +125,13 @@ void vlapic_ipi(struct vlapic *vlapic, u
 int vlapic_apicv_write(struct vcpu *v, unsigned int offset);
 
 struct vlapic *vlapic_lowest_prio(
-    struct domain *d, struct vlapic *source,
-    int short_hand, uint8_t dest, uint8_t dest_mode);
+    struct domain *d, const struct vlapic *source,
+    int short_hand, uint32_t dest, bool_t dest_mode);
 
 bool_t vlapic_match_dest(
-    struct vlapic *target, struct vlapic *source,
-    int short_hand, uint8_t dest, uint8_t dest_mode);
+    struct vlapic *target, const struct vlapic *source,
+    int short_hand, uint32_t dest, bool_t dest_mode);
+
+void vlapic_domain_unpause(const struct domain *);
 
 #endif /* __ASM_X86_HVM_VLAPIC_H__ */