8 files changed, 108 insertions, 35 deletions

diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 125c3a6..cfa9d7d 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -5,11 +5,13 @@
 #include <symbols.h>
 #include <sys/core.h>
 #include <sys/schedule.h>
-#include <sys/timer.h>
 #include <tests/test.h>
 #include <util/mutex.h>
+#include <util/status.h>
 #include <util/time.h>
 
+#define CPS 1000
+
 void handle_data(unsigned char);
 
 void c_irq_handler(void)
@@ -59,7 +61,7 @@ void c_irq_handler(void)
 				}
 				// Add task to handle the data
 				else {
-					add_thread(handle_data, (void*)data, 1);
+					//add_thread(handle_data, (void*)data, 1);
 				}
 				return;
 			}
@@ -75,7 +77,6 @@ void c_irq_handler(void)
 	}
 	// Check if CNTV triggered the interrupt
 	else if (source & (1 << 3)) {
-		c_timer();
 		return;
 	}
 	return;
@@ -87,14 +88,15 @@ unsigned long c_fiq_handler(void)
 	unsigned long source = load32(CORE0_FIQ_SOURCE);
 	// Check if CNTV triggered the interrupt
 	if (source & (1 << 3)) {
-		c_timer();
+		// Reset the counter
+		write_cntv_tval(cntfrq/CPS);
 		counter++;
-		if (counter % 0x6000 == 0) {
+		if (counter % 0x6000 == 0)
 			counter = 0;
-		}
-		if (counter % 0x30 == 0) {
+		if (counter % 0x10 == 0)
+			status();
+		if (counter % 0x40 == 0)
 			return 1;
-		}
 		return 0;
 	}
 	return 0;
diff --git a/src/graphics/lfb.c b/src/graphics/lfb.c
index 20053bc..ee31514 100644
--- a/src/graphics/lfb.c
+++ b/src/graphics/lfb.c
@@ -7,8 +7,10 @@
 
 unsigned char *lfb;                         /* raw frame buffer address */
 
-#define SCR_WIDTH  1024
-#define SCR_HEIGHT 768
+//#define SCR_WIDTH  1024
+//#define SCR_HEIGHT 768
+#define SCR_WIDTH  1920
+#define SCR_HEIGHT 1080
 
 /**
  * Set screen resolution to 1024x768
diff --git a/src/lib/mmu.S b/src/lib/mmu.S
new file mode 100644
index 0000000..faca3cc
--- /dev/null
+++ b/src/lib/mmu.S
@@ -0,0 +1,45 @@
+.section .text
+.globl mmu_start
+mmu_start:
+	mov r2, #0
+	// Invalidate Caches
+	mcr p15,0,r2,c7,c1,6 
+	// Invalidate TLB entries
+	mcr p15,0,r2,c8,c7,0 
+	// Data synchronisation barrier
+	mcr p15,0,r2,c7,c10,4 
+
+	// Set all domains to 0b11
+	mvn r2, #0
+	bic r2, #0xC
+	mcr p15,0,r2,c3,c0,0
+
+	// Set the translation table base address (remember to align 16 KiB!)
+	mcr p15,0,r0,c2,c0,0
+	mcr p15,0,r0,c2,c0,1
+	mov r3, #0
+	mcr p15,0,r3,c2,c0,2
+
+	// Set the bits mentioned above
+	mrc p15,0,r2,c1,c0,0
+	orr r2,r2,r1
+	mcr p15,0,r2,c1,c0,0
+	bx lr
+
+.globl mmu_stop
+mmu_stop:
+	mrc p15,0,r2,c1,c0,0
+	bic r2,#0x1000
+	bic r2,#0x0004
+	bic r2,#0x0001
+	mcr p15,0,r2,c1,c0,0
+	bx lr
+
+.globl tlb_invalidate
+tlb_invalidate:
+	mov r2, #0
+	// Invalidate Entries
+	mcr p15, 0, r2, c8, c7, 0
+	// DSB
+	mcr p15, 0, r2, c7, c10, 4
+	bx lr
diff --git a/src/lib/mmu.c b/src/lib/mmu.c
new file mode 100644
index 0000000..6a947c5
--- /dev/null
+++ b/src/lib/mmu.c
@@ -0,0 +1,32 @@
+#include <lib/mmu.h>
+
+#define CACHABLE 0x08
+#define BUFFERABLE 0x04
+#define NO_PERMISSIONS_REQUIRED 0b11 << 10
+#define MMU_TABLE_BASE 0x00004000
+
+void mmu_start(unsigned long base, unsigned long flags);
+
+void mmu_section(unsigned long virtual, unsigned long physical, unsigned long flags)
+{
+	unsigned long offset = virtual >> 20;
+	unsigned long* entry = (unsigned long*)(MMU_TABLE_BASE | (offset << 2));
+	unsigned long physval = (physical & 0xFFF00000) | (flags & 0x7FFC) | 0x00C02;
+	*entry = physval;
+}
+
+void mmu_init(void)
+{
+	mmu_section(0x00000000, 0x00000000, CACHABLE | BUFFERABLE);
+	for (unsigned long addr =  0x00100000;; addr += 0x00100000) {
+		mmu_section(addr, addr, NO_PERMISSIONS_REQUIRED);
+		if (addr == 0x02000000)
+			mmu_section(addr, addr, CACHABLE | BUFFERABLE | NO_PERMISSIONS_REQUIRED);
+		//else if (addr == 0x3F000000)
+		//else
+		//	mmu_section(addr, addr, NO_PERMISSIONS_REQUIRED);
+		if (addr == 0xFFF00000)
+			break;
+	}
+	mmu_start(MMU_TABLE_BASE,0x00000001|0x1000|0x0004);
+}
diff --git a/src/sys/core.c b/src/sys/core.c
index 1ee898c..99c08ae 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -5,12 +5,12 @@
 #include <graphics/drawer.h>
 #include <graphics/lfb.h>
 #include <lib/kmem.h>
+#include <lib/mmu.h>
 #include <lib/strings.h>
 #include <symbols.h>
 #include <sys/core.h>
 #include <sys/power.h>
 #include <sys/schedule.h>
-#include <sys/timer.h>
 #include <util/mutex.h>
 #include <util/time.h>
 
@@ -35,7 +35,7 @@ void sysinit(void)
 	// Get the frequency
 	cntfrq = read_cntfrq();
 	// Clear cntv interrupt and set next 1 second timer
-	write_cntv_tval(cntfrq);
+	write_cntv_tval(cntfrq/100);
 	// Route timer to core0 fiq
 	routing_core0cntv_to_core0fiq();
 	// Enable timer
@@ -47,6 +47,8 @@ void sysinit(void)
 	lfb_init();
 	lfb_showpicture();
 
+	mmu_init();
+
 	// Start Scheduler
 	init_scheduler();
 }
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index 654a9a7..23bf6a7 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -6,13 +6,23 @@
 kernel_main:
 	///https://wiki.osdev.org/ARM_Paging
 	// Query the ID_MMFR0 register
-	mrc p15, 0, r0, c0, c1, 4
+	//mrc p15, 0, r0, c0, c1, 4
 	bl sysinit
+	bl status
+	ldr r2, =ttbr_msg
+	mov r0, #0
+	mov r1, #0
+	mov r3, #0xFF00
+	bl draw_cstring
 
 	// Intentional undefined instruction
 	// .word 0xf7f0a000
 	cpsie aif, #0x10
 	svc #2 // Start scheduling!
-1:
+2:
 	wfe
-	b 1b
+	b 2b
+
+.section .data
+ttbr_msg:
+	.asciz "MMU Initialized!"
diff --git a/src/sys/timer.c b/src/sys/timer.c
deleted file mode 100644
index c8f9922..0000000
--- a/src/sys/timer.c
+++ /dev/null
@@ -1,17 +0,0 @@
-#include <drivers/uart.h>
-#include <globals.h>
-#include <graphics/drawer.h>
-#include <sys/core.h>
-#include <sys/timer.h>
-#include <util/mutex.h>
-#include <util/status.h>
-#include <util/time.h>
-#include <symbols.h>
-
-void c_timer(void)
-{
-	// Reset the counter
-	write_cntv_tval(cntfrq/CPS);
-
-	status();
-}
diff --git a/src/util/status.c b/src/util/status.c
index 1d3894b..a54e7c0 100644
--- a/src/util/status.c
+++ b/src/util/status.c
@@ -5,7 +5,6 @@
 #include <symbols.h>
 #include <sys/core.h>
 #include <sys/schedule.h>
-#include <sys/timer.h>
 #include <util/mutex.h>
 #include <util/status.h>
 #include <util/time.h>
@@ -104,8 +103,6 @@ void status(void)
 	g_Drawer.x = 0;
 	g_Drawer.y = 5;
 	write_string(&g_Drawer, "SVC      IRQ      FIQ      User/SYS\n");
-	for(int i = 0; i < 128; i++)
-		write_char(&g_Drawer, ' ');
 	g_Drawer.x = 0;
 	g_Drawer.y = 6;
 	unsigned long sp = (unsigned long)getsvcstack();