arc: trap handler, used by irq_offload, now handles thread switch

It was found that the test latency_measure, when compiled
for microkernel, would fail on the ARC. This because the
trap handler, used by irq_offload, wasn't supporting thread switching.
This submission adds the code to do that, and the code size is
bigger only when CONFIG_MICROKERNEL is defined.

To keep code a bit smaller, there is a trick exploited here where
the AE bit is cleared in the STATUS32 register and in AUX_IRQ_ACT,
bit 1 is set, to make it appear as if the machine has interrupted
at priority 1 level. It then can jump into some common interrupt
exit code for regular interrupts and perform an RTIE instruction
to switch into the new thread.

test/latency_measure/microkernel now passes.

Change-Id: I1872a80bb09a259814540567f51721203201679a
Signed-off-by: Chuck Jordan <cjordan@synopsys.com>

arch/arc/core/fault_s.S

@@ -49,6 +49,15 @@ GDATA(_firq_stack)
 SECTION_VAR(BSS, saved_stack_pointer)
 	.word 0
 
+#if CONFIG_NUM_IRQ_PRIO_LEVELS == 1
+#error "NUM_IRQ_PRIO_LEVELS==1 is not supported."
+/* The code below sets bit 1 in AUX_IRQ_ACT and thus requires
+ * priority 0 and 1 at a minimum. Supporting only 1 priority
+ * requires a change to this file but also changes to make
+ * FIRQ optional.
+ */
+#endif
+
 /*
  * @brief Fault handler installed in the fault and reserved vectors
  */
@@ -78,6 +87,15 @@ SECTION_SUBSEC_FUNC(TEXT,__fault,__ev_maligned)
 	 * a diagnostic message and halt.
 	 */
 
+#ifdef CONFIG_ARC_STACK_CHECKING
+	push_s r2
+	/* disable stack checking */
+	lr r2, [_ARC_V2_STATUS32]
+	bclr r2, r2, _ARC_V2_STATUS32_SC_BIT
+	kflag r2
+	pop_s r2
+#endif
+
 	st sp, [saved_stack_pointer]
 	mov_s sp, _firq_stack
 	add sp, sp, CONFIG_FIRQ_STACK_SIZE
@@ -85,6 +103,11 @@ SECTION_SUBSEC_FUNC(TEXT,__fault,__ev_maligned)
 	/* save caller saved registers */
 	_create_irq_stack_frame
 
+	lr r0,[_ARC_V2_ERSTATUS]
+	st_s r0, [sp, __tISF_status32_OFFSET]
+	lr r0,[_ARC_V2_ERET]
+	st_s r0, [sp, __tISF_pc_OFFSET] /* eret into pc */
+
 	jl _Fault
 
 	/* if _Fault returns, restore the registers */
@@ -107,19 +130,97 @@ SECTION_SUBSEC_FUNC(TEXT,__fault,__ev_trap)
 	 * a diagnostic message and halt.
 	 */
 
+#ifdef CONFIG_ARC_STACK_CHECKING
+	push_s r2
+	/* disable stack checking */
+	lr r2, [_ARC_V2_STATUS32]
+	bclr r2, r2, _ARC_V2_STATUS32_SC_BIT
+	kflag r2
+	pop_s r2
+#endif
+
+#ifndef CONFIG_MICROKERNEL
 	st sp, [saved_stack_pointer]
 	mov_s sp, _firq_stack
 	add sp, sp, CONFIG_FIRQ_STACK_SIZE
+#endif
 
 	/* save caller saved registers */
 	_create_irq_stack_frame
 
+	lr r0,[_ARC_V2_ERSTATUS]
+	st_s r0, [sp, __tISF_status32_OFFSET]
+	lr r0,[_ARC_V2_ERET]
+	st_s r0, [sp, __tISF_pc_OFFSET] /* eret into pc */
+
 	jl _irq_do_offload
 
+#ifdef CONFIG_MICROKERNEL
+	mov_s r1, _nanokernel
+	ld_s r2, [r1, __tNANO_current_OFFSET]
+#if CONFIG_NUM_IRQ_PRIO_LEVELS > 1
+	/* check if we're a nested interrupt: if so, let the
+	 * interrupted interrupt handle the reschedule
+	 */
+	lr r3, [_ARC_V2_AUX_IRQ_ACT]
+	/* the OS on ARCv2 always runs in kernel mode, so assume bit31 [U] in
+	 * AUX_IRQ_ACT is always 0: if the contents of AUX_IRQ_ACT is 0, it
+	 * means trap was taken from outside an interrupt handler.
+	 * But if it was inside, let that handler do the swap.
+	 */
+	breq r3, 0, _trap_check_for_swap
+_trap_return:
+	_pop_irq_stack_frame
+	rtie
+#endif
+
+.balign 4
+_trap_check_for_swap:
+	ld_s r0, [r2, __tTCS_flags_OFFSET]
+	and.f r0, r0, PREEMPTIBLE
+	bnz _e_check_if_a_fiber_is_ready
+	b _trap_return
+
+.balign 4
+_e_check_if_a_fiber_is_ready:
+	ld_s r0, [r1, __tNANO_fiber_OFFSET] /* incoming fiber in r0 */
+	brne r0, 0, _trap_reschedule
+	b _trap_return
+
+.balign 4
+_trap_reschedule:
+
+	_save_callee_saved_regs
+
+	st _CAUSE_RIRQ, [r2, __tTCS_relinquish_cause_OFFSET]
+	/* note: Ok to use _CAUSE_RIRQ since everything is saved */
+
+	ld_s r2, [r1, __tNANO_fiber_OFFSET]
+
+	st_s r2, [r1, __tNANO_current_OFFSET]
+	ld_s r3, [r2, __tTCS_link_OFFSET]
+	st_s r3, [r1, __tNANO_fiber_OFFSET]
+
+	/* clear AE bit to forget this was an exception */
+	lr r3, [_ARC_V2_STATUS32]
+	and r3,r3,(~_ARC_V2_STATUS32_AE)
+	kflag r3
+ 	/* pretend priority 1 int happened to use common handler */
+	lr r3, [_ARC_V2_AUX_IRQ_ACT]
+	or r3,r3,2
+	sr r3, [_ARC_V2_AUX_IRQ_ACT]
+
+	/* Assumption: r2 has current thread */
+	b _rirq_common_interrupt_swap
+#else
+	/* Nanokernel-only just returns from exception */
+
 	/* if _Fault returns, restore the registers */
 	_pop_irq_stack_frame
 
 	/* now restore the stack */
 	ld sp,[saved_stack_pointer]
 	rtie
+#endif
+
 #endif /* CONFIG_IRQ_OFFLOAD */

arch/arc/core/regular_irq.S

@@ -34,6 +34,16 @@
 
 GTEXT(_rirq_enter)
 GTEXT(_rirq_exit)
+GTEXT(_rirq_common_interrupt_swap)
+
+#if CONFIG_NUM_IRQ_PRIO_LEVELS > 2
+#error "NUM_IRQ_PRIO_LEVELS>2 is not supported."
+/*
+ * Nesting of Regularing interrupts is not yet supported.
+ * If your SOC supports more than 2, set this value to 2.
+ */
+#endif
+
 
 /**
  *
@@ -132,6 +142,9 @@ _rirq_reschedule:
 	ld_s r3, [r2, __tTCS_link_OFFSET]
 	st_s r3, [r1, __tNANO_fiber_OFFSET]
 
+_rirq_common_interrupt_swap:
+	/* r2 contains pointer to new thread */
+
 #ifdef CONFIG_ARC_STACK_CHECKING
 	/* Use stack top and down registers from restored context */
 	add r3, r2, __tTCS_NOFLOAT_SIZEOF