From 1bc8de83ffe78d66f1ff7b4a9557c9e27f3c283b Mon Sep 17 00:00:00 2001
From: Martin Stein <martin.stein@genode-labs.com>
Date: Tue, 22 Jul 2014 15:11:40 +0200
Subject: [PATCH] hw & arm_v6: fix bug in calculation of kernel SP

For ARMv6, this aspect wasn't updatet to the new kernel-stack
managment that came up with SMP support.

ref #1199
---
 repos/base-hw/src/core/arm/macros.s           | 21 +++++++++++++++++--
 .../base-hw/src/core/arm_v6/mode_transition.s |  7 ++++---
 .../base-hw/src/core/arm_v7/mode_transition.s |  8 ++-----
 3 files changed, 25 insertions(+), 11 deletions(-)

diff --git a/repos/base-hw/src/core/arm/macros.s b/repos/base-hw/src/core/arm/macros.s
index 9f4945391e..f9d7dde328 100644
--- a/repos/base-hw/src/core/arm/macros.s
+++ b/repos/base-hw/src/core/arm/macros.s
@@ -14,12 +14,12 @@
 .include "macros_arm.s"
 
 /**
- * Determine the top of the kernel stack of this processor and apply it as SP
+ * Calculate and apply kernel SP for a given kernel-stacks area
  *
  * \base_reg  register that contains the base of the kernel-stacks area
  * \size_reg  register that contains the size of one kernel stack
  */
-.macro _init_kernel_sp base_reg, size_reg, buf_reg
+.macro _init_kernel_sp base_reg, size_reg
 
 	/* get kernel name of processor */
 	_get_processor_id sp
@@ -31,6 +31,23 @@
 .endm
 
 
+/**
+ * Restore kernel SP from a given kernel context
+ *
+ * \context_reg  register that contains the base of the kernel context
+ * \buf_reg_*    registers that can be used as local buffers
+ */
+.macro _restore_kernel_sp context_reg, buf_reg_0, buf_reg_1
+
+	/* get base of the kernel-stacks area and the kernel-stack size */
+	add sp, \context_reg, #R12_OFFSET
+	ldm sp, {\buf_reg_0, \buf_reg_1}
+
+	/* calculate and apply kernel SP */
+	_init_kernel_sp \buf_reg_1, \buf_reg_0
+.endm
+
+
 /***************************************************
  ** Constant values that are pretty commonly used **
  ***************************************************/
diff --git a/repos/base-hw/src/core/arm_v6/mode_transition.s b/repos/base-hw/src/core/arm_v6/mode_transition.s
index f912c0cd32..6a14cbb443 100644
--- a/repos/base-hw/src/core/arm_v6/mode_transition.s
+++ b/repos/base-hw/src/core/arm_v6/mode_transition.s
@@ -114,12 +114,13 @@
 	 */
 	cps #19
 
-	/* get kernel context pointer */
+	/* apply kernel sp */
 	adr r0, _mt_master_context_begin
+	_restore_kernel_sp r0, r1, r2
 
 	/* load kernel context */
-	add r0, r0, #SP_OFFSET
-	ldm r0, {sp, lr, pc}
+	add r0, r0, #LR_OFFSET
+	ldm r0, {lr, pc}
 
 .endm
 
diff --git a/repos/base-hw/src/core/arm_v7/mode_transition.s b/repos/base-hw/src/core/arm_v7/mode_transition.s
index 4b36d8f4ad..0c8327c662 100644
--- a/repos/base-hw/src/core/arm_v7/mode_transition.s
+++ b/repos/base-hw/src/core/arm_v7/mode_transition.s
@@ -397,13 +397,9 @@
 	 */
 	cps #SVC_MODE
 
-	/* get base of the kernel-stacks area and the kernel-stack size */
+	/* apply kernel sp */
 	adr r0, _mt_master_context_begin
-	add r1, r0, #R12_OFFSET
-	ldm r1, {r2, r3}
-
-	/* determine top of the kernel stack of this processor and apply it as SP */
-	_init_kernel_sp r3, r2
+	_restore_kernel_sp r0, r1, r2
 
 	/* apply kernel lr and kernel pc */
 	add r1, r0, #LR_OFFSET