Merge tag 'u-boot-atmel-fixes-2021.01-b' of https://gitlab.denx.de/u-boot/custodians...

[platform/kernel/u-boot.git] / arch / x86 / cpu / mtrr.c

diff --git a/arch/x86/cpu/mtrr.c b/arch/x86/cpu/mtrr.c
index 7ec0733..6f095c5 100644 (file)
--- a/arch/x86/cpu/mtrr.c
+++ b/arch/x86/cpu/mtrr.c
@@ -19,8 +19,10 @@
 #include <common.h>
 #include <cpu_func.h>
 #include <log.h>
+#include <sort.h>
 #include <asm/cache.h>
 #include <asm/io.h>
+#include <asm/mp.h>
 #include <asm/msr.h>
 #include <asm/mtrr.h>
 
@@ -63,10 +65,83 @@ static void set_var_mtrr(uint reg, uint type, uint64_t start, uint64_t size)
        wrmsrl(MTRR_PHYS_MASK_MSR(reg), mask | MTRR_PHYS_MASK_VALID);
 }
 
+void mtrr_read_all(struct mtrr_info *info)
+{
+       int reg_count = mtrr_get_var_count();
+       int i;
+
+       for (i = 0; i < reg_count; i++) {
+               info->mtrr[i].base = native_read_msr(MTRR_PHYS_BASE_MSR(i));
+               info->mtrr[i].mask = native_read_msr(MTRR_PHYS_MASK_MSR(i));
+       }
+}
+
+void mtrr_write_all(struct mtrr_info *info)
+{
+       int reg_count = mtrr_get_var_count();
+       struct mtrr_state state;
+       int i;
+
+       for (i = 0; i < reg_count; i++) {
+               mtrr_open(&state, true);
+               wrmsrl(MTRR_PHYS_BASE_MSR(i), info->mtrr[i].base);
+               wrmsrl(MTRR_PHYS_MASK_MSR(i), info->mtrr[i].mask);
+               mtrr_close(&state, true);
+       }
+}
+
+static void write_mtrrs(void *arg)
+{
+       struct mtrr_info *info = arg;
+
+       mtrr_write_all(info);
+}
+
+static void read_mtrrs(void *arg)
+{
+       struct mtrr_info *info = arg;
+
+       mtrr_read_all(info);
+}
+
+/**
+ * mtrr_copy_to_aps() - Copy the MTRRs from the boot CPU to other CPUs
+ *
+ * @return 0 on success, -ve on failure
+ */
+static int mtrr_copy_to_aps(void)
+{
+       struct mtrr_info info;
+       int ret;
+
+       ret = mp_run_on_cpus(MP_SELECT_BSP, read_mtrrs, &info);
+       if (ret == -ENXIO)
+               return 0;
+       else if (ret)
+               return log_msg_ret("bsp", ret);
+
+       ret = mp_run_on_cpus(MP_SELECT_APS, write_mtrrs, &info);
+       if (ret)
+               return log_msg_ret("bsp", ret);
+
+       return 0;
+}
+
+static int h_comp_mtrr(const void *p1, const void *p2)
+{
+       const struct mtrr_request *req1 = p1;
+       const struct mtrr_request *req2 = p2;
+
+       s64 diff = req1->start - req2->start;
+
+       return diff < 0 ? -1 : diff > 0 ? 1 : 0;
+}
+
 int mtrr_commit(bool do_caches)
 {
        struct mtrr_request *req = gd->arch.mtrr_req;
        struct mtrr_state state;
+       int ret;
        int i;
 
        debug("%s: enabled=%d, count=%d\n", __func__, gd->arch.has_mtrr,
@@ -77,17 +152,24 @@ int mtrr_commit(bool do_caches)
        debug("open\n");
        mtrr_open(&state, do_caches);
        debug("open done\n");
+       qsort(req, gd->arch.mtrr_req_count, sizeof(*req), h_comp_mtrr);
        for (i = 0; i < gd->arch.mtrr_req_count; i++, req++)
                set_var_mtrr(i, req->type, req->start, req->size);
 
        /* Clear the ones that are unused */
        debug("clear\n");
-       for (; i < MTRR_COUNT; i++)
+       for (; i < mtrr_get_var_count(); i++)
                wrmsrl(MTRR_PHYS_MASK_MSR(i), 0);
        debug("close\n");
        mtrr_close(&state, do_caches);
        debug("mtrr done\n");
 
+       if (gd->flags & GD_FLG_RELOC) {
+               ret = mtrr_copy_to_aps();
+               if (ret)
+                       return log_msg_ret("copy", ret);
+       }
+
        return 0;
 }
 
@@ -116,7 +198,7 @@ int mtrr_add_request(int type, uint64_t start, uint64_t size)
        return 0;
 }
 
-static int get_var_mtrr_count(void)
+int mtrr_get_var_count(void)
 {
        return msr_read(MSR_MTRR_CAP_MSR).lo & MSR_MTRR_CAP_VCNT;
 }
@@ -127,7 +209,7 @@ static int get_free_var_mtrr(void)
        int vcnt;
        int i;
 
-       vcnt = get_var_mtrr_count();
+       vcnt = mtrr_get_var_count();
 
        /* Identify the first var mtrr which is not valid */
        for (i = 0; i < vcnt; i++) {
@@ -153,3 +235,84 @@ int mtrr_set_next_var(uint type, uint64_t start, uint64_t size)
 
        return 0;
 }
+
+/** enum mtrr_opcode - supported operations for mtrr_do_oper() */
+enum mtrr_opcode {
+       MTRR_OP_SET,
+       MTRR_OP_SET_VALID,
+};
+
+/**
+ * struct mtrr_oper - An MTRR operation to perform on a CPU
+ *
+ * @opcode: Indicates operation to perform
+ * @reg: MTRR reg number to select (0-7, -1 = all)
+ * @valid: Valid value to write for MTRR_OP_SET_VALID
+ * @base: Base value to write for MTRR_OP_SET
+ * @mask: Mask value to write for MTRR_OP_SET
+ */
+struct mtrr_oper {
+       enum mtrr_opcode opcode;
+       int reg;
+       bool valid;
+       u64 base;
+       u64 mask;
+};
+
+static void mtrr_do_oper(void *arg)
+{
+       struct mtrr_oper *oper = arg;
+       u64 mask;
+
+       switch (oper->opcode) {
+       case MTRR_OP_SET_VALID:
+               mask = native_read_msr(MTRR_PHYS_MASK_MSR(oper->reg));
+               if (oper->valid)
+                       mask |= MTRR_PHYS_MASK_VALID;
+               else
+                       mask &= ~MTRR_PHYS_MASK_VALID;
+               wrmsrl(MTRR_PHYS_MASK_MSR(oper->reg), mask);
+               break;
+       case MTRR_OP_SET:
+               wrmsrl(MTRR_PHYS_BASE_MSR(oper->reg), oper->base);
+               wrmsrl(MTRR_PHYS_MASK_MSR(oper->reg), oper->mask);
+               break;
+       }
+}
+
+static int mtrr_start_op(int cpu_select, struct mtrr_oper *oper)
+{
+       struct mtrr_state state;
+       int ret;
+
+       mtrr_open(&state, true);
+       ret = mp_run_on_cpus(cpu_select, mtrr_do_oper, oper);
+       mtrr_close(&state, true);
+       if (ret)
+               return log_msg_ret("run", ret);
+
+       return 0;
+}
+
+int mtrr_set_valid(int cpu_select, int reg, bool valid)
+{
+       struct mtrr_oper oper;
+
+       oper.opcode = MTRR_OP_SET_VALID;
+       oper.reg = reg;
+       oper.valid = valid;
+
+       return mtrr_start_op(cpu_select, &oper);
+}
+
+int mtrr_set(int cpu_select, int reg, u64 base, u64 mask)
+{
+       struct mtrr_oper oper;
+
+       oper.opcode = MTRR_OP_SET;
+       oper.reg = reg;
+       oper.base = base;
+       oper.mask = mask;
+
+       return mtrr_start_op(cpu_select, &oper);
+}