diff --git a/xen/arch/x86/psr.c b/xen/arch/x86/psr.c
index fb84223..70e7c41 100644
--- a/xen/arch/x86/psr.c
+++ b/xen/arch/x86/psr.c
@@ -204,6 +204,29 @@ static enum psr_feat_type psr_cbm_type_to_feat_type(enum cbm_type type)
     return feat_type;
 }
 
+static bool psr_check_cbm(unsigned int cbm_len, unsigned long cbm)
+{
+    unsigned int first_bit, zero_bit;
+
+    /* Set bits should only in the range of [0, cbm_len]. */
+    if ( cbm & (~0ul << cbm_len) )
+        return false;
+
+    /* At least one bit need to be set. */
+    if ( cbm == 0 )
+        return false;
+
+    first_bit = find_first_bit(&cbm, cbm_len);
+    zero_bit = find_next_zero_bit(&cbm, cbm_len, first_bit);
+
+    /* Set bits should be contiguous. */
+    if ( zero_bit < cbm_len &&
+         find_next_bit(&cbm, cbm_len, zero_bit) < cbm_len )
+        return false;
+
+    return true;
+}
+
 /* CAT common functions implementation. */
 static void cat_init_feature(const struct cpuid_leaf *regs,
                              struct feat_node *feat,
@@ -592,7 +615,14 @@ int psr_get_val(struct domain *d, unsigned int socket,
 /* Set value functions */
 static unsigned int get_cos_num(const struct psr_socket_info *info)
 {
-    return 0;
+    unsigned int num = 0, i;
+
+    /* Get all features total amount. */
+    for ( i = 0; i < ARRAY_SIZE(feat_props); i++ )
+        if ( feat_props[i] )
+            num += feat_props[i]->cos_num;
+
+    return num;
 }
 
 static int gather_val_array(uint32_t val[],
@@ -600,7 +630,47 @@ static int gather_val_array(uint32_t val[],
                             const struct psr_socket_info *info,
                             unsigned int old_cos)
 {
-    return -EINVAL;
+    unsigned int i;
+
+    if ( !val )
+        return -EINVAL;
+
+    /* Get all features current values according to old_cos. */
+    for ( i = 0; i < ARRAY_SIZE(info->features); i++ )
+    {
+        unsigned int cos = old_cos, j;
+        const struct feat_node *feat = info->features[i];
+        const struct feat_props *props = feat_props[i];
+
+        if ( !feat )
+            continue;
+
+        if ( !props )
+        {
+            ASSERT_UNREACHABLE();
+            return -ENOENT;
+        }
+
+        if ( array_len < props->cos_num )
+            return -ENOSPC;
+
+        /*
+         * If old_cos exceeds current feature's cos_max, we should get
+         * default value. So assign cos to 0 which stores default value.
+         */
+        if ( cos > feat->cos_max )
+            cos = 0;
+
+        /* Value getting order is same as feature array. */
+        for ( j = 0; j < props->cos_num; j++ )
+            val[j] = feat->cos_reg_val[cos * props->cos_num + j];
+
+        array_len -= props->cos_num;
+
+        val += props->cos_num;
+    }
+
+    return 0;
 }
 
 static int insert_val_into_array(uint32_t val[],
@@ -610,6 +680,60 @@ static int insert_val_into_array(uint32_t val[],
                                  enum cbm_type type,
                                  uint32_t new_val)
 {
+    const struct feat_node *feat;
+    const struct feat_props *props;
+    unsigned int i;
+
+    ASSERT(feat_type < PSR_SOCKET_FEAT_NUM);
+
+    /* Insert new value into array according to feature's position in array. */
+    for ( i = 0; i < feat_type; i++ )
+    {
+        if ( !info->features[i] )
+            continue;
+
+        props = feat_props[i];
+        if ( !props )
+        {
+            ASSERT_UNREACHABLE();
+            return -ENOENT;
+        }
+
+        if ( array_len <= props->cos_num )
+            return -ENOSPC;
+
+        array_len -= props->cos_num;
+
+        val += props->cos_num;
+    }
+
+    feat = info->features[feat_type];
+    if ( !feat )
+        return -ENOENT;
+
+    props = feat_props[feat_type];
+    if ( !props )
+    {
+        ASSERT_UNREACHABLE();
+        return -ENOENT;
+    }
+
+    if ( array_len < props->cos_num )
+        return -ENOSPC;
+
+    if ( !psr_check_cbm(feat->cbm_len, new_val) )
+        return -EINVAL;
+
+    /* Value setting position is same as feature array. */
+    for ( i = 0; i < props->cos_num; i++ )
+    {
+        if ( type == props->type[i] )
+        {
+            val[i] = new_val;
+            return 0;
+        }
+    }
+
     return -EINVAL;
 }
 
