mpu test

7 files changed, 341 insertions, 5 deletions

diff --git a/Makefile b/Makefile
index 477214b..3942f18 100644
--- a/Makefile
+++ b/Makefile
@@ -10,6 +10,8 @@ ASFLAGS=-mcpu=cortex-m3 -W --fatal-warnings $(DEBUG)
 CFLAGS=-mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib -nostartfiles -nodefaultlibs $(DEBUG) -Wall -Wextra -Werror
 LDFLAGS=-static
 
+CFLAGS+=-Os
+
 all: test1-kern.bin
 all: test2-kern.bin
 all: test3-kern.bin
@@ -17,6 +19,7 @@ all: test4-kern.bin
 all: test5-kern.bin
 all: test6-kern.bin
 all: test7-kern.bin
+all: test8-kern.bin
 
 test1-kern.elf: cortexm.ld common.ld init-m.o test1.o
 test2-kern.elf: cortexm.ld common.ld init-m.o test2.o
@@ -25,6 +28,7 @@ test4-kern.elf: cortexm.ld common.ld init-m.o test4.o
 test5-kern.elf: cortexm.ld common.ld init-m.o test5.o
 test6-kern.elf: cortexm.ld common.ld init-m.o test6.o
 test7-kern.elf: cortexm.ld common.ld init-m.o test7.o
+test8-kern.elf: cortexm.ld common.ld init-m.o test8.o inst_skip.o
 
 clean:
 	rm -f *.o *.elf *.map *.bin *.img
diff --git a/armv7m.h b/armv7m.h
index 7922119..c1339fa 100644
--- a/armv7m.h
+++ b/armv7m.h
@@ -50,7 +50,7 @@ uint32_t in32(void *addr)
 	return *A;
 }
 
-void abort(void);
+void abort(void) __attribute__((noreturn));
 
 static inline __attribute__((unused))
 void putc(char c)
@@ -77,4 +77,67 @@ void puthex(uint32_t v)
 	}
 }
 
+
+#define MPU_XN (1<<28)
+
+#define MPU_NANA (0<<24)
+#define MPU_RWNA (1<<24)
+#define MPU_RWRO (2<<24)
+#define MPU_RWRW (3<<24)
+#define MPU_RONA (5<<24)
+#define MPU_RORO (6<<24)
+
+#define MPU_STRONG (0<<16)
+#define MPU_DEVICE (1<<16)
+#define MPU_NORMAL (6<<16)
+
+/* ceil(log(v, 2))
+ *  log2_ceil(31) -> 6
+ *  log2_ceil(32) -> 6
+ *  log2_ceil(33) -> 7
+ */
+static inline
+unsigned log2_ceil(uint32_t v)
+{
+    unsigned r=0, c=0;
+    while(v) {
+        c += v&1;
+        v >>= 1;
+        r++;
+    }
+    if(c>1) r++;
+    return r;
+}
+
+static inline
+void set_mpu(unsigned region, uint32_t base, uint32_t size,
+			 uint32_t attrs)
+{
+	unsigned sbits = log2_ceil(size<32 ? 32 : size)-2;
+	uint32_t rbase = base&(~0x1f);
+	uint32_t rattr = (attrs&~0xffff) | (sbits<<1) | 1;
+	puts("set_mpu ");
+	putc('0'+region);
+	putc(' ');
+	puthex(rbase);
+	putc(' ');
+	puthex(rattr);
+	putc('\n');
+	out32((void*)0xe000ed98, region&0xff); /* RNR */
+	out32((void*)0xe000eda0, 0); /* Disable */
+	out32((void*)0xe000ed9c, rbase);
+	out32((void*)0xe000eda0, rattr);
+}
+
+static inline
+void enable_mpu(unsigned ena, unsigned hfnmiena)
+{
+	uint32_t val = 4 | (ena ? 1 : 0) | (hfnmiena ? 2 : 0);
+	out32((void*)0xe000ed94, val);
+}
+
+uint32_t* get_src_stack(uint32_t *sp);
+void inst_skip(uint32_t *sp);
+int get_svc(uint32_t *sp);
+
 #endif /* ARMV7m_h */
diff --git a/common.ld b/common.ld
index 226b21f..2108fe6 100644
--- a/common.ld
+++ b/common.ld
@@ -44,7 +44,8 @@ SECTIONS
     .jcr            : { KEEP (*(.jcr)) } >ROM
 
     /* end c++ stuff */
-    
+    __end_rom = .;
+
     .data : ALIGN(4)
     {
       __data_start = .;
diff --git a/init-m.S b/init-m.S
index f309229..abc8739 100644
--- a/init-m.S
+++ b/init-m.S
@@ -75,7 +75,10 @@ _data_loop:
   ldr r0, =run_table
   ldr r1, =0xe000ed08 /* VTOR */
   str r0, [r1]
-  
+
+  mov r0, _proc_stack_top
+  msr PSP, r0
+
   blx main
   
   /* fall through to abort() */
diff --git a/inst_skip.c b/inst_skip.c
new file mode 100644
index 0000000..cee535b
--- /dev/null
+++ b/inst_skip.c
@@ -0,0 +1,43 @@
+
+#include "armv7m.h"
+
+/* Call on exception entry to get the stack
+ * pointer containing the exception frame
+ */
+uint32_t* get_src_stack(uint32_t *sp)
+{
+	uint32_t ctrl = 0,
+			icsr = in32((void*)0xe000ed04);
+	__asm__ ("mrs %r0, CONTROL" : "=r"(ctrl) ::);
+
+	if((ctrl&2) && (icsr&(1<<11)))
+		__asm__ ("mrs %r0, PSP" : "=r"(sp) ::);
+
+	return sp;
+}
+
+/* adjust the stacked PC to step
+ * forward by one instruction.
+ */
+void inst_skip(uint32_t *sp)
+{
+	uint32_t pc = sp[6];
+	uint16_t inst = *(uint16_t*)pc;
+
+	if((inst>>11)>=0b11101)
+		pc += 4;
+	else
+		pc += 2;
+	sp[6] = pc;
+}
+
+int get_svc(uint32_t *sp)
+{
+	uint32_t pc = sp[6]-2;
+	uint16_t inst = *(uint16_t*)pc;
+
+	if((inst&0xff00)!=0xdf00)
+		return -1;
+	
+	return inst&0xff;
+}
diff --git a/test2.c b/test2.c
index 1350e2e..2cf26aa 100644
--- a/test2.c
+++ b/test2.c
@@ -4,7 +4,6 @@
 
 static int test;
 
-static __attribute__((unused))
 void hard(uint32_t *sp)
 {
 	uint32_t temp;
@@ -28,7 +27,6 @@ void hard(uint32_t *sp)
 	sp[6] = temp;
 }
 
-static __attribute__((unused))
 void usage(uint32_t *sp)
 {
 	uint32_t temp;
diff --git a/test8.c b/test8.c
new file mode 100644
index 0000000..3158952
--- /dev/null
+++ b/test8.c
@@ -0,0 +1,224 @@
+/* Test MPU
+ */
+#include "armv7m.h"
+
+char __end_rom, __after_all_load;
+
+char offlimits[1024] __attribute__((aligned(1024)));
+
+char privonly[1024] __attribute__((aligned(1024)));
+
+static
+void try(volatile char *p)
+{
+	uint32_t tval = 0;
+	puts("Try ");
+	puthex((uint32_t)p);
+	putc('\n');
+	__asm__ ("mov %r0, #'X'; ldr %r0, [%r1]" : "+r"(tval) : "r"(p) :);
+	puts("Got ");
+	putc(tval);
+	putc('\n');
+}
+
+static volatile
+unsigned expect_fault = 1;
+
+static
+void checkfault(unsigned v)
+{
+	if(v!=expect_fault) {
+		puts("expect_fault ");
+		puthex(expect_fault);
+		puts(" != ");
+		puthex(v);
+		putc('\n');
+		abort();
+	}
+}
+
+void hard(uint32_t *sp)
+{
+	if(expect_fault==10) {
+		sp = get_src_stack(sp);
+		inst_skip(sp);
+		puts("HardFault access offlimits\n");
+		try(offlimits);
+		expect_fault = 11;
+		return;
+	}
+	puts("Unexpected HardFault!!\n");
+	abort();
+}
+
+static __attribute__((naked))
+void hard_entry(void)
+{
+	asm("mov r0, sp");
+	asm("b hard");
+}
+
+void mem(uint32_t *sp)
+{
+	sp = get_src_stack(sp);
+	inst_skip(sp);
+	puts("In MemFault\n Addr ");
+	puthex(in32((void*)0xe000ed34));
+	puts(" From ");
+	puthex(sp[6]);
+	putc('\n');
+
+	switch(expect_fault) {
+	case 2: /* expected */
+		expect_fault = 3;
+		break;
+	case 1:
+		puts("Unexpected MemFault!!\n");
+		abort();
+	default:
+		puts("Unexpected state ");
+		puthex(expect_fault);
+		putc('\n');
+		abort();
+	}
+}
+
+static __attribute__((naked))
+void mem_entry(void)
+{
+	asm("mov r0, sp");
+	asm("b mem");
+}
+
+static inline
+void drop_priv(void)
+{
+	uint32_t val;
+	__asm__ ("mrs %0, CONTROL" : "=r"(val) ::);
+	val |= 1;
+	__asm__ ("msr CONTROL, %0" :: "r"(val) :);
+}
+
+void svc(void *sp)
+{
+	int num;
+	sp = get_src_stack(sp);
+	num = get_svc(sp);
+	puts("In SVC ");
+	if(num<0) {
+		puts("BAD\n");
+		abort();
+	} else {
+		puthex(num);
+		putc('\n');
+	}
+	switch(num) {
+	case 1: /* restore privlage */
+	{
+		uint32_t val;
+		__asm__ ("mrs %0, CONTROL" : "=r"(val) ::);
+		val &= ~1;
+		__asm__ ("msr CONTROL, %0" :: "r"(val) :);
+	}
+		break;
+	case 2:
+		abort();
+	default:
+		puts("Unknown\n");
+		abort();
+	}
+}
+
+static __attribute__((naked))
+void svc_entry(void)
+{
+	asm("mov r0, sp");
+	asm("b svc");
+}
+
+void main(void)
+{
+	run_table.hard = &hard_entry;
+	run_table.svc = &svc_entry;
+	run_table.mem = &mem_entry;
+
+	asm("cpsid if");
+
+	puts("1. In Main\n");
+	expect_fault = 1;
+
+	/* priority of entries is highest to lowest (0 checked last) */
+
+	/* base for ROM */
+	set_mpu(0, 0, (uint32_t)&__end_rom,
+			MPU_NORMAL|MPU_RORO);
+	/* prevent accidental *NULL */
+//	set_mpu(1, 0, 64,
+//			MPU_XN|MPU_NORMAL|MPU_NANA);
+	/* base for RAM */
+	set_mpu(2, 0x20000000, (uint32_t)(&__after_all_load)-0x20000000,
+			MPU_NORMAL|MPU_RWRW);
+	/* allow unpriv uart access */
+	set_mpu(3, (uint32_t)UART_DATA, 1024,
+			MPU_XN|MPU_DEVICE|MPU_RWRW);
+
+	/* disable all access to offlimits[] */
+	set_mpu(4, (uint32_t)offlimits, sizeof(offlimits),
+			MPU_XN|MPU_NORMAL|MPU_NANA);
+	/* disable unpriv access to privonly[] */
+	set_mpu(5, (uint32_t)privonly, sizeof(privonly),
+			MPU_XN|MPU_NORMAL|MPU_RONA);
+
+	puts("2. Access offlimits\n");
+	try(offlimits);
+	checkfault(1);
+
+	puts("3. Enable MPU\n");
+	enable_mpu(1,0);
+
+	asm("cpsie if");
+
+	expect_fault = 2;
+	puts("4. Access offlimits\n");
+	try(offlimits);
+	checkfault(3);
+
+	puts("6. In Main\n");
+	expect_fault = 1;
+
+	puts("7. Access privonly\n");
+	try(privonly);
+	checkfault(1);
+
+	puts("8. drop_priv\n");
+	expect_fault = 2;
+	drop_priv();
+	puts("9. Access privonly\n");
+	try(privonly);
+
+	puts("11. In Main\n");
+	asm("svc 1");
+	checkfault(3);
+	expect_fault = 1;
+
+	puts("12. Access privonly\n");
+	try(privonly);
+	checkfault(1);
+
+	puts("13. In Main\n");
+	asm("cpsid i");
+
+
+	puts("14. fault to hardfault\n");
+	expect_fault = 10;
+	try(offlimits);
+	checkfault(11);
+
+	puts("15. Enable MPU w/ HFNMIENA\n");
+	enable_mpu(1,1);
+	puts("16. fault to hardfault (will escalate to unrecoverable)\n");
+	expect_fault = 10;
+	try(offlimits);
+
+	puts("Shouldn't be here!\n");
+}