Fix a ARMv7-M interrupt disable/optimization bug

git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5155 7fd9a85b-ad96-42d3-883c-3090e2eb8679

2 files changed, 59 insertions, 53 deletions

diff --git a/nuttx/arch/arm/include/armv7-m/irq.h b/nuttx/arch/arm/include/armv7-m/irq.h
index 6cef85c02..950ce80ca 100644
--- a/nuttx/arch/arm/include/armv7-m/irq.h
+++ b/nuttx/arch/arm/include/armv7-m/irq.h
@@ -129,8 +129,25 @@ struct xcptcontext
 
 #ifndef __ASSEMBLY__
 
+/* Get/set the primask register */
+
+static inline uint8_t getprimask(void) __attribute__((always_inline));
+static inline uint8_t getprimask(void)
+{
+  uint32_t primask;
+  __asm__ __volatile__
+    (
+     "\tmrs  %0, primask\n"
+     : "=r" (primask)
+     :
+     : "memory");
+
+  return (uint8_t)primask;
+}
+
 /* Disable IRQs */
 
+static inline void irqdisable(void) __attribute__((always_inline));
 static inline void irqdisable(void)
 {
   __asm__ __volatile__ ("\tcpsid  i\n");
@@ -138,26 +155,22 @@ static inline void irqdisable(void)
 
 /* Save the current primask state & disable IRQs */
 
+static inline irqstate_t irqsave(void) __attribute__((always_inline));
 static inline irqstate_t irqsave(void)
 {
-  unsigned short primask;
+  /* Return the current value of primask register (before disabling) */
 
-  /* Return the current value of primask register and set
-   * bit 0 of the primask register to disable interrupts
-   */
+  uint8_t primask = getprimask();
 
-  __asm__ __volatile__
-    (
-     "\tmrs    %0, primask\n"
-     "\tcpsid  i\n"
-     : "=r" (primask)
-     :
-     : "memory");
+  /* Then set bit 0 of the primask register to disable interrupts */
+
+  irqdisable();
   return primask;
 }
 
 /* Enable IRQs */
 
+static inline void irqenable(void) __attribute__((always_inline));
 static inline void irqenable(void)
 {
   __asm__ __volatile__ ("\tcpsie  i\n");
@@ -165,61 +178,48 @@ static inline void irqenable(void)
 
 /* Restore saved primask state */
 
-static inline void irqrestore(irqstate_t primask)
+static inline void setprimask(uint32_t primask) __attribute__((always_inline));
+static inline void setprimask(uint32_t primask)
 {
-  /* If bit 0 of the primask is 0, then we need to restore
-   * interupts.
-   */
-
   __asm__ __volatile__
     (
-      "\ttst    %0, #1\n"
-      "\tbne    1f\n"
-      "\tcpsie  i\n"
-      "1:\n"
+      "\tmsr primask, %0\n"
       :
       : "r" (primask)
       : "memory");
 }
 
-/* Get/set the primask register */
-
-static inline uint8_t getprimask(void)
+static inline void irqrestore(irqstate_t primask) __attribute__((always_inline));
+static inline void irqrestore(irqstate_t primask)
 {
-  uint32_t primask;
-  __asm__ __volatile__
-    (
-     "\tmrs  %0, primask\n"
-     : "=r" (primask)
-     :
-     : "memory");
-  return (uint8_t)primask;
-}
+  /* If bit 0 of the primask is 0, then we need to restore
+   * interrupts.
+   */
 
-static inline void setprimask(uint32_t primask)
-{
-  __asm__ __volatile__
-    (
-      "\tmsr primask, %0\n"
-      :
-      : "r" (primask)
-      : "memory");
+  if ((primask & 1) == 0)
+    {
+      setprimask(primask);
+    }
 }
 
 /* Get/set the basepri register */
 
+static inline uint8_t getbasepri(void) __attribute__((always_inline));
 static inline uint8_t getbasepri(void)
 {
   uint32_t basepri;
+
   __asm__ __volatile__
     (
      "\tmrs  %0, basepri\n"
      : "=r" (basepri)
      :
      : "memory");
+
   return (uint8_t)basepri;
 }
 
+static inline void setbasepri(uint32_t basepri) __attribute__((always_inline));
 static inline void setbasepri(uint32_t basepri)
 {
   __asm__ __volatile__
@@ -232,6 +232,7 @@ static inline void setbasepri(uint32_t basepri)
 
 /* Get/set IPSR */
 
+static inline uint32_t getipsr(void) __attribute__((always_inline));
 static inline uint32_t getipsr(void)
 {
   uint32_t ipsr;
@@ -241,9 +242,11 @@ static inline uint32_t getipsr(void)
      : "=r" (ipsr)
      :
      : "memory");
+
   return ipsr;
 }
 
+static inline void setipsr(uint32_t ipsr) __attribute__((always_inline));
 static inline void setipsr(uint32_t ipsr)
 {
   __asm__ __volatile__
@@ -256,6 +259,7 @@ static inline void setipsr(uint32_t ipsr)
 
 /* Get/set CONTROL */
 
+static inline uint32_t getcontrol(void) __attribute__((always_inline));
 static inline uint32_t getcontrol(void)
 {
   uint32_t control;
@@ -265,9 +269,11 @@ static inline uint32_t getcontrol(void)
      : "=r" (control)
      :
      : "memory");
+
   return control;
 }
 
+static inline void setcontrol(uint32_t control) __attribute__((always_inline));
 static inline void setcontrol(uint32_t control)
 {
   __asm__ __volatile__
diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.c b/nuttx/arch/arm/src/stm32/stm32_spi.c
index 40b1a29a0..2d907bfca 100644
--- a/nuttx/arch/arm/src/stm32/stm32_spi.c
+++ b/nuttx/arch/arm/src/stm32/stm32_spi.c
@@ -49,7 +49,7 @@
  *   3. Add a calls to up_spiinitialize() in your low level application
  *      initialization logic
  *   4. The handle returned by up_spiinitialize() may then be used to bind the
- *      SPI driver to higher level logic (e.g., calling 
+ *      SPI driver to higher level logic (e.g., calling
  *      mmcsd_spislotinitialize(), for example, will bind the SPI driver to
  *      the SPI MMC/SD driver).
  *
@@ -881,7 +881,7 @@ static uint32_t spi_setfrequency(FAR struct spi_dev_s *dev, uint32_t frequency)
       else
         {
           /* Less than fPCLK/128.  This is as slow as we can go */
- 
+
           setbits = SPI_CR1_FPCLCKd256; /* 111: fPCLK/256 */
           actual = priv->spiclock >> 8;
         }
@@ -941,22 +941,22 @@ static void spi_setmode(FAR struct spi_dev_s *dev, enum spi_mode_e mode)
           setbits = 0;
           clrbits = SPI_CR1_CPOL|SPI_CR1_CPHA;
           break;
- 
+
         case SPIDEV_MODE1: /* CPOL=0; CPHA=1 */
           setbits = SPI_CR1_CPHA;
           clrbits = SPI_CR1_CPOL;
           break;
- 
+
         case SPIDEV_MODE2: /* CPOL=1; CPHA=0 */
           setbits = SPI_CR1_CPOL;
           clrbits = SPI_CR1_CPHA;
           break;
- 
+
         case SPIDEV_MODE3: /* CPOL=1; CPHA=1 */
           setbits = SPI_CR1_CPOL|SPI_CR1_CPHA;
           clrbits = 0;
           break;
- 
+
         default:
           return;
         }
@@ -1008,7 +1008,7 @@ static void spi_setbits(FAR struct spi_dev_s *dev, int nbits)
           setbits = 0;
           clrbits = SPI_CR1_DFF;
           break;
- 
+
         case 16:
           setbits = SPI_CR1_DFF;
           clrbits = 0;
@@ -1111,7 +1111,7 @@ static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
           }
 
           /* Exchange one word */
- 
+
           word = spi_send(dev, word);
 
           /* Is there a buffer to receive the return value? */
@@ -1120,7 +1120,7 @@ static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
             {
               *dest++ = word;
             }
-        } 
+        }
     }
   else
     {
@@ -1144,7 +1144,7 @@ static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
           }
 
           /* Exchange one word */
- 
+
           word = (uint8_t)spi_send(dev, (uint16_t)word);
 
           /* Is there a buffer to receive the return value? */
@@ -1152,7 +1152,7 @@ static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
           if (dest)
             {
               *dest++ = word;
-            } 
+            }
         }
     }
 }
@@ -1331,7 +1331,7 @@ static void spi_portinitialize(FAR struct stm32_spidev_s *priv)
   priv->txdma = stm32_dmachannel(priv->txch);
   DEBUGASSERT(priv->rxdma && priv->txdma);
 #endif
-  
+
   /* Enable spi */
 
   spi_modifycr1(priv, SPI_CR1_SPE, 0);
@@ -1360,7 +1360,7 @@ FAR struct spi_dev_s *up_spiinitialize(int port)
   FAR struct stm32_spidev_s *priv = NULL;
 
   irqstate_t flags = irqsave();
-  
+
 #ifdef CONFIG_STM32_SPI1
   if (port == 1)
     {