1 files changed, 40 insertions, 39 deletions

diff --git a/libmaple/nvic.c b/libmaple/nvic.c
index b1da605..a7a6d8e 100644
--- a/libmaple/nvic.c
+++ b/libmaple/nvic.c
@@ -26,56 +26,57 @@
  * @brief Nested interrupt controller routines
  */
 
-#include "libmaple.h"
 #include "nvic.h"
-#include "systick.h"
-
-void nvic_set_vector_table(uint32 addr, uint32 offset) {
-    __write(SCB_VTOR, (uint32)addr | (offset & 0x1FFFFF80));
-}
+#include "scb.h"
+#include "stm32.h"
 
 /**
- * @brief turn on interrupt number n
- * @param n interrupt number
+ * @brief Set interrupt priority for an interrupt line
+ *
+ * Note: The STM32 only implements 4 bits of priority, ignoring the
+ * lower 4 bits. This means there are only 16 levels of priority.
+ * Bits[3:0] read as zero and ignore writes.
+ *
+ * @param irqn device to set
+ * @param priority Priority to set, 0 being highest priority and 15
+ *                 being lowest.
  */
-void nvic_irq_enable(uint32 n) {
-    uint32 *iser = &((uint32*)NVIC_ISER0)[(n/32)];
-    __write(iser, BIT(n % 32));
+void nvic_irq_set_priority(nvic_irq_num irqn, uint8 priority) {
+    if (irqn < 0) {
+        /* This interrupt is in the system handler block  */
+        SCB_BASE->SHP[((uint32)irqn & 0xF) - 4] = (priority & 0xF) << 4;
+    } else {
+        NVIC_BASE->IP[irqn] = (priority & 0xF) << 4;
+    }
 }
 
 /**
- * @brief turn off interrupt number n
- * @param n interrupt number
+ * @brief Initialize the NVIC
+ * @param vector_table_address Vector table base address.
+ * @param offset Offset from vector_table_address.  Some restrictions
+ *               apply to the use of nonzero offsets; see ST RM0008
+ *               and the ARM Cortex M3 Technical Reference Manual.
  */
-void nvic_irq_disable(uint32 n) {
-    uint32 *icer = &((uint32*)NVIC_ICER0)[(n/32)];
-    __write(icer, BIT(n % 32));
-}
+void nvic_init(uint32 vector_table_address, uint32 offset) {
+    uint32 i;
+
+    nvic_set_vector_table(vector_table_address, offset);
 
-void nvic_irq_disable_all(void) {
-    /* Each ICER register contains 1 bit per interrupt.  Writing a 1
-       to that bit disables the corresponding interrupt.  So each of
-       the following lines disables up to 32 interrupts at a time.
-       Since low, medium, and high-density devices all have less than
-       64 interrupts, this suffices. */
-    /* TODO: fix for connectivity line: __write(NVIC_ICER2,1),
-       requires connectivity line support in libmaple.h */
-    __write(NVIC_ICER0, 0xFFFFFFFF);
-    __write(NVIC_ICER1, 0xFFFFFFFF);
+    /*
+     * Lower priority level for all peripheral interrupts to lowest
+     * possible.
+     */
+    for (i = 0; i < NR_INTERRUPTS; i++) {
+        nvic_irq_set_priority((nvic_irq_num)i, 0xF);
+    }
+
+    /* Lower systick interrupt priority to lowest level */
+    nvic_irq_set_priority(NVIC_SYSTICK, 0xF);
 }
 
 /**
- * @brief Initialize the NVIC according to VECT_TAB_FLASH,
- * VECT_TAB_RAM, or VECT_TAB_BASE.
+ * Reset the vector table address.
  */
-void nvic_init(void) {
-#ifdef VECT_TAB_FLASH
-    nvic_set_vector_table(USER_ADDR_ROM, 0x0);
-#elif defined VECT_TAB_RAM
-    nvic_set_vector_table(USER_ADDR_RAM, 0x0);
-#elif defined VECT_TAB_BASE
-    nvic_set_vector_table(((uint32)0x08000000), 0x0);
-#else
-#error "You must set a base address for the vector table!"
-#endif
+void nvic_set_vector_table(uint32 addr, uint32 offset) {
+    SCB_BASE->VTOR = addr | (offset & 0x1FFFFF80);
 }