Fix IRQ priority setting helpers

diff --git a/include/core/lpc_core.h b/include/core/lpc_core.h
index fb0e5cd..d2f4658 100644 (file)
--- a/include/core/lpc_core.h
+++ b/include/core/lpc_core.h
@@ -370,10 +370,12 @@ static inline void NVIC_ClearPendingIRQ(uint32_t IRQ)
 
 
 /* The following MACROS handle generation of the register offset and byte masks */
-#define LPC_NVIC_PRIO_BITS  5  /* Number of Bits used for Priority Levels */
+#define LPC_NVIC_PRIO_BITS  6  /* Number of Bits used for Priority Levels */
 #define LPC_IRQ_BIT_SHIFT(IRQ) (((uint32_t)(IRQ) & 0x03) * 8)
-#define LPC_IRQ_SHP_IDX(IRQ)   ((((int32_t)(IRQ) + 16) >> 2) - 1)
 #define LPC_IRQ_IP_IDX(IRQ)    ((uint32_t)(IRQ) >> 2)
+#define LPC_IRQ_SHP_IDX(IRQ)   ((((int32_t)(IRQ) + 16) >> 2) - 2)
+#define LPC_IRQ_SHP_BIT_SHIFT(IRQ) ((((int32_t)(IRQ) + 16) & 0x03) * 8)
+
 
 /*  Set Interrupt Priority
   This function sets the priority for the specified interrupt. The interrupt
@@ -381,15 +383,15 @@ static inline void NVIC_ClearPendingIRQ(uint32_t IRQ)
     interrupt, or negative to specify an internal (core) interrupt.
   Note: The priority cannot be set for every core interrupt.
   IRQ : Number of the interrupt for set priority
-  priority : Priority to set
+  priority : Priority to set (0 to 3, 0 is highest, 3 is lowest)
  */
 static inline void NVIC_SetPriority(uint32_t IRQ, uint32_t priority)
 {
        if ((int32_t)IRQ < 0) {
                if ((int32_t)IRQ > (-12)) {
                        struct syst_ctrl_block_regs* scb = LPC_SCB;
-                       scb->shp[LPC_IRQ_SHP_IDX(IRQ)] = (scb->shp[LPC_IRQ_SHP_IDX(IRQ)] & ~(0xFF << LPC_IRQ_BIT_SHIFT(IRQ))) |
-                               (((priority << (8 - LPC_NVIC_PRIO_BITS)) & 0xFF) << LPC_IRQ_BIT_SHIFT(IRQ));
+                       scb->shp[LPC_IRQ_SHP_IDX(IRQ)] = (scb->shp[LPC_IRQ_SHP_IDX(IRQ)] & ~(0xFF << LPC_IRQ_SHP_BIT_SHIFT(IRQ))) |
+                               (((priority << (8 - LPC_NVIC_PRIO_BITS)) & 0xFF) << LPC_IRQ_SHP_BIT_SHIFT(IRQ));
                }
        } else if (LPC_IRQ_IP_IDX(IRQ) < 8) {
                struct nvic_regs* nvic = LPC_NVIC;
@@ -414,7 +416,7 @@ static inline uint32_t NVIC_GetPriority(uint32_t IRQ)
                if ((int32_t)IRQ > (-12)) {
                        struct syst_ctrl_block_regs* scb = LPC_SCB;
                        /* Get priority for Cortex-M0 system interrupts */
-                       return ((uint32_t)((scb->shp[LPC_IRQ_SHP_IDX(IRQ)] >> LPC_IRQ_BIT_SHIFT(IRQ) ) >> (8 - LPC_NVIC_PRIO_BITS)));
+                       return ((uint32_t)((scb->shp[LPC_IRQ_SHP_IDX(IRQ)] >> LPC_IRQ_SHP_BIT_SHIFT(IRQ) ) >> (8 - LPC_NVIC_PRIO_BITS)));
                }
        } else if (LPC_IRQ_IP_IDX(IRQ) < 8) {
                struct nvic_regs* nvic = LPC_NVIC;