From 065b47f8bad025e59837fcbc8f9443b6317e6cd9 Mon Sep 17 00:00:00 2001
From: Frantisek Burian <BuFran@seznam.cz>
Date: Wed, 11 Sep 2013 09:33:45 -0700
Subject: [PATCH] [CM3] Add core interrupt masking for easy creation of atomic
 blocks

---
 include/libopencm3/cm3/cortex.h | 51 +++++++++++++++++++++++++++++++--
 1 file changed, 49 insertions(+), 2 deletions(-)

diff --git a/include/libopencm3/cm3/cortex.h b/include/libopencm3/cm3/cortex.h
index c99e7441..d334fc04 100644
--- a/include/libopencm3/cm3/cortex.h
+++ b/include/libopencm3/cm3/cortex.h
@@ -22,12 +22,59 @@
 
 static inline void cm_enable_interrupts(void)
 {
-        __asm__("CPSIE I\n");
+	__asm__("CPSIE I\n");
 }
 
 static inline void cm_disable_interrupts(void)
 {
-        __asm__("CPSID I\n");
+	__asm__("CPSID I\n");
+}
+
+static inline void cm_enable_faults(void)
+{
+	__asm__("CPSIE F\n");
 }
 
+static inline void cm_disable_faults(void)
+{
+	__asm__("CPSID F\n");
+}
+
+/* __attribute__(( always_inline )) */
+static inline bool cm_is_masked_interrupts(void)
+{
+	register uint32_t result;
+	__asm__ ("MRS %0, PRIMASK"  : "=r" (result) );
+	return (result);
+}
+
+/* __attribute__(( always_inline )) */
+static inline bool cm_is_masked_faults(void)
+{
+	register uint32_t result;
+	__asm__ ("MRS %0, FAULTMASK"  : "=r" (result) );
+	return (result);
+}
+
+/* __attribute__(( always_inline )) */
+static inline bool cm_mask_interrupts(bool mask)
+{
+	register bool old;
+	__asm__ __volatile__("MRS %0, PRIMASK"  : "=r" (old));
+	__asm__ __volatile__(""  ::: "memory");
+	__asm__ __volatile__("MSR PRIMASK, %0" : : "r" (mask));
+	return old;
+}
+
+/* __attribute__(( always_inline )) */
+static inline bool cm_mask_faults(bool mask)
+{
+	register bool old;
+	__asm__ __volatile__ ("MRS %0, FAULTMASK"  : "=r" (old));
+	__asm__ __volatile__(""  ::: "memory");
+	__asm__ __volatile__ ("MSR FAULTMASK, %0" : : "r" (mask));
+	return old;
+}
+
+
 #endif