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

@@ -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 **
  ***************************************************/

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
 

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