SPARC: implement ARCH_EXCEPT()

Introduce a new software trap 15 which is generated by the
ARCH_EXCEPT() function macro.

The handler for this software trap calls z_sparc_fatal_error() and
finally z_fatal_error() with "reason" and ESF as arguments.

Signed-off-by: Martin Åberg <martin.aberg@gaisler.com>

arch/sparc/core/fatal.c

@@ -23,10 +23,3 @@ FUNC_NORETURN void z_sparc_fatal_error(unsigned int reason,
 	z_fatal_error(reason, esf);
 	CODE_UNREACHABLE;
 }
-
-FUNC_NORETURN void _Fault(const z_arch_esf_t *esf)
-{
-	LOG_ERR("Trap tt=0x%02x", (esf->tbr >> 4) & 0xff);
-
-	z_sparc_fatal_error(K_ERR_CPU_EXCEPTION, esf);
-}

arch/sparc/core/fault_trap.S

@@ -9,6 +9,7 @@
 #include <offsets_short.h>
 #include <arch/sparc/sparc.h>
 
+GTEXT(__sparc_trap_except_reason)
 GTEXT(__sparc_trap_fault)
 
 /*
@@ -22,11 +23,14 @@ GTEXT(__sparc_trap_fault)
  * %l2: npc
  * %l6: tbr (set by trap code)
  * %fp: %sp of current register window at trap time
+ * %g1: reason
  *
  * This trap handler will trash some of the global registers, which is OK since
  * we will not return to where we trapped.
  */
-SECTION_FUNC(TEXT, __sparc_trap_fault)
+SECTION_FUNC(TEXT, __sparc_trap_except_reason)
+	mov	%g1, %l7
+.Ldoit:
 	/* We may have trapped into the invalid window. If so, make it valid. */
 	rd	%wim, %g2
 	srl	%g2, %l0, %g3
@@ -66,6 +70,7 @@ SECTION_FUNC(TEXT, __sparc_trap_fault)
 	 * %sp: %sp of interrupted task - ABI_frame - esf
 	 */
 
+	mov	%l7, %o0
 	/* Fill in the content of the exception stack frame */
 	st	%l1, [%sp + 96 + __z_arch_esf_t_pc_OFFSET]
 	st	%l2, [%sp + 96 + __z_arch_esf_t_npc_OFFSET]
@@ -76,11 +81,27 @@ SECTION_FUNC(TEXT, __sparc_trap_fault)
 	st	%g1, [%sp + 96 + __z_arch_esf_t_y_OFFSET]
 
 	/* Enable traps, raise PIL to mask all maskable interrupts. */
-	or	%l0, PSR_PIL, %o0
+	or	%l0, PSR_PIL, %o2
-	wr	%o0, PSR_ET, %psr
+	wr	%o2, PSR_ET, %psr
 	nop
 	nop
 	nop
-	/* Exception stack frame prepared earlier is the first argument. */
+	/*
-	call	_Fault
+	 * reason is the first argument.
-	 add	%sp, 96, %o0
+	 * Exception stack frame prepared earlier is the second argument.
+	 */
+	call	z_sparc_fatal_error
+	 add	%sp, 96, %o1
+
+
+/*
+ * Entry for trap we don't handle explicitly
+ *
+ * Just drop into __sparc_trap_except_reason with reason set to
+ * K_ERR_CPU_EXCEPTION. Note that "reason" is transported in %l7 of the
+ * trapped-into window and global %g1 is preserved.
+ */
+SECTION_FUNC(TEXT, __sparc_trap_fault)
+	b	.Ldoit
+	/* K_ERR_CPU_EXCEPTION */
+	 mov	%g0, %l7

arch/sparc/core/trap_table_mvt.S

@@ -29,7 +29,7 @@
 	rd	%psr, %l0; \
 	sethi	%hi(handler), %l4; \
 	jmp	%l4+%lo(handler); \
-	 nop;
+	 rd	%tbr, %l6;
 
 #define RESET_TRAP(handler) \
 	mov	%g0, %g4; \
@@ -168,7 +168,7 @@ __start:
 	SOFT_TRAP;                                    ! 12
 	IRQ_OFFLOAD_TRAP;                             ! 13
 	SOFT_TRAP;                                    ! 14
-	SOFT_TRAP;                                    ! 15
+	TRAP(__sparc_trap_except_reason);             ! 15
 	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 90 - 93
 	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 94 - 97
 	SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;   ! 98 - 9B

include/arch/sparc/arch.h

@@ -34,6 +34,7 @@
  */
 #define SPARC_SW_TRAP_FLUSH_WINDOWS     0x03
 #define SPARC_SW_TRAP_SET_PIL           0x09
+#define SPARC_SW_TRAP_EXCEPT            0x0F
 
 #ifndef _ASMLANGUAGE
 #include <sys/util.h>
@@ -111,6 +112,19 @@ struct __esf {
 
 typedef struct __esf z_arch_esf_t;
 
+#define ARCH_EXCEPT(reason_p)						\
+do {									\
+	register uint32_t _g1 __asm__("g1") = reason_p;			\
+									\
+	__asm__ volatile (						\
+		"ta %[vector]\n\t"					\
+		:							\
+		: [vector] "i" (SPARC_SW_TRAP_EXCEPT), "r" (_g1)	\
+		: "memory"						\
+	);								\
+	CODE_UNREACHABLE;						\
+} while (false)
+
 #ifdef __cplusplus
 }
 #endif