diff --git a/tzdriver/core/smc_smp.c b/tzdriver/core/smc_smp.c
index 32d25b2934d49de9f8fc588ce57e846f346ef829..4441599def0ef58172b9ce6882ddaaecaab5e492 100644
--- a/tzdriver/core/smc_smp.c
+++ b/tzdriver/core/smc_smp.c
@@ -1794,6 +1794,7 @@ clean:
 static int smc_svc_thread_fn(void *arg)
 {
 	(void)arg;
+	init_kthread_cpumask();
 	set_freezable();
 	while (!kthread_should_stop()) {
 		struct tc_ns_smc_cmd smc_cmd = { {0}, 0 };
diff --git a/tzdriver/include/internal_functions.h b/tzdriver/include/internal_functions.h
index 336da09e6ef5b62028ab0ced8bf939138584dccd..1a0677cac02134b13e68e53fa1727f4e7d10d24c 100644
--- a/tzdriver/include/internal_functions.h
+++ b/tzdriver/include/internal_functions.h
@@ -16,6 +16,7 @@
 #define INTERNAL_FUNCTIONS_H
 
 #include <linux/device.h>
+#include <linux/delay.h>
 #include <securec.h>
 #include "teek_ns_client.h"
 #include "teek_client_constants.h"
@@ -38,6 +39,24 @@ static inline void fault_monitor_end(void)
 #else
 static inline void init_kthread_cpumask(void)
 {
+#ifdef CONFIG_CPU_AFF_NR
+	unsigned int i;
+	struct cpumask cpu_mask;
+
+	for (i = 0; i < CONFIG_CPU_AFF_NR; i++)
+		cpumask_set_cpu(i, &cpu_mask);
+
+	if (cpumask_empty(&cpu_mask))
+		return;
+	if(set_cpus_allowed_ptr(current, &cpu_mask))
+		return;
+#ifdef CONFIG_CPU_ISOLATION_OPT
+	/*wait for cpu affined */
+	while (!cpumask_test_cpu(task_cpu(current), &cpu_mask)) {
+		usleep_range(100, 500);
+	}
+#endif /*CONFIG_CPU_ISOLATION_OPT*/
+#endif /*CONFIG_CPU_AFF_NR*/
 }
 
 static inline void tz_kthread_bind_mask(struct task_struct *kthread)