arm: implement __svc on Cortex M0

This is needed for irq_offload() and k_oops()/k_panic() Issue: ZEP-2221 Signed-off-by: Andrew Boie <[email protected]>
rsalveti · Jun 2, 2017 · a2156fe · a2156fe
1 parent 5b4867b
commit a2156fe
Show file tree

Hide file tree

Showing 4 changed files with 59 additions and 10 deletions.
diff --git a/arch/arm/core/cortex_m/vector_table.S b/arch/arm/core/cortex_m/vector_table.S
@@ -47,7 +47,7 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table)
     .word __reserved
     .word __reserved
     .word __reserved
-    .word __reserved	/* SVC not used for now (PendSV used instead) */
+    .word __svc
     .word __reserved
 #elif defined(CONFIG_ARMV7_M)
     .word __mpu_fault

diff --git a/arch/arm/core/cortex_m/vector_table.h b/arch/arm/core/cortex_m/vector_table.h
@@ -39,6 +39,7 @@ GTEXT(__reset)
 GTEXT(__nmi)
 GTEXT(__hard_fault)
 #if defined(CONFIG_ARMV6_M)
+GTEXT(__svc)
 #elif defined(CONFIG_ARMV7_M)
 GTEXT(__mpu_fault)
 GTEXT(__bus_fault)

diff --git a/arch/arm/core/irq_offload.c b/arch/arm/core/irq_offload.c
@@ -22,9 +22,17 @@ void _irq_do_offload(void)
 
 void irq_offload(irq_offload_routine_t routine, void *parameter)
 {
-	int key;
+#if defined(CONFIG_ARMV6_M) && defined(CONFIG_ASSERT)
+	/* Cortex M0 hardfaults if you make a SVC call with interrupts
+	 * locked.
+	 */
+	unsigned int key;
 
-	key = irq_lock();
+	__asm__ volatile("mrs %0, PRIMASK;" : "=r" (key) : : "memory");
+	__ASSERT(key == 0, "irq_offload called with interrupts locked\n");
+#endif
+
+	k_sched_lock();
 	offload_routine = routine;
 	offload_param = parameter;
 
@@ -34,6 +42,5 @@ void irq_offload(irq_offload_routine_t routine, void *parameter)
 			  : "memory");
 
 	offload_routine = NULL;
-
-	irq_unlock(key);
+	k_sched_unlock();
 }
diff --git a/arch/arm/core/swap.S b/arch/arm/core/swap.S
@@ -20,12 +20,7 @@
 _ASM_FILE_PROLOGUE
 
 GTEXT(__swap)
-#if defined(CONFIG_ARMV6_M)
-#elif defined(CONFIG_ARMV7_M)
 GTEXT(__svc)
-#else
-#error Unknown ARM architecture
-#endif /* CONFIG_ARMV6_M */
 GTEXT(__pendsv)
 GTEXT(_do_kernel_oops)
 GDATA(_k_neg_eagain)
@@ -217,6 +212,52 @@ _thread_irq_disabled:
     bx lr
 
 #if defined(CONFIG_ARMV6_M)
+SECTION_FUNC(TEXT, __svc)
+    /* Use EXC_RETURN state to find out if stack frame is on the
+     * MSP or PSP
+     */
+    ldr r0, =0x4
+    mov r1, lr
+    tst r1, r0
+    beq _stack_frame_msp
+    mrs r0, PSP
+    bne _stack_frame_endif
+_stack_frame_msp:
+    mrs r0, MSP
+_stack_frame_endif:
+
+    /* Figure out what SVC call number was invoked */
+    ldr r1, [r0, #24]   /* grab address of PC from stack frame */
+    /* SVC is a two-byte instruction, point to it and read  encoding */
+    subs r1, r1, #2
+    ldrb r1, [r1, #0]
+
+   /*
+    * grab service call number:
+    * 1: irq_offload (if configured)
+    * 2: kernel panic or oops (software generated fatal exception)
+    * Planned implementation of system calls for memory protection will
+    * expand this case.
+    */
+
+    cmp r1, #2
+    beq _oops
+
+#if CONFIG_IRQ_OFFLOAD
+    push {lr}
+    blx _irq_do_offload  /* call C routine which executes the offload */
+    pop {r3}
+    mov lr, r3
+#endif
+
+    /* exception return is done in _IntExit() */
+    b _IntExit
+
+_oops:
+    push {lr}
+    blx _do_kernel_oops
+    pop {pc}
+
 #elif defined(CONFIG_ARMV7_M)
 /**
  *