Like it's done with CPUID, introduce support for passing MSR values to
xc_cpuid_apply_policy().  The chosen format for expressing MSR policy
data matches the current one used for CPUID.  Note that existing
callers of xc_cpuid_apply_policy() can pass NULL as the value for the
newly introduced 'msr' parameter in order to preserve the same
functionality, and in fact that's done in libxl on this patch.

Signed-off-by: Roger Pau Monné <roger.pau@citrix.com>
Acked-by: Anthony PERARD <anthony.perard@citrix.com>
---
Changes since v2:
 - Some code adjustment, no functional change.
---
 tools/include/xenctrl.h         |  21 +++-
 tools/libs/guest/xg_cpuid_x86.c | 169 +++++++++++++++++++++++++++++++-
 tools/libs/light/libxl_cpuid.c  |   2 +-
 3 files changed, 188 insertions(+), 4 deletions(-)

diff --git a/tools/include/xenctrl.h b/tools/include/xenctrl.h
index dba33d5d0f39..faec1dd82453 100644
--- a/tools/include/xenctrl.h
+++ b/tools/include/xenctrl.h
@@ -1822,6 +1822,21 @@ struct xc_xend_cpuid {
     char *policy[4];
 };
 
+/*
+ * MSR policy data.
+ *
+ * The format of the policy string is the following:
+ *   '1' -> force to 1
+ *   '0' -> force to 0
+ *   'x' -> we don't care (use default)
+ *   'k' -> pass through host value
+ */
+struct xc_msr {
+    uint32_t index;
+    char policy[65];
+};
+#define XC_MSR_INPUT_UNUSED 0xffffffffu
+
 /*
  * Make adjustments to the CPUID settings for a domain.
  *
@@ -1833,13 +1848,15 @@ struct xc_xend_cpuid {
  * Either pass a full new @featureset (and @nr_features), or adjust individual
  * features (@pae, @itsc, @nested_virt).
  *
- * Then (optionally) apply legacy XEND overrides (@xend) to the result.
+ * Then (optionally) apply legacy XEND CPUID overrides (@xend) or MSR (@msr)
+ * to the result.
  */
 int xc_cpuid_apply_policy(xc_interface *xch,
                           uint32_t domid, bool restore,
                           const uint32_t *featureset,
                           unsigned int nr_features, bool pae, bool itsc,
-                          bool nested_virt, const struct xc_xend_cpuid *xend);
+                          bool nested_virt, const struct xc_xend_cpuid *xend,
+                          const struct xc_msr *msr);
 int xc_mca_op(xc_interface *xch, struct xen_mc *mc);
 int xc_mca_op_inject_v2(xc_interface *xch, unsigned int flags,
                         xc_cpumap_t cpumap, unsigned int nr_cpus);
diff --git a/tools/libs/guest/xg_cpuid_x86.c b/tools/libs/guest/xg_cpuid_x86.c
index 5b035223f4f5..f2b1e809011d 100644
--- a/tools/libs/guest/xg_cpuid_x86.c
+++ b/tools/libs/guest/xg_cpuid_x86.c
@@ -423,10 +423,170 @@ static int xc_cpuid_xend_policy(
     return rc;
 }
 
+static int compare_msr(const void *l, const void *r)
+{
+    const xen_msr_entry_t *lhs = l;
+    const xen_msr_entry_t *rhs = r;
+
+    if ( lhs->idx == rhs->idx )
+        return 0;
+
+    return lhs->idx < rhs->idx ? -1 : 1;
+}
+
+static xen_msr_entry_t *find_msr(
+    xen_msr_entry_t *msrs, unsigned int nr_msrs,
+    uint32_t index)
+{
+    const xen_msr_entry_t key = { .idx = index };
+
+    return bsearch(&key, msrs, nr_msrs, sizeof(*msrs), compare_msr);
+}
+
+
+static int xc_msr_policy(xc_interface *xch, domid_t domid,
+                         const struct xc_msr *msr)
+{
+    int rc;
+    bool hvm;
+    xc_domaininfo_t di;
+    unsigned int nr_leaves, nr_msrs;
+    uint32_t err_leaf = -1, err_subleaf = -1, err_msr = -1;
+    /*
+     * Three full policies.  The host, default for the domain type,
+     * and domain current.
+     */
+    xen_msr_entry_t *host = NULL, *def = NULL, *cur = NULL;
+    unsigned int nr_host, nr_def, nr_cur;
+
+    if ( (rc = xc_domain_getinfo_single(xch, domid, &di)) < 0 )
+    {
+        PERROR("Failed to obtain d%d info", domid);
+        rc = -errno;
+        goto out;
+    }
+    hvm = di.flags & XEN_DOMINF_hvm_guest;
+
+    rc = xc_cpu_policy_get_size(xch, &nr_leaves, &nr_msrs);
+    if ( rc )
+    {
+        PERROR("Failed to obtain policy info size");
+        rc = -errno;
+        goto out;
+    }
+
+    if ( (host = calloc(nr_msrs, sizeof(*host))) == NULL ||
+         (def  = calloc(nr_msrs, sizeof(*def)))  == NULL ||
+         (cur  = calloc(nr_msrs, sizeof(*cur)))  == NULL )
+    {
+        ERROR("Unable to allocate memory for %u CPUID leaves", nr_leaves);
+        rc = -ENOMEM;
+        goto out;
+    }
+
+    /* Get the domain's current policy. */
+    nr_leaves = 0;
+    nr_cur = nr_msrs;
+    rc = get_domain_cpu_policy(xch, domid, &nr_leaves, NULL, &nr_cur, cur);
+    if ( rc )
+    {
+        PERROR("Failed to obtain d%d current policy", domid);
+        rc = -errno;
+        goto out;
+    }
+
+    /* Get the domain type's default policy. */
+    nr_leaves = 0;
+    nr_def = nr_msrs;
+    rc = get_system_cpu_policy(xch, hvm ? XEN_SYSCTL_cpu_policy_hvm_default
+                                        : XEN_SYSCTL_cpu_policy_pv_default,
+                               &nr_leaves, NULL, &nr_def, def);
+    if ( rc )
+    {
+        PERROR("Failed to obtain %s def policy", hvm ? "hvm" : "pv");
+        rc = -errno;
+        goto out;
+    }
+
+    /* Get the host policy. */
+    nr_leaves = 0;
+    nr_host = nr_msrs;
+    rc = get_system_cpu_policy(xch, XEN_SYSCTL_cpu_policy_host,
+                               &nr_leaves, NULL, &nr_host, host);
+    if ( rc )
+    {
+        PERROR("Failed to obtain host policy");
+        rc = -errno;
+        goto out;
+    }
+
+    for ( ; msr->index != XC_MSR_INPUT_UNUSED; ++msr )
+    {
+        xen_msr_entry_t *cur_msr = find_msr(cur, nr_cur, msr->index);
+        const xen_msr_entry_t *def_msr = find_msr(def, nr_def, msr->index);
+        const xen_msr_entry_t *host_msr = find_msr(host, nr_host, msr->index);
+        unsigned int i;
+
+        if ( cur_msr == NULL || def_msr == NULL || host_msr == NULL )
+        {
+            ERROR("Missing MSR %#x", msr->index);
+            rc = -ENOENT;
+            goto out;
+        }
+
+        for ( i = 0; i < ARRAY_SIZE(msr->policy) - 1; i++ )
+        {
+            bool val;
+
+            if ( msr->policy[i] == '1' )
+                val = true;
+            else if ( msr->policy[i] == '0' )
+                val = false;
+            else if ( msr->policy[i] == 'x' )
+                val = test_bit(63 - i, &def_msr->val);
+            else if ( msr->policy[i] == 'k' )
+                val = test_bit(63 - i, &host_msr->val);
+            else
+            {
+                ERROR("MSR index %#x: bad character '%c' in policy string '%s'",
+                      msr->index, msr->policy[i], msr->policy);
+                rc = -EINVAL;
+                goto out;
+            }
+
+            if ( val )
+                set_bit(63 - i, &cur_msr->val);
+            else
+                clear_bit(63 - i, &cur_msr->val);
+        }
+    }
+
+    /* Feed the transformed policy back up to Xen. */
+    rc = xc_set_domain_cpu_policy(xch, domid, 0, NULL, nr_cur, cur,
+                                  &err_leaf, &err_subleaf, &err_msr);
+    if ( rc )
+    {
+        PERROR("Failed to set d%d's policy (err leaf %#x, subleaf %#x, msr %#x)",
+               domid, err_leaf, err_subleaf, err_msr);
+        rc = -errno;
+        goto out;
+    }
+
+    /* Success! */
+
+ out:
+    free(cur);
+    free(def);
+    free(host);
+
+    return rc;
+}
+
 int xc_cpuid_apply_policy(xc_interface *xch, uint32_t domid, bool restore,
                           const uint32_t *featureset, unsigned int nr_features,
                           bool pae, bool itsc, bool nested_virt,
-                          const struct xc_xend_cpuid *xend)
+                          const struct xc_xend_cpuid *xend,
+                          const struct xc_msr *msr)
 {
     int rc;
     bool hvm;
@@ -663,6 +823,13 @@ int xc_cpuid_apply_policy(xc_interface *xch, uint32_t domid, bool restore,
     if ( xend && (rc = xc_cpuid_xend_policy(xch, domid, xend)) )
         goto out;
 
+    if ( msr )
+    {
+        rc = xc_msr_policy(xch, domid, msr);
+        if ( rc )
+            goto out;
+    }
+
     rc = 0;
 
 out:
diff --git a/tools/libs/light/libxl_cpuid.c b/tools/libs/light/libxl_cpuid.c
index f5ce9f97959c..c96aeb3bce46 100644
--- a/tools/libs/light/libxl_cpuid.c
+++ b/tools/libs/light/libxl_cpuid.c
@@ -502,7 +502,7 @@ int libxl__cpuid_legacy(libxl_ctx *ctx, uint32_t domid, bool restore,
             info->tsc_mode == LIBXL_TSC_MODE_ALWAYS_EMULATE);
 
     r = xc_cpuid_apply_policy(ctx->xch, domid, restore, NULL, 0,
-                              pae, itsc, nested_virt, info->cpuid);
+                              pae, itsc, nested_virt, info->cpuid, NULL);
     if (r)
         LOGEVD(ERROR, -r, domid, "Failed to apply CPUID policy");
 
-- 
2.41.0