From a1f49718aee378fa075303c86106309091d56b80 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Mon, 31 Jan 2022 19:05:50 -0700
Subject: Break exceptions off to own files

---
 linker.ld                  |   1 +
 src/boot.S                 | 139 +--------------------------------------------
 src/cpu/irq.c              |  29 +---------
 src/exceptions/data.S      |  29 ++++++++++
 src/exceptions/fiq.S       |  26 +++++++++
 src/exceptions/irq.S       |   8 +++
 src/exceptions/prefetch.S  |  20 +++++++
 src/exceptions/svc.S       |  45 +++++++++++++++
 src/exceptions/undefined.S |  34 +++++++++++
 src/lib/mem.c              |   8 +--
 src/sys/core.c             |   6 +-
 src/sys/kernel.S           |   2 +-
 12 files changed, 176 insertions(+), 171 deletions(-)
 create mode 100644 src/exceptions/data.S
 create mode 100644 src/exceptions/fiq.S
 create mode 100644 src/exceptions/irq.S
 create mode 100644 src/exceptions/prefetch.S
 create mode 100644 src/exceptions/svc.S
 create mode 100644 src/exceptions/undefined.S

diff --git a/linker.ld b/linker.ld
index 17c951a..474bdeb 100644
--- a/linker.ld
+++ b/linker.ld
@@ -9,6 +9,7 @@ SECTIONS
     .text :
     {
         KEEP(*(.text.boot))
+        KEEP(*(.text.exceptions))
         KEEP(*(.text.kernel))
         *(.text*)
     }
diff --git a/src/boot.S b/src/boot.S
index 0cbd8e3..8c9becf 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -6,7 +6,7 @@
  
 _start:
 reset:
-	cpsid if
+	cpsid aif
 	// disable core0,1,2.
 	mrc p15, #0, r1, c0, c0, #5
 	and r1, r1, #3
@@ -50,12 +50,10 @@ reset:
 	mov r8, #0
 	b       2f
  
-1:
-	// store multiple at r4.
+1:	// store multiple at r4.
 	stmia r4!, {r5-r8}
  
-	// If we are still below bss_end, loop.
-2:
+2:	// If we are still below bss_end, loop.
 	cmp r4, r9
 	blo 1b
 
@@ -63,12 +61,6 @@ reset:
 	ldr r3, =kernel_main
 	blx r3
 
-irq:
-	push {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
-	bl c_irq_handler
-	pop  {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
-	subs pc, lr, #4
-
 // TODO: Each core needs to set up their stacks
 core1run:
 core2run:
@@ -78,123 +70,6 @@ io_halt:
 	wfi
 	b io_halt
 
-undefined:
-	stmfd sp!, {r0-r12,lr}
-	ldr r4, [lr, #-4]
-	mov r0, #0
-	mov r1, #17
-	ldr r2, =undefined_msg
-	bl draw_string
-	mov r0, #18
-	mov r1, #17
-	mov r2, r4
-	bl draw_hex32
-	mov r0, #27
-	mov r1, #17
-	ldr r2, =undefined_at
-	bl draw_string
-	// Output lr
-	mov r1, #0x1000
-	ldr r0, [r1, #-4]
-	sub r2, r0, #4
-	mov r0, #31
-	mov r1, #17
-	bl draw_hex32
-	// Skip instruction for now
-	// In future, 
-	//  ldmfd sp!, {r0-r12,lr} // Note the lack of ^ since subs will handle it
-	//  subs pc, lr, #4
-	ldmfd sp!, {r0-r12,pc}^
-svc:
-	cpsid f
-	stmfd sp!, {r0-r12,lr}
-	ldr r0, [lr, #-4]
-	bic r0, #0xFF000000
-	// SVC #0 returns to supervisor mode
-	// TODO: Make supervisor mode return to a specific location
-	//  (rather than to a user location) such as the kernel loop
-	cmp r0, #3
-	bgt svc_exit
-	beq svc_000003
-	cmp r0, #2
-	beq svc_000002
-	cmp r0, #1
-	beq svc_000001
-	cmp r0, #1
-	beq svc_000000
-svc_000000:
-	cps #0x13
-	b svc_exit
-svc_000001:
-	b svc_exit
-svc_000002:
-	ldmfd sp!, {r0-r12,lr}
-	b schedule
-svc_000003:
-	ldr r3, =scheduler
-	ldr r2, [r3, #0]
-	ldr r1, [r2, #8] // sp_base
-	cmp r1, #-1
-	beq svc_exit
-	ldr r3, =stacks_table
-	mov r0, #0
-	strb r0, [r3, r1]
-	// Free the thread after freeing the stack
-	mov r0, r2
-	bl free
-	b svc_exit
-svc_exit:
-	ldmfd sp!, {r0-r12,pc}^
-prefetch:
-	stmfd sp!, {r0-r12,lr}
-	mov r4, lr
-	mov r0, #0
-	mov r1, #16
-	ldr r2, =prefetch_msg
-	bl draw_string
-	// Output return address
-	mov r0, #17
-	mov r1, #16
-	mov r2, r4
-	bl draw_hex32
-	ldmfd sp!, {r0-r12,lr}
-	subs pc, lr, #4
-data:
-	stmfd sp!, {r0-r12,lr}
-	mov r4, lr
-	mov r0, #0
-	mov r1, #15
-	ldr r2, =data_msg
-	bl draw_string
-	// Output return address
-	mov r0, #13
-	mov r1, #15
-	mov r2, r4
-	sub r2, #8
-	bl draw_hex32
-	mov r0, #22
-	mov r1, #15
-	ldr r2, [r4, #-8]
-	bl draw_hex32
-	ldmfd sp!, {r0-r12,lr}
-	subs pc, lr, #4 // Should be 8 once I can actually handle the abort
-fiq:
-	stmfd sp!, {r0-r12,lr}
-	bl c_fiq_handler
-	cmp r0, #1
-	bne 1f
-	ldmfd sp!, {r0-r12,lr}
-	sub lr, #4
-	push {r3}
-	ldr r3, =irqlr
-	str lr, [r3, #0]
-	pop {r3}
-	cps #0x13
-	b schedule
-1:
-	ldmfd sp!, {r0-r12,lr}
-	subs pc, lr, #4
-
 .align 5
 vector:
 	ldr pc, reset_handler
@@ -214,11 +89,3 @@ data_handler:       .word data
 unused_handler:     .word io_halt
 irq_handler:        .word irq
 fiq_handler:        .word fiq
-
-.section .data
-undefined_msg:	.asciz "Undefined Handler"
-undefined_at:	.asciz "@ 0x"
-svc_msg:	.asciz "SVC Handler #"
-prefetch_msg:	.asciz "Prefetch Handler"
-data_msg:	.asciz "Data Handler"
-fiq_msg:	.asciz "FIQ\n"
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index e6c6a80..7f2a89e 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -97,49 +97,24 @@ void handle_data(unsigned char data)
 	if (off < 2048) {
 		// Newline Case
 		if (data == 0x0D) {
-			for(int i = off; i>=0;i--)
-				cmd[i] = 0x0;
-			off = 0;
 		// Backspace Case
 		} else if (data == 0x08 || data == 0x7F) {
-			if (off > 0) {
-				off -= 1;
-			}
-			cmd[off] = 0x0;
 		// Lock Case
 		} else if (data == 0x6C) {
-			cmd[off] = (char) data;
-			off += 1;
 			lock_mutex(&exe_cnt_m, SYS_PID);
 		// Release Case
 		} else if (data == 0x72) {
-			cmd[off] = (char) data;
-			off += 1;
 			release_mutex(&exe_cnt_m, SYS_PID);
 		} else if (data == 0x61) {
-			cmd[off] = (char) data;
-			off += 1;
 			add_thread(testfxn, 0, 3);
 		} else if (data == 0x62) {
-			cmd[off] = (char) data;
-			off += 1;
 			add_thread(uart_scheduler, 0, 2);
+		} else if (data == 0x63) {
+			add_thread(heap_info, 0, 2);
 		// Else output
 		} else {
-			cmd[off] = (char) data;
-			off += 1;
 		}
 	} else if (off == 2048) {
-		if (data == 0x0D) {
-			for(int i = off; i>=0;i--)
-				cmd[i] = 0x0;
-			off = 0;
-		} else if (data == 0x08 || data == 0x7F) {
-			if (off > 0) {
-				off -= 1;
-			}
-			cmd[off] = 0x0;
-		}
 	}
 	cmdidx = off;
 	g_Drawer.x = 0;
diff --git a/src/exceptions/data.S b/src/exceptions/data.S
new file mode 100644
index 0000000..2268de5
--- /dev/null
+++ b/src/exceptions/data.S
@@ -0,0 +1,29 @@
+.section ".text.exceptions"
+.globl data
+data:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	mov r4, lr
+	mov r0, #0
+	mov r1, #15
+	ldr r2, =data_msg
+	bl draw_string
+	// Output return address
+	mov r0, #13
+	mov r1, #15
+	mov r2, r4
+	sub r2, #8
+	bl draw_hex32
+	mov r0, #22
+	mov r1, #15
+	ldr r2, [r4, #-8]
+	bl draw_hex32
+	mov r0, #32
+	mov r1, #15
+	mrs r2, spsr
+	bl draw_hex32
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4 // Should be 8 once I can actually handle the abort
+
+.section .data
+data_msg:	.asciz "Data Handler"
diff --git a/src/exceptions/fiq.S b/src/exceptions/fiq.S
new file mode 100644
index 0000000..f323a02
--- /dev/null
+++ b/src/exceptions/fiq.S
@@ -0,0 +1,26 @@
+.section ".text.exceptions"
+.globl fiq
+fiq:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	bl c_fiq_handler
+	cmp r0, #1
+	bne 1f
+	mrs r1, spsr
+	and r1, r1, #0x1f
+	cmp r1, #0x10
+	bne 1f
+	ldmfd sp!, {r0-r12,lr}
+	sub lr, #4
+	push {r3}
+	ldr r3, =irqlr
+	str lr, [r3, #0]
+	pop {r3}
+	cps #0x13
+	b schedule
+1:
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4
+
+.section .data
+fiq_msg:	.asciz "FIQ\n"
diff --git a/src/exceptions/irq.S b/src/exceptions/irq.S
new file mode 100644
index 0000000..025fc41
--- /dev/null
+++ b/src/exceptions/irq.S
@@ -0,0 +1,8 @@
+.section ".text.exceptions"
+.globl irq
+irq:
+	cpsid aif
+	push {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
+	bl c_irq_handler
+	pop  {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
+	subs pc, lr, #4
diff --git a/src/exceptions/prefetch.S b/src/exceptions/prefetch.S
new file mode 100644
index 0000000..1aeba10
--- /dev/null
+++ b/src/exceptions/prefetch.S
@@ -0,0 +1,20 @@
+.section ".text.exceptions"
+.globl prefetch
+prefetch:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	mov r4, lr
+	mov r0, #0
+	mov r1, #16
+	ldr r2, =prefetch_msg
+	bl draw_string
+	// Output return address
+	mov r0, #17
+	mov r1, #16
+	mov r2, r4
+	bl draw_hex32
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4
+
+.section .data
+prefetch_msg:	.asciz "Prefetch Handler"
diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S
new file mode 100644
index 0000000..a5e9982
--- /dev/null
+++ b/src/exceptions/svc.S
@@ -0,0 +1,45 @@
+.section ".text.exceptions"
+.globl svc
+svc:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	ldr r0, [lr, #-4]
+	bic r0, #0xFF000000
+	// SVC #0 returns to supervisor mode
+	// TODO: Make supervisor mode return to a specific location
+	//  (rather than to a user location) such as the kernel loop
+	cmp r0, #3
+	bgt svc_exit
+	beq svc_000003
+	cmp r0, #2
+	beq svc_000002
+	cmp r0, #1
+	beq svc_000001
+	cmp r0, #1
+	beq svc_000000
+svc_000000:
+	cps #0x13
+	b svc_exit
+svc_000001:
+	b svc_exit
+svc_000002:
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+svc_000003:
+	ldr r3, =scheduler
+	ldr r2, [r3, #0]
+	ldr r1, [r2, #8] // sp_base
+	cmp r1, #-1
+	beq svc_exit
+	ldr r3, =stacks_table
+	mov r0, #0
+	strb r0, [r3, r1]
+	// Free the thread after freeing the stack
+	mov r0, r2
+	bl free
+	b svc_exit
+svc_exit:
+	ldmfd sp!, {r0-r12,pc}^
+
+.section .data
+svc_msg:	.asciz "SVC Handler #"
diff --git a/src/exceptions/undefined.S b/src/exceptions/undefined.S
new file mode 100644
index 0000000..b8eab29
--- /dev/null
+++ b/src/exceptions/undefined.S
@@ -0,0 +1,34 @@
+.section ".text.exceptions"
+.globl undefined
+undefined:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	ldr r4, [lr, #-4]
+	mov r0, #0
+	mov r1, #17
+	ldr r2, =undefined_msg
+	bl draw_string
+	mov r0, #18
+	mov r1, #17
+	mov r2, r4
+	bl draw_hex32
+	mov r0, #27
+	mov r1, #17
+	ldr r2, =undefined_at
+	bl draw_string
+	// Output lr
+	mov r1, #0x1000
+	ldr r0, [r1, #-4]
+	sub r2, r0, #4
+	mov r0, #31
+	mov r1, #17
+	bl draw_hex32
+	// Skip instruction for now
+	// In future, 
+	//  ldmfd sp!, {r0-r12,lr} // Note the lack of ^ since subs will handle it
+	//  subs pc, lr, #4
+	ldmfd sp!, {r0-r12,pc}^
+
+.section .data
+undefined_msg:	.asciz "Undefined Handler"
+undefined_at:	.asciz "@ 0x"
diff --git a/src/lib/mem.c b/src/lib/mem.c
index 73c6a48..2f50d3d 100644
--- a/src/lib/mem.c
+++ b/src/lib/mem.c
@@ -222,10 +222,10 @@ void free(void* memloc)
 	unsigned char size = base[MEM_SIZE_OFFSET];
 	// TODO: Use Null PID
 	base[MEM_USE_OFFSET] = 0;
-	// // Clear out old memory
-	// for(unsigned int i = 0; i < size; i++) {
-	// 	base[i + MEM_BASE_SIZE] = 0;
-	// }
+	// Clear out old memory
+	for(unsigned int i = 0; i < size; i++) {
+		base[i + MEM_BASE_SIZE] = 0;
+	}
 	// If it is the last entry, clear it and move the heap top down
 	if (base + size + MEM_META_SIZE == rpi_heap_top) {
 		while(base[MEM_USE_OFFSET] == 0 && base >= rpi_heap) {
diff --git a/src/sys/core.c b/src/sys/core.c
index d84770d..39eaffd 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -61,9 +61,9 @@ void sysinit(void)
 	// Start Scheduler
 	init_scheduler();
 
-	// Enable IRQ & FIQ
-	enableirq();
-	enablefiq();
+	//// // Enable IRQ & FIQ
+	//// enableirq();
+	//// enablefiq();
 
 	add_thread(testlocal, 0, 0);
 	add_thread(testlocal, 0, 1);
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index b76df62..93074b6 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -17,7 +17,7 @@ kernel_main:
 	// svc #1
 	// mrs r0, cpsr
 	// bl uart_hexn
-	cps #0x10
+	cpsie aif, #0x10
 	svc #2 // Start scheduling!
 1:
 	wfe
-- 
cgit v1.2.1