Modularized

6 files changed, 263 insertions, 0 deletions

diff --git a/kernel/exceptions/data.S b/kernel/exceptions/data.S
new file mode 100644
index 0000000..fe33215
--- /dev/null
+++ b/kernel/exceptions/data.S
@@ -0,0 +1,29 @@
+.section ".text.exceptions"
+.globl data
+data:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	ldr r4, [lr, #-4]
+	// Output return address
+	mov r0, #80
+	mov r1, #0
+	mov r2, r4
+	sub r2, #8
+	bl draw_hex32
+	// Output the data at the address
+	mov r0, #80
+	mov r1, #1
+	ldr r2, [r4, #-8]
+	bl draw_hex32
+	// Output the Program Status
+	mov r0, #80
+	mov r1, #2
+	mrs r2, spsr
+	bl draw_hex32
+	// Output the data-fault register
+	mov r0, #80
+	mov r1, #3
+	mrc p15, 0, r2, c5, c0, 0 //// https://developer.arm.com/documentation/ddi0464/d/System-Control/Register-descriptions/Data-Fault-Status-Register?lang=en
+	bl draw_hex32
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4 // Should be 8 once I can actually handle the abort
diff --git a/kernel/exceptions/fiq.S b/kernel/exceptions/fiq.S
new file mode 100644
index 0000000..005ed76
--- /dev/null
+++ b/kernel/exceptions/fiq.S
@@ -0,0 +1,27 @@
+.section ".text.exceptions"
+.globl fiq
+fiq:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	bl c_fiq_handler
+	cmp r0, #1
+	bne 1f
+	// Schedule if interrupted a thread
+	mrs r1, spsr
+	and r1, r1, #0x1f
+	cmp r1, #0x10
+	bne 1f
+	ldmfd sp!, {r0-r12,lr}
+	// Don't skip missed instruction upon return
+	sub lr, #4
+	push {r3}
+	// Store the instruction in a special area for
+	//  future processing
+	ldr r3, =irqlr
+	str lr, [r3, #0]
+	pop {r3}
+	cps #0x13
+	b schedule
+1:
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4
diff --git a/kernel/exceptions/irq.S b/kernel/exceptions/irq.S
new file mode 100644
index 0000000..a7e78bc
--- /dev/null
+++ b/kernel/exceptions/irq.S
@@ -0,0 +1,28 @@
+.section ".text.exceptions"
+.globl irq
+irq:
+	cpsid ai
+	stmfd sp!, {r0-r12,lr}
+	// Run IRQ handler
+	bl c_irq_handler
+	cmp r0, #1
+	bne 1f
+	// Schedule if interrupted a thread
+	mrs r1, spsr
+	and r1, r1, #0x1f
+	cmp r1, #0x10
+	bne 1f
+	ldmfd sp!, {r0-r12,lr}
+	// Don't skip missed instruction upon return
+	sub lr, #4
+	push {r3}
+	// Store the instruction in a special area for
+	//  future processing
+	ldr r3, =irqlr
+	str lr, [r3, #0]
+	pop {r3}
+	cps #0x13
+	b schedule
+1:
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4
diff --git a/kernel/exceptions/prefetch.S b/kernel/exceptions/prefetch.S
new file mode 100644
index 0000000..59674bd
--- /dev/null
+++ b/kernel/exceptions/prefetch.S
@@ -0,0 +1,13 @@
+.section ".text.exceptions"
+.globl prefetch
+prefetch:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	ldr r4, [lr, #-4]
+	// Output return address
+	mov r0, #98
+	mov r1, #0
+	mov r2, r4
+	bl draw_hex32
+	ldmfd sp!, {r0-r12,lr}
+	subs pc, lr, #4
diff --git a/kernel/exceptions/svc.S b/kernel/exceptions/svc.S
new file mode 100644
index 0000000..a24bac9
--- /dev/null
+++ b/kernel/exceptions/svc.S
@@ -0,0 +1,145 @@
+.section ".text.exceptions"
+.globl svc
+svc:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	// Get the SVC Exception #
+	ldr r0, [lr, #-4]
+	bic r0, #0xFF000000
+	// Check it is within our defined SVC
+	cmp r0, #7
+	adrle r3, svc_table_1
+	ldrle pc, [r3, r0, LSL #2]
+	sub r0, #8
+	cmp r0, #7
+	bgt svc_exit
+	//// Jump to the appropriate Call
+	adr r3, svc_table_2
+	ldr pc, [r3, r0, LSL #2]
+svc_000000: // SYS_YIELD
+	bl yield
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+svc_000001: // SYS_TIME
+	mov r2, #0x3004
+	movt r2, #0x3F00
+	ldr r0, [r2, #4] // <- SYS_TIMER_CLO
+	ldr r1, [r2, #0] // <- SYS_TIMER_CHI
+	str r0, [sp]	 // Return value
+	str r1, [sp, #4] // Return value hi
+	b svc_exit
+svc_000002: // Run Schedule
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+svc_000003: // Add Thread
+	ldr r0, [sp, #0]
+	ldr r1, [sp, #4]
+	ldr r2, [sp, #8]
+	and r2, #0xFF
+	bl svc_add_thread
+	str r0, [sp, #0]
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+svc_000004: // Lock Lock (usr_r0 = struct Lock*)
+	ldr r3, =scheduler
+	ldr r2, [r3, #0] // struct Thread* rthread
+	ldr r1, [r2, #0x10] // unsigned long pid
+	ldr r0, [sp, #0] // struct Lock* m
+1:	clrex
+	ldrex r2, [r0, #0]
+	cmp r2, #0
+	// If it is not available, wait-queue the thread
+	bne svc_000004_delay_mutex
+	// Otherwise lock it
+	strexeq r2, r1, [r0, #0]
+	teq r2, #0
+	bne 1b
+	dmb
+	b svc_exit
+svc_000004_delay_mutex: // Wait-queue the current thread
+	// r0 = struct Lock* m
+	bl sched_mutex_yield
+	ldmfd sp!, {r0-r12,lr}
+	sub lr, #4
+	b schedule
+svc_000005: // Release Lock
+	ldr r0, [sp, #0] // struct Lock* m
+	mov r1, #0
+	dmb
+	// Unlock
+	str r1, [r0, #0]
+	dsb
+	sev
+	// Awake any threads waiting for this lock
+	bl sched_mutex_resurrect
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+	b svc_exit
+svc_000006: // Semaphore decrease
+	ldr r0, [sp, #0] // struct Semaphore* s
+1:	clrex
+	ldrex r2, [r0, #0]
+	cmp r2, #0
+	beq svc_000006_delay_semaphore
+	sub r1, r2, #1
+	strex r2, r1, [r0, #0]
+	teq r2, #0
+	bne 1b
+	dmb
+	b svc_exit
+svc_000006_delay_semaphore:
+	bl sched_semaphore_yield
+	ldmfd sp!, {r0-r12,lr}
+	sub lr, #4
+	b schedule
+	b svc_exit
+svc_000007: // Semaphore increase
+	ldr r0, [sp, #0] // struct Semaphore* s
+1:	clrex
+	ldrex r2, [r0, #0]
+	add r1, r2, #1
+	strexeq r2, r1, [r0, #0]
+	teq r2, #0
+	bne 1b
+	dmb
+	cmp r1, #1
+	bne svc_exit
+	mov r1, #1
+	bl sched_semaphore_resurrect
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+	b svc_exit
+svc_000008: // Semaphore add #
+	ldr r0, [sp, #0] // struct Semaphore* s
+	ldr r3, [sp, #1] // unsigned long # times to increase
+1:	clrex
+	ldrex r2, [r0, #0]
+	add r1, r2, #1
+	strexeq r2, r1, [r0, #0]
+	teq r2, #0
+	bne 1b
+	dmb
+	mov r1, r3
+	bl sched_semaphore_resurrect
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+	b svc_exit
+svc_000009: // SYS_TIME_2
+	mrc p15, 0, r0, c9, c13, 0
+	str r0, [sp, #0]
+	b svc_exit
+svc_exit:
+	ldmfd sp!, {r0-r12,pc}^
+
+svc_table_1:
+	.word svc_000000
+	.word svc_000001
+	.word svc_000002
+	.word svc_000003
+	.word svc_000004
+	.word svc_000005
+	.word svc_000006
+	.word svc_000007
+svc_table_2:
+	.word svc_000008
+	.word svc_000009
diff --git a/kernel/exceptions/undefined.S b/kernel/exceptions/undefined.S
new file mode 100644
index 0000000..856e30f
--- /dev/null
+++ b/kernel/exceptions/undefined.S
@@ -0,0 +1,21 @@
+.section ".text.exceptions"
+.globl undefined
+undefined:
+	cpsid aif
+	stmfd sp!, {r0-r12,lr}
+	ldr r4, [lr, #-4]
+	mov r0, #62
+	mov r1, #0
+	mov r2, r4
+	bl draw_hex32
+	// Output lr
+	ldr r0, [sp, #0x34]
+	sub r2, r0, #4
+	mov r0, #62
+	mov r1, #1
+	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}^