From 7e91901303219f10cf865906931c07c31d2e37f4 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 4 Jul 2017 14:15:03 +0200
Subject: arm: Fix ARMv7-M interrupt processing

Right after a "msr basepri_max, %[basepri]" instruction an interrupt
service may still take place (observed at least on Cortex-M7).  However,
pendable service calls that are activated during this interrupt service
may be delayed until interrupts are enable again.  The
_ARMV7M_Pendable_service_call() did not check that a thread dispatch is
allowed.  Move this test from _ARMV7M_Interrupt_service_leave() to
_ARMV7M_Pendable_service_call().

Close #3060.
---
 cpukit/score/cpu/arm/armv7m-isr-dispatch.c    | 50 +++++++++++++++++----------
 cpukit/score/cpu/arm/armv7m-isr-enter-leave.c | 28 +++++++++------
 2 files changed, 48 insertions(+), 30 deletions(-)

diff --git a/cpukit/score/cpu/arm/armv7m-isr-dispatch.c b/cpukit/score/cpu/arm/armv7m-isr-dispatch.c
index e460e9c3e7..ffd1589db1 100644
--- a/cpukit/score/cpu/arm/armv7m-isr-dispatch.c
+++ b/cpukit/score/cpu/arm/armv7m-isr-dispatch.c
@@ -5,10 +5,10 @@
  */
 
 /*
- * Copyright (c) 2011-2014 Sebastian Huber.  All rights reserved.
+ * Copyright (c) 2011, 2017 Sebastian Huber.  All rights reserved.
  *
  *  embedded brains GmbH
- *  Obere Lagerstr. 30
+ *  Dornierstr. 4
  *  82178 Puchheim
  *  Germany
  *  <rtems@embedded-brains.de>
@@ -48,29 +48,42 @@ static void _ARMV7M_Trigger_lazy_floating_point_context_save( void )
 
 void _ARMV7M_Pendable_service_call( void )
 {
-  ARMV7M_Exception_frame *ef;
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
 
-  _ISR_Nest_level = 1;
+  /*
+   * We must check here if a thread dispatch is allowed.  Right after a
+   * "msr basepri_max, %[basepri]" instruction an interrupt service may still
+   * take place.  However, pendable service calls that are activated during
+   * this interrupt service may be delayed until interrupts are enable again.
+   */
+  if (
+    ( cpu_self->isr_nest_level | cpu_self->thread_dispatch_disable_level ) == 0
+  ) {
+    ARMV7M_Exception_frame *ef;
 
-  _ARMV7M_SCB->icsr = ARMV7M_SCB_ICSR_PENDSVCLR;
-  _ARMV7M_Trigger_lazy_floating_point_context_save();
+    cpu_self->isr_nest_level = 1;
 
-  ef = (ARMV7M_Exception_frame *) _ARMV7M_Get_PSP();
-  --ef;
-  _ARMV7M_Set_PSP( (uint32_t) ef );
+    _ARMV7M_SCB->icsr = ARMV7M_SCB_ICSR_PENDSVCLR;
+    _ARMV7M_Trigger_lazy_floating_point_context_save();
 
-  /*
-   * According to "ARMv7-M Architecture Reference Manual" section B1.5.6
-   * "Exception entry behavior" the return address is half-word aligned.
-   */
-  ef->register_pc = (void *)
-    ((uintptr_t) _ARMV7M_Thread_dispatch & ~((uintptr_t) 1));
+    ef = (ARMV7M_Exception_frame *) _ARMV7M_Get_PSP();
+    --ef;
+    _ARMV7M_Set_PSP( (uint32_t) ef );
 
-  ef->register_xpsr = 0x01000000U;
+    /*
+     * According to "ARMv7-M Architecture Reference Manual" section B1.5.6
+     * "Exception entry behavior" the return address is half-word aligned.
+     */
+    ef->register_pc = (void *)
+      ((uintptr_t) _ARMV7M_Thread_dispatch & ~((uintptr_t) 1));
+
+    ef->register_xpsr = 0x01000000U;
+  }
 }
 
 void _ARMV7M_Supervisor_call( void )
 {
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
   ARMV7M_Exception_frame *ef;
 
   _ARMV7M_Trigger_lazy_floating_point_context_save();
@@ -79,10 +92,9 @@ void _ARMV7M_Supervisor_call( void )
   ++ef;
   _ARMV7M_Set_PSP( (uint32_t) ef );
 
-  _ISR_Nest_level = 0;
-  RTEMS_COMPILER_MEMORY_BARRIER();
+  cpu_self->isr_nest_level = 0;
 
-  if ( _Thread_Dispatch_necessary ) {
+  if ( cpu_self->dispatch_necessary ) {
     _ARMV7M_Pendable_service_call();
   }
 }
diff --git a/cpukit/score/cpu/arm/armv7m-isr-enter-leave.c b/cpukit/score/cpu/arm/armv7m-isr-enter-leave.c
index 4ab800b2a7..90e97fb0a4 100644
--- a/cpukit/score/cpu/arm/armv7m-isr-enter-leave.c
+++ b/cpukit/score/cpu/arm/armv7m-isr-enter-leave.c
@@ -5,10 +5,10 @@
  */
 
 /*
- * Copyright (c) 2011 Sebastian Huber.  All rights reserved.
+ * Copyright (c) 2011, 2017 Sebastian Huber.  All rights reserved.
  *
  *  embedded brains GmbH
- *  Obere Lagerstr. 30
+ *  Dornierstr. 4
  *  82178 Puchheim
  *  Germany
  *  <rtems@embedded-brains.de>
@@ -30,19 +30,25 @@
 
 void _ARMV7M_Interrupt_service_enter( void )
 {
-  ++_Thread_Dispatch_disable_level;
-  ++_ISR_Nest_level;
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
+
+  ++cpu_self->thread_dispatch_disable_level;
+  ++cpu_self->isr_nest_level;
 }
 
 void _ARMV7M_Interrupt_service_leave( void )
 {
-  --_ISR_Nest_level;
-  --_Thread_Dispatch_disable_level;
-  if (
-    _ISR_Nest_level == 0
-      && _Thread_Dispatch_disable_level == 0
-      && _Thread_Dispatch_necessary
-  ) {
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
+
+  --cpu_self->thread_dispatch_disable_level;
+  --cpu_self->isr_nest_level;
+
+  /*
+   * Optimistically activate a pendable service call if a thread dispatch is
+   * necessary.  The _ARMV7M_Pendable_service_call() will check that a thread
+   * dispatch is allowed.
+   */
+  if ( cpu_self->dispatch_necessary ) {
     _ARMV7M_SCB->icsr = ARMV7M_SCB_ICSR_PENDSVSET;
   }
 }
-- 
cgit v1.2.3