[rtems commit] bsps/powerpc: Fix robust thread dispatch

[Sebastian Huber]

Module:    rtems
Branch:    master
Commit:    a4bca6858604e7e32b001e4121da7e83bc614320
Changeset: http://git.rtems.org/rtems/commit/?id=a4bca6858604e7e32b001e4121da7e83bc614320

Author:    Sebastian Huber <sebastian.huber at embedded-brains.de>
Date:      Thu Sep 21 13:32:25 2017 +0200

bsps/powerpc: Fix robust thread dispatch

Implement thread dispatch code in ppc_exc_wrapup() similar to
ppc_exc_interrupt().

Update #2811.

---

 .../new-exceptions/bspsupport/ppc_exc_hdl.c        | 27 +++++++++++++++++-----
 1 file changed, 21 insertions(+), 6 deletions(-)

diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_hdl.c b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_hdl.c
index f78003f..5c6754d 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_hdl.c
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_hdl.c
@@ -93,11 +93,26 @@ rtems_status_code ppc_exc_set_handler(unsigned vector, ppc_exc_handler_t handler
 
 void ppc_exc_wrapup(BSP_Exception_frame *frame)
 {
-  /* dispatch_disable level is decremented from assembly code.  */
-  if ( _Thread_Dispatch_necessary ) {
-    /* FIXME: I believe it should be OK to re-enable
-     *        interrupts around the execution of _Thread_Dispatch();
-     */
-    _Thread_Dispatch();
+  Per_CPU_Control *cpu_self;
+
+  cpu_self = _Per_CPU_Get();
+
+  if (cpu_self->isr_dispatch_disable) {
+    return;
   }
+
+  while (cpu_self->dispatch_necessary) {
+    uint32_t msr;
+    rtems_interrupt_level level;
+
+    cpu_self->isr_dispatch_disable = 1;
+    cpu_self->thread_dispatch_disable_level = 1;
+    msr = ppc_machine_state_register();
+    _Thread_Do_dispatch(cpu_self, msr | MSR_EE);
+    rtems_interrupt_local_disable(level);
+    (void) level;
+    cpu_self = _Per_CPU_Get();
+  }
+
+  cpu_self->isr_dispatch_disable = 0;
 }