From d8579dde956f095d9dc53d45dcf4b78cd6234751 Mon Sep 17 00:00:00 2001
From: Matt Anderson <mda2376@gmail.com>
Date: Wed, 19 Jun 2019 04:01:37 -0400
Subject: [PATCH] CortexM0: IPR and SHPR are only word addressable

For ARMv6M, the IPR and SHPR registers are accessible only when
adddressed with a 32bit word read or write.

Currently in libopencm3 all NVIC interrupt priority register accesses
are made using an 8bit read or write, which results in the hardware
ignoring the write or always returning 0 on read.

Address this by introducing NVIC_IPR32() and SCS_SHPR32() macro and
conditional implementation of nvic_set_priority when building for
cortex-m0.

See ARMv6M developer documentation:
IPR: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0497a/Cihgjeed.html
SHPR: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0497a/CIAGECDD.html
---
 include/libopencm3/cm3/nvic.h |  7 ++++++-
 include/libopencm3/cm3/scs.h  |  6 ++++++
 lib/cm3/nvic.c                | 17 +++++++++++++++++
 3 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/include/libopencm3/cm3/nvic.h b/include/libopencm3/cm3/nvic.h
index 0bd51340..c967f15d 100644
--- a/include/libopencm3/cm3/nvic.h
+++ b/include/libopencm3/cm3/nvic.h
@@ -92,10 +92,15 @@
 
 /** IPR: Interrupt Priority Registers
  * @note 240 8bit Registers
- * @@note 32 8bit Registers on CM0
+ * @note 32 8bit Registers on CM0, requires word access
  */
+#if defined(__ARM_ARCH_6M__)
+#define NVIC_IPR32(ipr_id)		MMIO32(NVIC_BASE + 0x300 + \
+						((ipr_id) * 4))
+#else
 #define NVIC_IPR(ipr_id)		MMIO8(NVIC_BASE + 0x300 + \
 						(ipr_id))
+#endif
 
 #if defined(__ARM_ARCH_7M__) || defined(__ARM_ARCH_7EM__)
 /** STIR: Software Trigger Interrupt Register */
diff --git a/include/libopencm3/cm3/scs.h b/include/libopencm3/cm3/scs.h
index 27f6093f..cb0a6ddb 100644
--- a/include/libopencm3/cm3/scs.h
+++ b/include/libopencm3/cm3/scs.h
@@ -52,8 +52,14 @@
 
 /** System Handler Priority 8 bits Registers, SHPR1/2/3.
  * Note: 12 8bit Registers
+ * Note: 3 32bit Registers on CM0, requires word access
  */
+#if defined(__ARM_ARCH_6M__)
+#define SCS_SHPR32(ipr_id)		MMIO32(SCS_BASE + 0xD18 + ((ipr_id) * 4))
+#else
 #define SCS_SHPR(ipr_id)		MMIO8(SCS_BASE + 0xD18 + (ipr_id))
+#endif
+
 
 /**
  * Debug Halting Control and Status Register (DHCSR).
diff --git a/lib/cm3/nvic.c b/lib/cm3/nvic.c
index 7b20941f..37f473b1 100644
--- a/lib/cm3/nvic.c
+++ b/lib/cm3/nvic.c
@@ -156,10 +156,27 @@ void nvic_set_priority(uint8_t irqn, uint8_t priority)
 	 * handling would mean signed integers. */
 	if (irqn >= NVIC_IRQ_COUNT) {
 		/* Cortex-M  system interrupts */
+#if defined(__ARM_ARCH_6M__)
+		/* ARM6M supports only 32bit word access to SHPR registers */
+		irqn = (irqn & 0xF) - 4;
+		uint8_t shift = (irqn & 0x3) << 3;
+		uint8_t reg = irqn >> 2;
+		SCS_SHPR32(reg) = ((SCS_SHPR32(reg) & ~(0xFFUL << shift)) |
+				((uint32_t) priority) << shift);
+#else
 		SCS_SHPR((irqn & 0xF) - 4) = priority;
+#endif
 	} else {
 		/* Device specific interrupts */
+#if defined(__ARM_ARCH_6M__)
+		/* ARM6M supports only 32bit word access to IPR registers */
+		uint8_t shift = (irqn & 0x3) << 3;
+		uint8_t reg = irqn >> 2;
+		NVIC_IPR32(reg) = ((NVIC_IPR32(reg) & ~(0xFFUL << shift)) |
+				((uint32_t) priority) << shift);
+#else
 		NVIC_IPR(irqn) = priority;
+#endif
 	}
 }