diff --git a/drivers/firmware/qcom_scm.c b/drivers/firmware/qcom_scm.c
index a861033616ee..75506a1bbcfc 100644
--- a/drivers/firmware/qcom_scm.c
+++ b/drivers/firmware/qcom_scm.c
@@ -17,6 +17,8 @@
 #include <linux/reset-controller.h>
 #include <linux/arm-smccc.h>
 
+#include <asm/smp_plat.h>
+
 #include "qcom_scm.h"
 
 static bool download_mode = IS_ENABLED(CONFIG_QCOM_SCM_DOWNLOAD_MODE_DEFAULT);
@@ -260,15 +262,36 @@ static bool __qcom_scm_is_call_available(struct device *dev, u32 svc_id,
 	return ret ? false : !!res.result[0];
 }
 
-/**
- * qcom_scm_set_warm_boot_addr() - Set the warm boot address for cpus
- * @entry: Entry point function for the cpus
- * @cpus: The cpumask of cpus that will use the entry point
- *
- * Set the Linux entry point for the SCM to transfer control to when coming
- * out of a power down. CPU power down may be executed on cpuidle or hotplug.
- */
-int qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
+static int __qcom_scm_set_boot_addr_mc(void *entry, const cpumask_t *cpus,
+				       unsigned int flags)
+{
+	struct qcom_scm_desc desc = {
+		.svc = QCOM_SCM_SVC_BOOT,
+		.cmd = QCOM_SCM_BOOT_SET_ADDR_MC,
+		.owner = ARM_SMCCC_OWNER_SIP,
+		.arginfo = QCOM_SCM_ARGS(6),
+	};
+	unsigned int cpu;
+	u64 map;
+
+	/* Need a device for DMA of the additional arguments */
+	if (!__scm || __get_convention() == SMC_CONVENTION_LEGACY)
+		return -EOPNOTSUPP;
+
+	desc.args[0] = virt_to_phys(entry);
+	for_each_cpu(cpu, cpus) {
+		map = cpu_logical_map(cpu);
+		desc.args[1] |= BIT(MPIDR_AFFINITY_LEVEL(map, 0));
+		desc.args[2] |= BIT(MPIDR_AFFINITY_LEVEL(map, 1));
+		desc.args[3] |= BIT(MPIDR_AFFINITY_LEVEL(map, 2));
+	}
+	desc.args[4] = ~0ULL; /* Reserved for affinity level 3 */
+	desc.args[5] = flags;
+
+	return qcom_scm_call(__scm->dev, &desc, NULL);
+}
+
+static int __qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
 {
 	int ret;
 	int flags = 0;
@@ -304,17 +327,28 @@ int qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
 
 	return ret;
 }
-EXPORT_SYMBOL(qcom_scm_set_warm_boot_addr);
 
 /**
- * qcom_scm_set_cold_boot_addr() - Set the cold boot address for cpus
+ * qcom_scm_set_warm_boot_addr() - Set the warm boot address for cpus
  * @entry: Entry point function for the cpus
  * @cpus: The cpumask of cpus that will use the entry point
  *
- * Set the cold boot address of the cpus. Any cpu outside the supported
- * range would be removed from the cpu present mask.
+ * Set the Linux entry point for the SCM to transfer control to when coming
+ * out of a power down. CPU power down may be executed on cpuidle or hotplug.
  */
-int qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
+int qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
+{
+	if (!cpus || cpumask_empty(cpus))
+		return -EINVAL;
+
+	if (__qcom_scm_set_boot_addr_mc(entry, cpus, QCOM_SCM_BOOT_MC_FLAG_WARMBOOT))
+		/* Fallback to old SCM call */
+		return __qcom_scm_set_warm_boot_addr(entry, cpus);
+	return 0;
+}
+EXPORT_SYMBOL(qcom_scm_set_warm_boot_addr);
+
+static int __qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
 {
 	int flags = 0;
 	int cpu;
@@ -331,9 +365,6 @@ int qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
 		.owner = ARM_SMCCC_OWNER_SIP,
 	};
 
-	if (!cpus || cpumask_empty(cpus))
-		return -EINVAL;
-
 	for_each_cpu(cpu, cpus) {
 		if (cpu < ARRAY_SIZE(scm_cb_flags))
 			flags |= scm_cb_flags[cpu];
@@ -346,6 +377,25 @@ int qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
 
 	return qcom_scm_call_atomic(__scm ? __scm->dev : NULL, &desc, NULL);
 }
+
+/**
+ * qcom_scm_set_cold_boot_addr() - Set the cold boot address for cpus
+ * @entry: Entry point function for the cpus
+ * @cpus: The cpumask of cpus that will use the entry point
+ *
+ * Set the cold boot address of the cpus. Any cpu outside the supported
+ * range would be removed from the cpu present mask.
+ */
+int qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
+{
+	if (!cpus || cpumask_empty(cpus))
+		return -EINVAL;
+
+	if (__qcom_scm_set_boot_addr_mc(entry, cpus, QCOM_SCM_BOOT_MC_FLAG_COLDBOOT))
+		/* Fallback to old SCM call */
+		return __qcom_scm_set_cold_boot_addr(entry, cpus);
+	return 0;
+}
 EXPORT_SYMBOL(qcom_scm_set_cold_boot_addr);
 
 /**
diff --git a/drivers/firmware/qcom_scm.h b/drivers/firmware/qcom_scm.h
index d92156ceb3ac..2a6a87b75231 100644
--- a/drivers/firmware/qcom_scm.h
+++ b/drivers/firmware/qcom_scm.h
@@ -78,8 +78,12 @@ extern int scm_legacy_call(struct device *dev, const struct qcom_scm_desc *desc,
 #define QCOM_SCM_BOOT_SET_ADDR		0x01
 #define QCOM_SCM_BOOT_TERMINATE_PC	0x02
 #define QCOM_SCM_BOOT_SET_DLOAD_MODE	0x10
+#define QCOM_SCM_BOOT_SET_ADDR_MC	0x11
 #define QCOM_SCM_BOOT_SET_REMOTE_STATE	0x0a
 #define QCOM_SCM_FLUSH_FLAG_MASK	0x3
+#define QCOM_SCM_BOOT_MC_FLAG_AARCH64	BIT(0)
+#define QCOM_SCM_BOOT_MC_FLAG_COLDBOOT	BIT(1)
+#define QCOM_SCM_BOOT_MC_FLAG_WARMBOOT	BIT(2)
 
 #define QCOM_SCM_SVC_PIL		0x02
 #define QCOM_SCM_PIL_PAS_INIT_IMAGE	0x01
