task: ASM reg func

Move registers_save/registers_restore in task module to assembly language.

Signed-off-by: Yuji Mano <yuji.mano@am.sony.com>
Acked-by: Kazunori Asayama <asayama@sm.sony.co.jp>

2 files changed, 107 insertions, 116 deletions

diff --git a/task/src/mpu/module/task_module.c b/task/src/mpu/module/task_module.c
index 67b3eb6..e087cbe 100644
--- a/task/src/mpu/module/task_module.c
+++ b/task/src/mpu/module/task_module.c
@@ -115,121 +115,8 @@ static void dma_context(uint32_t low_size, uint32_t high_size, int put)
 	dma_wait();
 }
 
-static void registers_save(void *ptr)
-{
-	/* push non-volatile registers on to the workload stack */
-	asm volatile (
-		"stqd	$80, -16(%[ptr]);"
-		"stqd	$81, -32(%[ptr]);"
-		"stqd	$82, -48(%[ptr]);"
-		"stqd	$83, -64(%[ptr]);"
-		"stqd	$84, -80(%[ptr]);"
-		"stqd	$85, -96(%[ptr]);"
-		"stqd	$86, -112(%[ptr]);"
-		"stqd	$87, -128(%[ptr]);"
-		"stqd	$88, -144(%[ptr]);"
-		"stqd	$89, -160(%[ptr]);"
-		"stqd	$90, -176(%[ptr]);"
-		"stqd	$91, -192(%[ptr]);"
-		"stqd	$92, -208(%[ptr]);"
-		"stqd	$93, -224(%[ptr]);"
-		"stqd	$94, -240(%[ptr]);"
-		"stqd	$95, -256(%[ptr]);"
-		"stqd	$96, -272(%[ptr]);"
-		"stqd	$97, -288(%[ptr]);"
-		"stqd	$98, -304(%[ptr]);"
-		"stqd	$99, -320(%[ptr]);"
-		"stqd	$100, -336(%[ptr]);"
-		"stqd	$101, -352(%[ptr]);"
-		"stqd	$102, -368(%[ptr]);"
-		"stqd	$103, -384(%[ptr]);"
-		"stqd	$104, -400(%[ptr]);"
-		"stqd	$105, -416(%[ptr]);"
-		"stqd	$106, -432(%[ptr]);"
-		"stqd	$107, -448(%[ptr]);"
-		"stqd	$108, -464(%[ptr]);"
-		"stqd	$109, -480(%[ptr]);"
-		"stqd	$110, -496(%[ptr]);"
-		"stqd	$111, -512(%[ptr]);"
-		"stqd	$112, -528(%[ptr]);"
-		"stqd	$113, -544(%[ptr]);"
-		"stqd	$114, -560(%[ptr]);"
-		"stqd	$115, -576(%[ptr]);"
-		"stqd	$116, -592(%[ptr]);"
-		"stqd	$117, -608(%[ptr]);"
-		"stqd	$118, -624(%[ptr]);"
-		"stqd	$119, -640(%[ptr]);"
-		"stqd	$120, -656(%[ptr]);"
-		"stqd	$121, -672(%[ptr]);"
-		"stqd	$122, -688(%[ptr]);"
-		"stqd	$123, -704(%[ptr]);"
-		"stqd	$124, -720(%[ptr]);"
-		"stqd	$125, -736(%[ptr]);"
-		"stqd	$126, -752(%[ptr]);"
-		"stqd	$127, -768(%[ptr]);"
-		: : [ptr] "r" (ptr));
-}
-
-static void registers_restore(void *ptr)
-{
-	/* pop non-volatile registers from saved workload stack */
-	asm volatile (
-		"lqd	$80, -16(%[ptr]);"
-		"lqd	$81, -32(%[ptr]);"
-		"lqd	$82, -48(%[ptr]);"
-		"lqd	$83, -64(%[ptr]);"
-		"lqd	$84, -80(%[ptr]);"
-		"lqd	$85, -96(%[ptr]);"
-		"lqd	$86, -112(%[ptr]);"
-		"lqd	$87, -128(%[ptr]);"
-		"lqd	$88, -144(%[ptr]);"
-		"lqd	$89, -160(%[ptr]);"
-		"lqd	$90, -176(%[ptr]);"
-		"lqd	$91, -192(%[ptr]);"
-		"lqd	$92, -208(%[ptr]);"
-		"lqd	$93, -224(%[ptr]);"
-		"lqd	$94, -240(%[ptr]);"
-		"lqd	$95, -256(%[ptr]);"
-		"lqd	$96, -272(%[ptr]);"
-		"lqd	$97, -288(%[ptr]);"
-		"lqd	$98, -304(%[ptr]);"
-		"lqd	$99, -320(%[ptr]);"
-		"lqd	$100, -336(%[ptr]);"
-		"lqd	$101, -352(%[ptr]);"
-		"lqd	$102, -368(%[ptr]);"
-		"lqd	$103, -384(%[ptr]);"
-		"lqd	$104, -400(%[ptr]);"
-		"lqd	$105, -416(%[ptr]);"
-		"lqd	$106, -432(%[ptr]);"
-		"lqd	$107, -448(%[ptr]);"
-		"lqd	$108, -464(%[ptr]);"
-		"lqd	$109, -480(%[ptr]);"
-		"lqd	$110, -496(%[ptr]);"
-		"lqd	$111, -512(%[ptr]);"
-		"lqd	$112, -528(%[ptr]);"
-		"lqd	$113, -544(%[ptr]);"
-		"lqd	$114, -560(%[ptr]);"
-		"lqd	$115, -576(%[ptr]);"
-		"lqd	$116, -592(%[ptr]);"
-		"lqd	$117, -608(%[ptr]);"
-		"lqd	$118, -624(%[ptr]);"
-		"lqd	$119, -640(%[ptr]);"
-		"lqd	$120, -656(%[ptr]);"
-		"lqd	$121, -672(%[ptr]);"
-		"lqd	$122, -688(%[ptr]);"
-		"lqd	$123, -704(%[ptr]);"
-		"lqd	$124, -720(%[ptr]);"
-		"lqd	$125, -736(%[ptr]);"
-		"lqd	$126, -752(%[ptr]);"
-		"lqd	$127, -768(%[ptr]);"
-		: : [ptr] "r" (ptr));
-}
-
 void __task_save(void *task_heap)
 {
-	/* save registers state */
-	registers_save(__task_stack);
-
 	/* save workload stack pointer */
 	task->stack = (uint32_t)__task_stack;
 
@@ -258,9 +145,6 @@ void __task_restore(int task_cached)
 
 	/* restore workload stack pointer */
 	__task_stack = (void *)task->stack;
-
-	/* restore registers state */
-	registers_restore(__task_stack);
 }
 
 static void task_yield(void *task_heap)
diff --git a/task/src/mpu/module/task_switch.S b/task/src/mpu/module/task_switch.S
index 192f9d8..a85fc99 100644
--- a/task/src/mpu/module/task_switch.S
+++ b/task/src/mpu/module/task_switch.S
@@ -96,6 +96,7 @@ task_save:
 
 	stqa	$SP, __task_stack		/* save task stack */
 	lqa	$SP, __module_stack		/* restore module stack */
+	brsl	$LR, __registers_save		/* save registers */
 	brsl	$LR, __task_save		/* call task save body */
 
 	lqa	$2, __task_stack		/* load saved task stack ptr */
@@ -116,6 +117,7 @@ task_restore:
 	ai	$SP, $SP, -32			/* push stack frame */
 
 	brsl	$LR, __task_restore		/* call task restore body */
+	brsl	$LR, __registers_restore	/* restore registers */
 	lqr	$SP, __task_stack		/* restore task stack */
 	sync					/* sync before execution */
 
@@ -124,3 +126,108 @@ task_restore:
 	bi	$LR				/* return from task_save call */
 
 .size	task_restore, .-task_restore
+
+
+__registers_save:
+	lqr	$75, __task_stack
+	stqd	$80, -16($75)
+	stqd	$81, -32($75)
+	stqd	$82, -48($75)
+	stqd	$83, -64($75)
+	stqd	$84, -80($75)
+	stqd	$85, -96($75)
+	stqd	$86, -112($75)
+	stqd	$87, -128($75)
+	stqd	$88, -144($75)
+	stqd	$89, -160($75)
+	stqd	$90, -176($75)
+	stqd	$91, -192($75)
+	stqd	$92, -208($75)
+	stqd	$93, -224($75)
+	stqd	$94, -240($75)
+	stqd	$95, -256($75)
+	stqd	$96, -272($75)
+	stqd	$97, -288($75)
+	stqd	$98, -304($75)
+	stqd	$99, -320($75)
+	stqd	$100, -336($75)
+	stqd	$101, -352($75)
+	stqd	$102, -368($75)
+	stqd	$103, -384($75)
+	stqd	$104, -400($75)
+	stqd	$105, -416($75)
+	stqd	$106, -432($75)
+	stqd	$107, -448($75)
+	stqd	$108, -464($75)
+	stqd	$109, -480($75)
+	stqd	$110, -496($75)
+	stqd	$111, -512($75)
+	stqd	$112, -528($75)
+	stqd	$113, -544($75)
+	stqd	$114, -560($75)
+	stqd	$115, -576($75)
+	stqd	$116, -592($75)
+	stqd	$117, -608($75)
+	stqd	$118, -624($75)
+	stqd	$119, -640($75)
+	stqd	$120, -656($75)
+	stqd	$121, -672($75)
+	stqd	$122, -688($75)
+	stqd	$123, -704($75)
+	stqd	$124, -720($75)
+	stqd	$125, -736($75)
+	stqd	$126, -752($75)
+	stqd	$127, -768($75)
+	bi	$LR
+
+__registers_restore:
+	lqr	$75, __task_stack
+	lqd	$80, -16($75)
+	lqd	$81, -32($75)
+	lqd	$82, -48($75)
+	lqd	$83, -64($75)
+	lqd	$84, -80($75)
+	lqd	$85, -96($75)
+	lqd	$86, -112($75)
+	lqd	$87, -128($75)
+	lqd	$88, -144($75)
+	lqd	$89, -160($75)
+	lqd	$90, -176($75)
+	lqd	$91, -192($75)
+	lqd	$92, -208($75)
+	lqd	$93, -224($75)
+	lqd	$94, -240($75)
+	lqd	$95, -256($75)
+	lqd	$96, -272($75)
+	lqd	$97, -288($75)
+	lqd	$98, -304($75)
+	lqd	$99, -320($75)
+	lqd	$100, -336($75)
+	lqd	$101, -352($75)
+	lqd	$102, -368($75)
+	lqd	$103, -384($75)
+	lqd	$104, -400($75)
+	lqd	$105, -416($75)
+	lqd	$106, -432($75)
+	lqd	$107, -448($75)
+	lqd	$108, -464($75)
+	lqd	$109, -480($75)
+	lqd	$110, -496($75)
+	lqd	$111, -512($75)
+	lqd	$112, -528($75)
+	lqd	$113, -544($75)
+	lqd	$114, -560($75)
+	lqd	$115, -576($75)
+	lqd	$116, -592($75)
+	lqd	$117, -608($75)
+	lqd	$118, -624($75)
+	lqd	$119, -640($75)
+	lqd	$120, -656($75)
+	lqd	$121, -672($75)
+	lqd	$122, -688($75)
+	lqd	$123, -704($75)
+	lqd	$124, -720($75)
+	lqd	$125, -736($75)
+	lqd	$126, -752($75)
+	lqd	$127, -768($75)
+	bi	$LR