From 6a72262c7ce45a57ae2b3d002aaa30f5b5fd23d5 Mon Sep 17 00:00:00 2001 From: John Hodge Date: Sat, 8 Oct 2011 18:48:57 +0800 Subject: [PATCH] Kernel/armv7 - Working on interrupt support, currently broken --- Kernel/arch/armv7/include/assembly.h | 46 ++++++++++++++++++ Kernel/arch/armv7/main.c | 2 + Kernel/arch/armv7/mm_virt.c | 3 +- Kernel/arch/armv7/proc.S | 67 ++++++++----------------- Kernel/arch/armv7/proc.c | 39 +++++++++++++-- Kernel/arch/armv7/start.S | 73 +++++++++++++++++++++++++--- 6 files changed, 168 insertions(+), 62 deletions(-) create mode 100644 Kernel/arch/armv7/include/assembly.h diff --git a/Kernel/arch/armv7/include/assembly.h b/Kernel/arch/armv7/include/assembly.h new file mode 100644 index 00000000..0c5c57fb --- /dev/null +++ b/Kernel/arch/armv7/include/assembly.h @@ -0,0 +1,46 @@ +/* + * Acess2 ARMv7 + * - By John Hodge (thePowersGang) + * + * arch/arm7/include/assembly.h + * - Assembly specific macros + */ +#ifndef _ASSEMBLY_H_ +#define _ASSEMBLY_H_ + +#define PUSH_GPRS \ + str r0, [sp,#-1*4];\ + str r1, [sp,#-2*4];\ + str r2, [sp,#-3*4];\ + str r3, [sp,#-4*4];\ + str r4, [sp,#-5*4];\ + str r5, [sp,#-6*4];\ + str r6, [sp,#-7*4];\ + str r7, [sp,#-8*4];\ + str r8, [sp,#-9*4];\ + str r9, [sp,#-10*4];\ + str r10, [sp,#-11*4];\ + str r11, [sp,#-12*4];\ + str r12, [sp,#-13*4];\ + str sp, [sp,#-14*4];\ + str lr, [sp,#-15*4];\ + sub sp, #16*4 + +#define POP_GPRS add sp, #16*4; \ + ldr r0, [sp,#-1*4]; \ + ldr r1, [sp,#-2*4]; \ + ldr r2, [sp,#-3*4]; \ + ldr r3, [sp,#-4*4]; \ + ldr r4, [sp,#-5*4]; \ + ldr r5, [sp,#-6*4]; \ + ldr r6, [sp,#-7*4]; \ + ldr r7, [sp,#-8*4]; \ + ldr r8, [sp,#-9*4]; \ + ldr r9, [sp,#-10*4]; \ + ldr r10, [sp,#-11*4]; \ + ldr r11, [sp,#-12*4]; \ + ldr r12, [sp,#-13*4]; \ + ldr lr, [sp,#-15*4]; + +#endif + diff --git a/Kernel/arch/armv7/main.c b/Kernel/arch/armv7/main.c index 37eda682..de28ee87 100644 --- a/Kernel/arch/armv7/main.c +++ b/Kernel/arch/armv7/main.c @@ -5,6 +5,7 @@ * arch/arm7/main.c */ #include +#include // === IMPORTS === extern void Interrupts_Setup(void); @@ -35,6 +36,7 @@ int kmain(void) VFS_Init(); // Boot modules? + Module_EnsureLoaded("armv7_GIC"); // LogF("Moving to arch-independent init\n"); diff --git a/Kernel/arch/armv7/mm_virt.c b/Kernel/arch/armv7/mm_virt.c index 1090b0ec..b6d022df 100644 --- a/Kernel/arch/armv7/mm_virt.c +++ b/Kernel/arch/armv7/mm_virt.c @@ -479,7 +479,8 @@ tVAddr MM_NewKStack(int bShared) // 1 guard page for( ofs = PAGE_SIZE; ofs < MM_KSTACK_SIZE; ofs += PAGE_SIZE ) { - if( MM_Allocate(addr + ofs) == 0 ) { + if( MM_Allocate(addr + ofs) == 0 ) + { while(ofs) { ofs -= PAGE_SIZE; diff --git a/Kernel/arch/armv7/proc.S b/Kernel/arch/armv7/proc.S index 841764b6..832adbd1 100644 --- a/Kernel/arch/armv7/proc.S +++ b/Kernel/arch/armv7/proc.S @@ -6,39 +6,7 @@ * - Process management assembly */ -#define PUSH_GPRS \ - str r0, [sp,#-1*4];\ - str r1, [sp,#-2*4];\ - str r2, [sp,#-3*4];\ - str r3, [sp,#-4*4];\ - str r4, [sp,#-5*4];\ - str r5, [sp,#-6*4];\ - str r6, [sp,#-7*4];\ - str r7, [sp,#-8*4];\ - str r8, [sp,#-9*4];\ - str r9, [sp,#-10*4];\ - str r10, [sp,#-11*4];\ - str r11, [sp,#-12*4];\ - str r12, [sp,#-13*4];\ - str sp, [sp,#-14*4];\ - str lr, [sp,#-15*4];\ - sub sp, #16*4 - -#define POP_GPRS add sp, #16*4; \ - ldr r0, [sp,#-1*4]; \ - ldr r1, [sp,#-2*4]; \ - ldr r2, [sp,#-3*4]; \ - ldr r3, [sp,#-4*4]; \ - ldr r4, [sp,#-5*4]; \ - ldr r5, [sp,#-6*4]; \ - ldr r6, [sp,#-7*4]; \ - ldr r7, [sp,#-8*4]; \ - ldr r8, [sp,#-9*4]; \ - ldr r9, [sp,#-10*4]; \ - ldr r10, [sp,#-11*4]; \ - ldr r11, [sp,#-12*4]; \ - ldr r12, [sp,#-13*4]; \ - ldr lr, [sp,#-15*4]; +#include "include/assembly.h" .globl KernelThreadHeader @ SP+12: Argument 1 @@ -54,15 +22,14 @@ KernelThreadHeader: @ Get arguments sub r5, #1 ldrhs r0, [sp],#4 - sub r5, #1 - ldrhs r1, [sp],#4 - sub r5, #1 - ldrhs r2, [sp],#4 - sub r5, #1 - ldrhs r3, [sp],#4 +@ suble r5, #1 +@ ldrhs r1, [sp],#4 +@ suble r5, #1 +@ ldrhs r2, [sp],#4 +@ suble r5, #1 +@ ldrhs r3, [sp],#4 - mov lr, pc - mov pc, r4 + blx r4 ldr r0, =0 bl Threads_Exit @@ -76,18 +43,22 @@ KernelThreadHeader: @ SP+0: New address space SwitchTask: PUSH_GPRS - + + @ Save IP ldr r4, =.return str r4, [r3] + @ Save SP str sp, [r1] - mov r0, sp - + @ Only update TTBR0 if the task has an explicit address space - ldr r0, [sp,#0x40] - tst r0, r0 - mcrne p15, 0, r0, c2, c0, 0 @ Set TTBR0 to r0 + ldr r1, [sp,#0x40] + tst r1, r1 + mcrne p15, 0, r1, c2, c0, 0 @ Set TTBR0 to r0 + + @ Restore SP + mov sp, r0 - mov pc, r2 + bx r2 .return: POP_GPRS diff --git a/Kernel/arch/armv7/proc.c b/Kernel/arch/armv7/proc.c index 7dfbb456..318cabda 100644 --- a/Kernel/arch/armv7/proc.c +++ b/Kernel/arch/armv7/proc.c @@ -30,6 +30,8 @@ void ArchThreads_Init(void) void Proc_IdleThread(void *unused) { + Threads_SetPriority(gpIdleThread, -1); + Threads_SetName("Idle Thread"); for(;;) { __asm__ __volatile__ ("wfi"); Proc_Reschedule(); @@ -65,7 +67,32 @@ void Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char ** tTID Proc_SpawnWorker( void (*Fnc)(void*), void *Ptr ) { - return -1; + tThread *new; + Uint32 sp; + + new = Threads_CloneTCB(NULL, 0); + if(!new) return -1; + + new->KernelStack = MM_NewKStack(1); + if(!new->KernelStack) { + // TODO: Delete thread + Log_Error("Proc", "Unable to allocate kernel stack"); + return -1; + } + + sp = new->KernelStack; + + *(Uint32*)(sp -= 4) = (Uint)Ptr; + *(Uint32*)(sp -= 4) = 1; + *(Uint32*)(sp -= 4) = (Uint)Fnc; + *(Uint32*)(sp -= 4) = (Uint)new; + + new->SavedState.SP = sp; + new->SavedState.IP = (Uint)KernelThreadHeader; + + Threads_AddActive(new); + + return new->TID; } tTID Proc_NewKThread( void (*Fnc)(void*), void *Ptr ) @@ -76,6 +103,7 @@ tTID Proc_NewKThread( void (*Fnc)(void*), void *Ptr ) new = Threads_CloneTCB(NULL, 0); if(!new) return -1; + // TODO: Non-shared stack new->KernelStack = MM_NewKStack(1); if(!new->KernelStack) { // TODO: Delete thread @@ -85,10 +113,10 @@ tTID Proc_NewKThread( void (*Fnc)(void*), void *Ptr ) sp = new->KernelStack; - *(Uint32*)(sp -= 4) = (Uint)new; - *(Uint32*)(sp -= 4) = (Uint)Fnc; - *(Uint32*)(sp -= 4) = 1; *(Uint32*)(sp -= 4) = (Uint)Ptr; + *(Uint32*)(sp -= 4) = 1; + *(Uint32*)(sp -= 4) = (Uint)Fnc; + *(Uint32*)(sp -= 4) = (Uint)new; new->SavedState.SP = sp; new->SavedState.IP = (Uint)KernelThreadHeader; @@ -113,7 +141,8 @@ void Proc_Reschedule(void) if(!next) next = gpIdleThread; if(!next || next == cur) return; - Log("Switching to %p (%i) IP=%p SP=%p", next, next->TID, next->SavedState.IP, next->SavedState.SP); + Log("Switching to %p (%i %s) IP=%p SP=%p", next, next->TID, next->ThreadName, next->SavedState.IP, next->SavedState.SP); + Log("Requested by %p", __builtin_return_address(0)); gpCurrentThread = next; // TODO: Change kernel stack? diff --git a/Kernel/arch/armv7/start.S b/Kernel/arch/armv7/start.S index e1bbe0c9..e2f2d2c2 100644 --- a/Kernel/arch/armv7/start.S +++ b/Kernel/arch/armv7/start.S @@ -1,3 +1,6 @@ + +#include "include/assembly.h" + KERNEL_BASE = 0x80000000 PCI_PADDR = 0x60000000 @ Realview UART0_PADDR = 0x10009000 @ Realview @@ -6,14 +9,14 @@ UART0_PADDR = 0x10009000 @ Realview @ .section .init interrupt_vector_table: - b _start @ Reset - b . @ #UD - b SyscallHandler @ SVC (SWI assume) - b . @ Prefetch abort - b . @ Data abort - b . @ Not Used - b IRQHandler @ IRQ - b . @ FIQ (Fast interrupt) +ivt_reset: b _start @ Reset +ivt_undef: b . @ #UD +ivt_svc: b SyscallHandler @ SVC (SWI assume) +ivt_prefetch: b DataAbort @ Prefetch abort +ivt_data: b DataAbort @ Data abort +ivt_unused: b . @ Not Used +ivt_irq: b IRQHandler @ IRQ +ivt_fiq: b . @ FIQ (Fast interrupt) .globl _start _start: @@ -32,6 +35,9 @@ _start: orr r0, r0, #1 << 23 mcr p15, 0, r0, c1, c0, 0 + @ Prepare for interrupts + cps #19 + ldr sp, =stack+0x10000 @ Set up stack ldr r0, =kmain mov pc, r0 @@ -44,9 +50,60 @@ _ptr_kmain: SyscallHandler: b . +.globl gpIRQHandler +gpIRQHandler: .long 0 +IRQ_saved_sp: .long 0 +IRQ_saved_lr: .long 0 +.globl IRQHandler IRQHandler: + sub lr, #4 @ Adjust LR to the correct value + srsdb sp!, #19 @ Switch to supervisor mode (DDI0406B D1.6.5) (actually SRSFD) + cpsie i, #19 + + PUSH_GPRS + + ldr r0, =csIRQ_Tag + ldr r1, =csIRQ_Fmt + ldr r4, =Log_Debug + blx r4 + + @ Call the registered handler + ldr r0, gpIRQHandler + blx r0 + + @ Restore CPU state + POP_GPRS + rfeia sp! @ Pop state (actually RFEFD) + bx lr + +.globl DataAbort +DataAbort: + sub lr, #8 @ Adjust LR to the correct value + srsdb sp!, #19 @ Switch to supervisor mode (DDI0406B D1.6.5) (actually SRSFD) + cpsie i, #19 + PUSH_GPRS + + mov r2, lr + ldr r1, =csDataAbort_Fmt + ldr r0, =csDataAbort_Tag + ldr r4, =Log_Error + blx r4 b . + POP_GPRS + rfeia sp! @ Pop state (actually RFEFD) + bx lr + +csIRQ_Tag: +csDataAbort_Tag: + .asciz "ARMv7" +csIRQ_Fmt: + .asciz "IRQ" +csDataAbort_Fmt: + .asciz "Data Abort at %p" + +.comm irqstack, 0x1000 + .section .padata .globl kernel_table0 -- 2.20.1