From bb5e3d40d800465a22c9288571f0a54fc8cab80c Mon Sep 17 00:00:00 2001
From: Travis Geiselbrecht <geist@foobox.com>
Date: Sat, 27 Oct 2012 16:31:28 -0700
Subject: [PATCH] [arch][arm-m] make number of priority levels hard coded

-Turn off the dynamic priority calculation so we can much more efficiently
set different levels. All arm-ms support at least 3 bits of priority,
so default to that.
---
 arch/arm/arm-m/arch.c                 |  8 ++++--
 arch/arm/arm-m/include/arch/arm/cm3.h | 39 +++++++++++++++++++++++++--
 2 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/arch/arm/arm-m/arch.c b/arch/arm/arm-m/arch.c
index 88a1f71c..66a02ebd 100644
--- a/arch/arm/arm-m/arch.c
+++ b/arch/arm/arm-m/arch.c
@@ -34,8 +34,10 @@ extern void *vectab;
 extern int _end_of_ram;
 void *_heap_end = &_end_of_ram;
 
+#if ARM_M_DYNAMIC_PRIORITY_SIZE
 unsigned int cm3_num_irq_pri_bits;
 unsigned int cm3_irq_pri_mask;
+#endif
 
 void arch_early_init(void)
 {
@@ -56,6 +58,7 @@ void arch_early_init(void)
 		NVIC->IP[i*4+3] = 128; /* medium priority */
 	}
 
+#if ARM_M_DYNAMIC_PRIORITY_SIZE
 	/* number of priorities */
 	for (i=0; i < 7; i++) {
 		__set_BASEPRI(1 << i);
@@ -64,6 +67,7 @@ void arch_early_init(void)
 	}
 	cm3_num_irq_pri_bits = 8 - i;
 	cm3_irq_pri_mask = ~((1 << i) - 1) & 0xff;
+#endif
 
 	/* leave BASEPRI at 0 */
 	__set_BASEPRI(0);
@@ -99,7 +103,7 @@ void arch_idle(void)
 {
 }
 
-void cm3_set_irqpri(uint32_t pri)
+void _cm3_set_irqpri(uint32_t pri)
 {
 	if (pri == 0) {
 		__disable_irq(); // cpsid i
@@ -114,7 +118,7 @@ void cm3_set_irqpri(uint32_t pri)
 			__set_BASEPRI(1 << (8 - cm3_num_irq_pri_bits));
 		else
 			__set_BASEPRI(_pri);
-		__enable_irq(); // cpsid i
+		__enable_irq(); // cpsie i
 	}
 }
 
diff --git a/arch/arm/arm-m/include/arch/arm/cm3.h b/arch/arm/arm-m/include/arch/arm/cm3.h
index 664414d5..e39a6e87 100644
--- a/arch/arm/arm-m/include/arch/arm/cm3.h
+++ b/arch/arm/arm-m/include/arch/arm/cm3.h
@@ -81,10 +81,45 @@ struct cm3_exception_frame_long {
 	uint32_t psr;
 };
 
-void cm3_set_irqpri(uint32_t pri);
-
+#if ARM_M_DYNAMIC_PRIORITY_SIZE
 extern unsigned int cm3_num_irq_pri_bits;
 extern unsigned int cm3_irq_pri_mask;
+#else
+/* if we don't want to calculate the nubmer of priority bits, then assume
+ * the cpu implements 3 (8 priority levels), which is the minimum according to spec.
+ */
+#ifndef ARM_M_PRIORITY_BITS
+#define ARM_M_PRIORITY_BITS 3
+#endif
+static const unsigned int cm3_num_irq_pri_bits = 8 - ARM_M_PRIORITY_BITS;
+static const unsigned int cm3_irq_pri_mask = ~((1 << ARM_M_PRIORITY_BITS) - 1) & 0xff;
+#endif
+
+void _cm3_set_irqpri(uint32_t pri);
+
+static __ALWAYS_INLINE void cm3_set_irqpri(uint32_t pri)
+{
+	if (__ISCONSTANT(pri)) {
+		if (pri == 0) {
+			__disable_irq(); // cpsid i
+			__set_BASEPRI(0);
+		} else if (pri >= 256) {
+			__set_BASEPRI(0);
+			__enable_irq();
+		} else {
+			uint32_t _pri = pri & cm3_irq_pri_mask;
+
+			if (_pri == 0)
+				__set_BASEPRI(1 << (8 - cm3_num_irq_pri_bits));
+			else
+				__set_BASEPRI(_pri);
+			__enable_irq(); // cpsie i
+		}
+	} else {
+		_cm3_set_irqpri(pri);
+	}
+}
+
 
 static __ALWAYS_INLINE inline uint32_t cm3_highest_priority(void)
 {