Memory stuff and add not working (so far) MMU/pagetable enable code

main.c

@@ -150,6 +150,9 @@ void *_main(void *base)
 	gecko_init();
 	gecko_puts("MiniIOS v0.1 loading\n");
 
+	gecko_puts("Configuring caches and MMU...\n");
+	mem_initialize();
+
 
 
 	irq_initialize();
@@ -186,6 +189,8 @@ void *_main(void *base)
 	ipc_shutdown();
 	gecko_puts("Shutting down interrupts...\n");
 	irq_shutdown();
+	gecko_puts("Shutting down caches and MMU...\n");
+	mem_shutdown();
 
 	//vector = patch_boot2(base, (((u64)tidh)<<32) | tidl);
 

memory.c

@@ -4,12 +4,17 @@
 #include "utils.h"
 #include "gecko.h"
 #include "hollywood.h"
+#include "irq.h"
 
 void _dc_inval_entries(void *start, int count);
 void _dc_flush_entries(void *start, int count);
 void _dc_flush(void);
+void _dc_inval(void);
 void _ic_inval(void);
 void _drain_write_buffer(void);
+void _tlb_inval(void);
+
+extern u32 __page_table[4096];
 
 #define LINESIZE 0x20
 #define CACHESIZE 0x4000
@@ -166,6 +171,7 @@ void ahb_memflush(enum AHBDEV dev)
 
 void dc_flushrange(void *start, u32 size)
 {
+	u32 cookie = irq_kill();
 	if(size > 0x4000) {
 		_dc_flush();
 	} else {
@@ -175,27 +181,34 @@ void dc_flushrange(void *start, u32 size)
 	}
 	_drain_write_buffer();
 	//ahb_memflush(MEMORY);
+	irq_restore(cookie);
 }
 
 void dc_invalidaterange(void *start, u32 size)
 {
+	u32 cookie = irq_kill();
 	void *end = ALIGN_FORWARD(((u8*)start) + size, LINESIZE);
 	start = ALIGN_BACKWARD(start, LINESIZE);
 	_dc_inval_entries(start, (end - start) / LINESIZE);
 	//_magic_bullshit(0);
+	irq_restore(cookie);
 }
 
 void dc_flushall(void)
 {
+	u32 cookie = irq_kill();
 	_dc_flush();
 	_drain_write_buffer();
 	//ahb_memflush(MEMORY);
+	irq_restore(cookie);
 }
 
 void ic_invalidateall(void)
 {
+	u32 cookie = irq_kill();
 	_ic_inval();
 	//_magic_bullshit(0);
+	irq_restore(cookie);
 }
 
 void mem_protect(int enable, void *start, void *end)
@@ -216,3 +229,80 @@ void mem_setswap(int enable)
 		write32(HW_MEMMIRR, d | 0x20);
 
 }
+
+#define SECTION				0x012
+
+#define	NONBUFFERABLE		0x000
+#define	BUFFERABLE			0x004
+#define	WRITETHROUGH_CACHE	0x008
+#define	WRITEBACK_CACHE		0x00C
+
+#define DOMAIN(x)			((x)<<5)
+
+#define AP_ROM				0x000
+#define AP_NOUSER			0x400
+#define AP_ROUSER			0x800
+#define AP_RWUSER			0xC00
+
+// from, to, size: units of 1MB
+void map_section(u16 from, u16 to, u16 size, u32 attributes)
+{
+	attributes |= SECTION;
+	while(size--) {
+		__page_table[from++] = (to++<<20) | attributes;
+	}
+}
+
+void mem_initialize(void)
+{
+	u32 cookie = irq_kill();
+
+	gecko_printf("MEM: cleaning up\n");
+
+	_ic_inval();
+	_dc_inval();
+	_tlb_inval();
+
+	gecko_printf("MEM: mapping sections\n");
+
+	memset32(__page_table, 0, 16384);
+
+	map_section(0x000, 0x000, 0x018, WRITEBACK_CACHE | DOMAIN(0) | AP_RWUSER);
+	map_section(0x100, 0x100, 0x040, WRITEBACK_CACHE | DOMAIN(0) | AP_RWUSER);
+	map_section(0x0d0, 0x0d0, 0x001, NONBUFFERABLE | DOMAIN(0) | AP_RWUSER);
+	map_section(0x0d8, 0x0d8, 0x001, NONBUFFERABLE | DOMAIN(0) | AP_RWUSER);
+	map_section(0xfff, 0xfff, 0x001, WRITEBACK_CACHE | DOMAIN(0) | AP_RWUSER);
+
+	set_dacr(0xFFFFFFFF); //manager access for all domains, ignore AP
+
+	_drain_write_buffer();
+
+	gecko_printf("MEM: enabling caches\n");
+
+	u32 cr = get_cr();
+	cr |= 0x1004; //ICACHE/DCACHE and MMU enable
+	set_cr(cr);
+
+	gecko_printf("MEM: enabling MMU\n");
+
+	cr |= 0x0001; //ICACHE/DCACHE and MMU enable
+	set_cr(cr);
+
+	gecko_printf("MEM: init done\n");
+
+	irq_restore(cookie);
+}
+
+void mem_shutdown(void)
+{
+	u32 cookie = irq_kill();
+	_dc_flush();
+	_drain_write_buffer();
+	u32 cr = get_cr();
+	cr &= ~0x1005; //disable ICACHE, DCACHE, MMU
+	set_cr(cr);
+	_ic_inval();
+	_dc_inval();
+	_tlb_inval();
+	irq_restore(cookie);
+}

memory.h

@@ -24,4 +24,58 @@ void ahb_memflush(enum AHBDEV dev);
 void mem_protect(int enable, void *start, void *end);
 void mem_setswap(int enable);
 
+void mem_initialize(void);
+void mem_shutdown(void);
+
+static inline u32 get_cr(void)
+{
+	u32 data;
+	__asm__ volatile ( "mrc\tp15, 0, %0, c1, c0, 0" : "=r" (data) );
+	return data;
+}
+static inline u32 get_ttbr(void)
+{
+	u32 data;
+	__asm__ volatile ( "mrc\tp15, 0, %0, c2, c0, 0" : "=r" (data) );
+	return data;
+}
+static inline u32 get_dacr(void)
+{
+	u32 data;
+	__asm__ volatile ( "mrc\tp15, 0, %0, c3, c0, 0" : "=r" (data) );
+	return data;
+}
+static inline void set_cr(u32 data)
+{
+	__asm__ volatile ( "mcr\tp15, 0, %0, c1, c0, 0" :: "r" (data) );
+}
+static inline void set_ttbr(u32 data)
+{
+	__asm__ volatile ( "mcr\tp15, 0, %0, c2, c0, 0" :: "r" (data) );
+}
+static inline void set_dacr(u32 data)
+{
+	__asm__ volatile ( "mcr\tp15, 0, %0, c3, c0, 0" :: "r" (data) );
+}
+
+static inline u32 get_dfsr(void)
+{
+	u32 data;
+	__asm__ volatile ( "mrc\tp15, 0, %0, c5, c0, 0" : "=r" (data) );
+	return data;
+}
+static inline u32 get_ifsr(void)
+{
+	u32 data;
+	__asm__ volatile ( "mrc\tp15, 0, %0, c5, c0, 1" : "=r" (data) );
+	return data;
+}
+static inline u32 get_far(void)
+{
+	u32 data;
+	__asm__ volatile ( "mrc\tp15, 0, %0, c6, c0, 0" : "=r" (data) );
+	return data;
+}
+
+
 #endif

memory_asm.S

@@ -3,8 +3,10 @@
 .globl _dc_inval_entries
 .globl _dc_flush_entries
 .globl _dc_flush
+.globl _dc_inval
 .globl _ic_inval
 .globl _drain_write_buffer
+.globl _tlb_inval
 
 .text
 
@@ -27,6 +29,11 @@ _dc_flush:
 	bne		_dc_flush
 	bx		lr
 
+_dc_inval:
+	mov		r0, #0
+	mcr		p15, 0, r0, c7, c6, 0
+	bx		lr
+
 _ic_inval:
 	mov		r0, #0
 	mcr		p15, 0, r0, c7, c5, 0
@@ -37,3 +44,7 @@ _drain_write_buffer:
 	mcr		p15, 0, r0, c7, c10, 4
 	bx		lr
 
+_tlb_inval:
+	mov		r0, #0
+	mcr		p15, 0, r0, c8, c7
+	bx		lr