cpukit/microblaze: Add exception extensions

Add the functions necessary to support RTEMS_EXCEPTION_EXTENSIONS and
mark this functionality as available on MicroBlaze.

3 files changed, 388 insertions, 0 deletions

diff --git a/cpukit/score/cpu/microblaze/include/rtems/score/cpu.h b/cpukit/score/cpu/microblaze/include/rtems/score/cpu.h
index fe0d9a8996..5ca0609e91 100644
--- a/cpukit/score/cpu/microblaze/include/rtems/score/cpu.h
+++ b/cpukit/score/cpu/microblaze/include/rtems/score/cpu.h
@@ -194,6 +194,10 @@ typedef struct {
 #define MICROBLAZE_MSR_C   ( 1 << 2 )
 #define MICROBLAZE_MSR_IE  ( 1 << 1 )
 
+#define MICROBLAZE_ESR_DS  ( 1 << 12 )
+#define MICROBLAZE_ESR_EC_MASK   0x1f
+#define MICROBLAZE_ESR_ESS_MASK  0x7f
+#define MICROBLAZE_ESR_ESS_SHIFT 5
 
 #define _CPU_MSR_GET( _msr_value ) \
   do { \
@@ -358,6 +362,40 @@ void _CPU_Context_switch(
   Context_Control  *heir
 );
 
+/* Selects the appropriate resume function based on CEF state */
+RTEMS_NO_RETURN void _CPU_Exception_resume( CPU_Exception_frame *frame );
+
+RTEMS_NO_RETURN void _MicroBlaze_Exception_resume_from_exception(
+  CPU_Exception_frame *frame
+);
+
+RTEMS_NO_RETURN void _MicroBlaze_Exception_resume_from_break(
+  CPU_Exception_frame *frame
+);
+
+/*
+ * Only functions for exception cases since debug exception frames will never
+ * need dispatch
+ */
+RTEMS_NO_RETURN void _CPU_Exception_dispatch_and_resume(
+  CPU_Exception_frame *frame
+);
+
+void _CPU_Exception_disable_thread_dispatch( void );
+
+int _CPU_Exception_frame_get_signal( CPU_Exception_frame *frame );
+
+void _CPU_Exception_frame_set_resume(
+  CPU_Exception_frame *frame,
+  void                *address
+);
+
+void _CPU_Exception_frame_make_resume_next_instruction(
+  CPU_Exception_frame *frame
+);
+
+uint32_t *_MicroBlaze_Get_return_address( CPU_Exception_frame *ef );
+
 RTEMS_NO_RETURN void _CPU_Context_restore(
   Context_Control *new_context
 );
diff --git a/cpukit/score/cpu/microblaze/microblaze-exception-extensions.S b/cpukit/score/cpu/microblaze/microblaze-exception-extensions.S
new file mode 100644
index 0000000000..252e901393
--- /dev/null
+++ b/cpukit/score/cpu/microblaze/microblaze-exception-extensions.S
@@ -0,0 +1,177 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ * @file
+ *
+ * @ingroup RTEMSScoreCPUMicroBlaze
+ *
+ * @brief MicroBlaze exception extensions ASM implementation
+ */
+
+/*
+ * Copyright (C) 2022 On-Line Applications Research Corporation (OAR)
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <rtems/asm.h>
+#include <rtems/score/percpu.h>
+
+	.text
+        .globl _CPU_Exception_dispatch_and_resume
+        .globl _MicroBlaze_Exception_resume_from_exception
+        .globl _MicroBlaze_Exception_resume_from_break
+	.align 2
+
+_CPU_Exception_dispatch_and_resume:
+	/* Subtract 1 from ISR_NEST_LEVEL */
+	lwi r3, r0, _Per_CPU_Information + 8
+	addik r3, r3, -1
+	swi r3, r0, _Per_CPU_Information + 8
+
+	/* Subtract 1 from THREAD_DISPATCH_DISABLE_LEVEL */
+	lwi r3, r0, _Per_CPU_Information + 16
+	addik r3, r3, -1
+	swi r3, r0, _Per_CPU_Information + 16
+
+	/* Is THREAD_DISPATCH_DISABLE_LEVEL != 0? */
+	bnei r3, _MicroBlaze_Exception_resume_from_exception
+
+	/* Is DISPATCH_NEEDED == 0? */
+	lwi r3, r0, _Per_CPU_Information + 20
+	beqi r3, _MicroBlaze_Exception_resume_from_exception
+
+	bralid r15, _Thread_Dispatch
+	nop
+/* Fall through to restore exception frame */
+
+_MicroBlaze_Exception_resume_from_exception:
+	/* Move argument to stack pointer */
+	addi r1, r5, 0
+
+	/* Retrieve and store MSR */
+	lwi r3, r1, MICROBLAZE_EXCEPTION_FRAME_MSR
+	mts rmsr, r3
+
+	/* Retrieve and store EAR */
+	lwi r3, r1, MICROBLAZE_EXCEPTION_FRAME_EAR
+	mts rear, r3
+
+	/* Retrieve and store ESR */
+	lwi r3, r1, MICROBLAZE_EXCEPTION_FRAME_ESR
+	mts resr, r3
+
+	/* Restore program state */
+	lwi  r2, r1, MICROBLAZE_EXCEPTION_FRAME_R2
+	lwi  r3, r1, MICROBLAZE_EXCEPTION_FRAME_R3
+	lwi  r4, r1, MICROBLAZE_EXCEPTION_FRAME_R4
+	lwi  r5, r1, MICROBLAZE_EXCEPTION_FRAME_R5
+	lwi  r6, r1, MICROBLAZE_EXCEPTION_FRAME_R6
+	lwi  r7, r1, MICROBLAZE_EXCEPTION_FRAME_R7
+	lwi  r8, r1, MICROBLAZE_EXCEPTION_FRAME_R8
+	lwi  r9, r1, MICROBLAZE_EXCEPTION_FRAME_R9
+	lwi r10, r1, MICROBLAZE_EXCEPTION_FRAME_R10
+	lwi r11, r1, MICROBLAZE_EXCEPTION_FRAME_R11
+	lwi r12, r1, MICROBLAZE_EXCEPTION_FRAME_R12
+	lwi r13, r1, MICROBLAZE_EXCEPTION_FRAME_R13
+	lwi r14, r1, MICROBLAZE_EXCEPTION_FRAME_R14
+	lwi r15, r1, MICROBLAZE_EXCEPTION_FRAME_R15
+	lwi r16, r1, MICROBLAZE_EXCEPTION_FRAME_R16
+	lwi r17, r1, MICROBLAZE_EXCEPTION_FRAME_R17
+	lwi r18, r1, MICROBLAZE_EXCEPTION_FRAME_R18
+	lwi r19, r1, MICROBLAZE_EXCEPTION_FRAME_R19
+	lwi r20, r1, MICROBLAZE_EXCEPTION_FRAME_R20
+	lwi r21, r1, MICROBLAZE_EXCEPTION_FRAME_R21
+	lwi r22, r1, MICROBLAZE_EXCEPTION_FRAME_R22
+	lwi r23, r1, MICROBLAZE_EXCEPTION_FRAME_R23
+	lwi r24, r1, MICROBLAZE_EXCEPTION_FRAME_R24
+	lwi r25, r1, MICROBLAZE_EXCEPTION_FRAME_R25
+	lwi r26, r1, MICROBLAZE_EXCEPTION_FRAME_R26
+	lwi r27, r1, MICROBLAZE_EXCEPTION_FRAME_R27
+	lwi r28, r1, MICROBLAZE_EXCEPTION_FRAME_R28
+	lwi r29, r1, MICROBLAZE_EXCEPTION_FRAME_R29
+	lwi r30, r1, MICROBLAZE_EXCEPTION_FRAME_R30
+	lwi r31, r1, MICROBLAZE_EXCEPTION_FRAME_R31
+
+	/* Free stack space */
+	addik r1, r1, CPU_EXCEPTION_FRAME_SIZE
+
+	/* Return from exception mode */
+	/* Branch to BTR is handled by upper layers */
+	rted r17, 0
+	nop
+
+/* There is no dispatch version of resume from break */
+_MicroBlaze_Exception_resume_from_break:
+	/* Move argument to stack pointer */
+	addi r1, r5, 0
+
+	/* Retrieve and store MSR */
+	lwi r3, r1, MICROBLAZE_EXCEPTION_FRAME_MSR
+	mts rmsr, r3
+
+	/* Retrieve and store EAR */
+	lwi r3, r1, MICROBLAZE_EXCEPTION_FRAME_EAR
+	mts rear, r3
+
+	/* Retrieve and store ESR */
+	lwi r3, r1, MICROBLAZE_EXCEPTION_FRAME_ESR
+	mts resr, r3
+
+	/* Restore program state */
+	lwi  r2, r1, MICROBLAZE_EXCEPTION_FRAME_R2
+	lwi  r3, r1, MICROBLAZE_EXCEPTION_FRAME_R3
+	lwi  r4, r1, MICROBLAZE_EXCEPTION_FRAME_R4
+	lwi  r5, r1, MICROBLAZE_EXCEPTION_FRAME_R5
+	lwi  r6, r1, MICROBLAZE_EXCEPTION_FRAME_R6
+	lwi  r7, r1, MICROBLAZE_EXCEPTION_FRAME_R7
+	lwi  r8, r1, MICROBLAZE_EXCEPTION_FRAME_R8
+	lwi  r9, r1, MICROBLAZE_EXCEPTION_FRAME_R9
+	lwi r10, r1, MICROBLAZE_EXCEPTION_FRAME_R10
+	lwi r11, r1, MICROBLAZE_EXCEPTION_FRAME_R11
+	lwi r12, r1, MICROBLAZE_EXCEPTION_FRAME_R12
+	lwi r13, r1, MICROBLAZE_EXCEPTION_FRAME_R13
+	lwi r14, r1, MICROBLAZE_EXCEPTION_FRAME_R14
+	lwi r15, r1, MICROBLAZE_EXCEPTION_FRAME_R15
+	lwi r16, r1, MICROBLAZE_EXCEPTION_FRAME_R16
+	lwi r17, r1, MICROBLAZE_EXCEPTION_FRAME_R17
+	lwi r18, r1, MICROBLAZE_EXCEPTION_FRAME_R18
+	lwi r19, r1, MICROBLAZE_EXCEPTION_FRAME_R19
+	lwi r20, r1, MICROBLAZE_EXCEPTION_FRAME_R20
+	lwi r21, r1, MICROBLAZE_EXCEPTION_FRAME_R21
+	lwi r22, r1, MICROBLAZE_EXCEPTION_FRAME_R22
+	lwi r23, r1, MICROBLAZE_EXCEPTION_FRAME_R23
+	lwi r24, r1, MICROBLAZE_EXCEPTION_FRAME_R24
+	lwi r25, r1, MICROBLAZE_EXCEPTION_FRAME_R25
+	lwi r26, r1, MICROBLAZE_EXCEPTION_FRAME_R26
+	lwi r27, r1, MICROBLAZE_EXCEPTION_FRAME_R27
+	lwi r28, r1, MICROBLAZE_EXCEPTION_FRAME_R28
+	lwi r29, r1, MICROBLAZE_EXCEPTION_FRAME_R29
+	lwi r30, r1, MICROBLAZE_EXCEPTION_FRAME_R30
+	lwi r31, r1, MICROBLAZE_EXCEPTION_FRAME_R31
+
+	/* Free stack space */
+	addik r1, r1, CPU_EXCEPTION_FRAME_SIZE
+
+	/* Return from debug mode */
+	rtbd r16, 0
+	nop
diff --git a/cpukit/score/cpu/microblaze/microblaze-exception-extensions.c b/cpukit/score/cpu/microblaze/microblaze-exception-extensions.c
new file mode 100644
index 0000000000..78d65106d3
--- /dev/null
+++ b/cpukit/score/cpu/microblaze/microblaze-exception-extensions.c
@@ -0,0 +1,173 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ * @file
+ *
+ * @ingroup RTEMSScoreCPUMicroBlaze
+ *
+ * @brief MicroBlaze exception extensions implementation
+ */
+
+/*
+ * Copyright (C) 2022 On-Line Applications Research Corporation (OAR)
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <rtems/fatal.h>
+#include <rtems/score/threadimpl.h>
+
+RTEMS_NO_RETURN void _CPU_Exception_resume( CPU_Exception_frame *frame )
+{
+  /* Break in progress */
+  if ( ( frame->msr & MICROBLAZE_MSR_BIP ) != 0 ) {
+    _MicroBlaze_Exception_resume_from_break( frame );
+  }
+
+  /* Exception in progress */
+  if ( ( frame->msr & MICROBLAZE_MSR_EIP ) != 0 ) {
+    _MicroBlaze_Exception_resume_from_exception( frame );
+  }
+
+  /* Execution should never reach this point */
+  rtems_fatal( RTEMS_FATAL_SOURCE_EXCEPTION, (rtems_fatal_code) frame );
+}
+
+void _CPU_Exception_disable_thread_dispatch( void )
+{
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
+
+  /* Increment interrupt nest and thread dispatch disable level */
+  ++cpu_self->thread_dispatch_disable_level;
+  ++cpu_self->isr_nest_level;
+}
+
+/* -1 means not mappable/recoverable */
+int _CPU_Exception_frame_get_signal( CPU_Exception_frame *ef )
+{
+  uint32_t EC = ef->esr & MICROBLAZE_ESR_EC_MASK;
+
+  /* Break in progress */
+  if ( ( ef->msr & MICROBLAZE_MSR_BIP ) != 0 ) {
+    return -1;
+  }
+
+  switch ( EC ) {
+   case 0x0:  /* Stream */
+   case 0x7:  /* Privileged or Stack Protection */
+     return -1;
+
+   case 0x5:  /* Divide */
+   case 0x6:  /* FPU */
+     return SIGFPE;
+
+   case 0x3:  /* Instruction Abort */
+   case 0x4:  /* Data Abort */
+     return SIGSEGV;
+
+   case 0x1:  /* Unaligned access */
+   case 0x2:  /* Illegal op-code */
+   default:
+     return SIGILL;
+  }
+}
+
+void _CPU_Exception_frame_set_resume( CPU_Exception_frame *ef, void *address )
+{
+  /* Break in progress */
+  if ( ( ef->msr & MICROBLAZE_MSR_BIP ) != 0 ) {
+    ef->r16 = address;
+    return;
+  }
+
+  /* Exception in progress */
+  if ( ( ef->msr & MICROBLAZE_MSR_EIP ) != 0 ) {
+    ef->r17 = address;
+    return;
+  }
+
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
+
+  /* Interrupt in progress must be determined by stack pointer location */
+  if (
+    ef->r1 >= (uint32_t) cpu_self->interrupt_stack_low
+    && ef->r1 < (uint32_t) cpu_self->interrupt_stack_high
+  ) {
+    ef->r14 = address;
+    return;
+  }
+
+  /* Default to normal link register */
+  ef->r15 = address;
+}
+
+/*
+ * This returns the target return address, not necessarily the address of the
+ * instruction that caused exception. These are the same if it's a MMU exception
+ * and the BTR overrides the return address if the exception occurred in a delay
+ * slot. */
+uint32_t *_MicroBlaze_Get_return_address( CPU_Exception_frame *ef )
+{
+  /* Break in progress */
+  if ( ( ef->msr & MICROBLAZE_MSR_BIP ) != 0 ) {
+    return ef->r16;
+  }
+
+  /* Exception in progress */
+  if ( ( ef->msr & MICROBLAZE_MSR_EIP ) != 0 ) {
+    if ( ( ef->esr & MICROBLAZE_ESR_DS ) != 0 ) {
+      return ef->btr;
+    }
+
+    return ef->r17;
+  }
+
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
+
+  /* Interrupt in progress must be determined by stack pointer location */
+  if (
+    ef->r1 >= (uint32_t) cpu_self->interrupt_stack_low
+    && ef->r1 < (uint32_t) cpu_self->interrupt_stack_high
+  ) {
+    return ef->r14;
+  }
+
+  /* Default to normal link register */
+  return ef->r15;
+}
+
+/*
+ * This can only change the resume address in the case of an exception in a
+ * branch delay slot instruction.
+ */
+void _CPU_Exception_frame_make_resume_next_instruction(
+  CPU_Exception_frame *ef
+)
+{
+  uintptr_t ret_addr = (uintptr_t) _MicroBlaze_Get_return_address( ef );
+
+  _CPU_Exception_frame_set_resume( ef, (uint32_t *) ret_addr );
+}