From 93bf62580a68533dc8252b9a2a055c02f34ecb67 Mon Sep 17 00:00:00 2001 From: Christian Cunningham Date: Thu, 24 Mar 2022 09:38:08 -0700 Subject: Modularized --- kernel/exceptions/data.S | 29 +++++++++ kernel/exceptions/fiq.S | 27 ++++++++ kernel/exceptions/irq.S | 28 ++++++++ kernel/exceptions/prefetch.S | 13 ++++ kernel/exceptions/svc.S | 145 ++++++++++++++++++++++++++++++++++++++++++ kernel/exceptions/undefined.S | 21 ++++++ 6 files changed, 263 insertions(+) create mode 100644 kernel/exceptions/data.S create mode 100644 kernel/exceptions/fiq.S create mode 100644 kernel/exceptions/irq.S create mode 100644 kernel/exceptions/prefetch.S create mode 100644 kernel/exceptions/svc.S create mode 100644 kernel/exceptions/undefined.S (limited to 'kernel/exceptions') 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}^ -- cgit v1.2.1