3 files changed, 29 insertions, 9 deletions
diff --git a/src/cpu/x86/mp_init.c b/src/cpu/x86/mp_init.c
index ef576ec26d..a696cd0a5c 100644
--- a/src/cpu/x86/mp_init.c
+++ b/src/cpu/x86/mp_init.c
@@ -43,6 +43,7 @@
 struct mp_callback {
 	void (*func)(void *);
 	void *arg;
+	int logical_cpu_number;
 };
 
 /*
@@ -924,11 +925,13 @@ static void ap_wait_for_instruction(void)
 {
 	struct mp_callback lcb;
 	struct mp_callback **per_cpu_slot;
+	int cur_cpu;
 
 	if (!IS_ENABLED(CONFIG_PARALLEL_MP_AP_WORK))
 		return;
 
-	per_cpu_slot = &ap_callbacks[cpu_index()];
+	cur_cpu = cpu_index();
+	per_cpu_slot = &ap_callbacks[cur_cpu];
 
 	while (1) {
 		struct mp_callback *cb = read_callback(per_cpu_slot);
@@ -942,13 +945,19 @@ static void ap_wait_for_instruction(void)
 		memcpy(&lcb, cb, sizeof(lcb));
 		mfence();
 		store_callback(per_cpu_slot, NULL);
-		lcb.func(lcb.arg);
+		if (lcb.logical_cpu_number && (cur_cpu !=
+				lcb.logical_cpu_number))
+			continue;
+		else
+			lcb.func(lcb.arg);
 	}
 }
 
-int mp_run_on_aps(void (*func)(void *), void *arg, long expire_us)
+int mp_run_on_aps(void (*func)(void *), void *arg, int logical_cpu_num,
+		long expire_us)
 {
-	struct mp_callback lcb = { .func = func, .arg = arg };
+	struct mp_callback lcb = { .func = func, .arg = arg,
+				.logical_cpu_number = logical_cpu_num};
 	return run_ap_work(&lcb, expire_us);
 }
 
@@ -957,7 +966,7 @@ int mp_run_on_all_cpus(void (*func)(void *), void *arg, long expire_us)
 	/* Run on BSP first. */
 	func(arg);
 
-	return mp_run_on_aps(func, arg, expire_us);
+	return mp_run_on_aps(func, arg, MP_RUN_ON_ALL_CPUS, expire_us);
 }
 
 int mp_park_aps(void)
@@ -968,7 +977,8 @@ int mp_park_aps(void)
 
 	stopwatch_init(&sw);
 
-	ret = mp_run_on_aps(park_this_cpu, NULL, 250 * USECS_PER_MSEC);
+	ret = mp_run_on_aps(park_this_cpu, NULL, MP_RUN_ON_ALL_CPUS,
+				250 * USECS_PER_MSEC);
 
 	duration_msecs = stopwatch_duration_msecs(&sw);
 
diff --git a/src/include/cpu/x86/mp.h b/src/include/cpu/x86/mp.h
index fba3e75af7..c04252ec35 100644
--- a/src/include/cpu/x86/mp.h
+++ b/src/include/cpu/x86/mp.h
@@ -116,6 +116,11 @@ struct mp_ops {
  */
 int mp_init_with_smm(struct bus *cpu_bus, const struct mp_ops *mp_ops);
 
+enum {
+	/* Function runs on all cores (both BSP and APs) */
+	MP_RUN_ON_ALL_CPUS,
+	/* Need to specify cores (only on APs) numbers */
+};
 
 /*
  * After APs are up and PARALLEL_MP_AP_WORK is enabled one can issue work
@@ -123,10 +128,13 @@ int mp_init_with_smm(struct bus *cpu_bus, const struct mp_ops *mp_ops);
  * to issue work. i.e. the APs should not call any of these functions.
  *
  * Input parameter expire_us <= 0 to specify an infinite timeout.
+ * logical_cpu_num = MP_RUN_ON_ALL_CPUS to execute function over all cores (BSP
+ * + APs) else specified AP number using logical_cpu_num.
  *
  * All functions return < 0 on error, 0 on success.
  */
-int mp_run_on_aps(void (*func)(void *), void *arg, long expire_us);
+int mp_run_on_aps(void (*func)(void *), void *arg, int logical_cpu_num,
+		long expire_us);
 
 /* Like mp_run_on_aps() but also runs func on BSP. */
 int mp_run_on_all_cpus(void (*func)(void *), void *arg, long expire_us);
diff --git a/src/soc/amd/common/block/pi/def_callouts.c b/src/soc/amd/common/block/pi/def_callouts.c
index 0afa82517d..2c17a3f8a5 100644
--- a/src/soc/amd/common/block/pi/def_callouts.c
+++ b/src/soc/amd/common/block/pi/def_callouts.c
@@ -219,7 +219,8 @@ AGESA_STATUS agesa_RunFuncOnAp(UINT32 Func, UINTN Data, VOID *ConfigPtr)
 	agesadata.Func = Func;
 	agesadata.Data = Data;
 	agesadata.ConfigPtr = ConfigPtr;
-	mp_run_on_aps(callout_ap_entry, NULL, 100 * USECS_PER_MSEC);
+	mp_run_on_aps(callout_ap_entry, NULL, MP_RUN_ON_ALL_CPUS,
+			100 * USECS_PER_MSEC);
 
 	return AGESA_SUCCESS;
 }
@@ -231,7 +232,8 @@ AGESA_STATUS agesa_RunFcnOnAllAps(UINT32 Func, UINTN Data, VOID *ConfigPtr)
 	agesadata.Func = Func;
 	agesadata.Data = Data;
 	agesadata.ConfigPtr = ConfigPtr;
-	mp_run_on_aps(callout_ap_entry, NULL, 100 * USECS_PER_MSEC);
+	mp_run_on_aps(callout_ap_entry, NULL, MP_RUN_ON_ALL_CPUS,
+			100 * USECS_PER_MSEC);
 
 	return AGESA_SUCCESS;
 }