Kernel/arm7 - Working on threading

diff --git a/Kernel/arch/arm7/proc.S b/Kernel/arch/arm7/proc.S
index 7f9b88e..841764b 100644 (file)
--- a/Kernel/arch/arm7/proc.S
+++ b/Kernel/arch/arm7/proc.S
@@ -7,52 +7,89 @@
  */
 
 #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];\
+       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];
-
-.globl SwitchStacks
+       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];
+
+.globl KernelThreadHeader
+@ SP+12: Argument 1
+@ SP+8: Argument Count
+@ SP+4: Function
+@ SP+0: Thread Pointer
+KernelThreadHeader:
+       ldr r0, [sp],#4
+       @ TODO: Do something with the thread pointer
+       
+       ldr r4, [sp],#4 @ Function
+       ldr r5, [sp],#4
+       @ 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
+
+       mov lr, pc
+       mov pc, r4
+       
+       ldr r0, =0
+       bl Threads_Exit
+       b .
+
+.globl SwitchTask
 @ R0: New stack
 @ R1: Pointer to where to save old stack
-@ R2: New address space
-SwitchStacks:
+@ R2: New IP
+@ R3: Pointer to save old IP
+@ SP+0: New address space
+SwitchTask:
        PUSH_GPRS
        
+       ldr r4, =.return
+       str r4, [r3]
        str sp, [r1]
        mov r0, sp
        
        @ Only update TTBR0 if the task has an explicit address space
-       tst r2, r2
-       mcrne p15, 0, r2, c2, c0, 0     @ Set TTBR0 to r2
+       ldr r0, [sp,#0x40]
+       tst r0, r0
+       mcrne p15, 0, r0, c2, c0, 0     @ Set TTBR0 to r0
+
+       mov pc, r2
 
+.return:
        POP_GPRS
        bx lr
+

diff --git a/Kernel/arch/arm7/proc.c b/Kernel/arch/arm7/proc.c
index 28b0af0..30279cc 100644 (file)
--- a/Kernel/arch/arm7/proc.c
+++ b/Kernel/arch/arm7/proc.c
@@ -11,19 +11,33 @@
 
 // === IMPORTS ===
 extern tThread gThreadZero;
+extern void    SwitchTask(Uint32 NewSP, Uint32 *OldSP, Uint32 NewIP, Uint32 *OldIP, Uint32 MemPtr);
 
 // === PROTOTYPES ===
+void   Proc_IdleThread(void *unused);
+tTID   Proc_NewKThread( void (*Fnc)(void*), void *Ptr );
 
 // === GLOBALS ===
 tThread        *gpCurrentThread = &gThreadZero;
+tThread *gpIdleThread = NULL;
 
 // === CODE ===
 void ArchThreads_Init(void)
 {
 }
 
+void Proc_IdleThread(void *unused)
+{
+       for(;;)
+               Proc_Reschedule();
+}
+
 void Proc_Start(void)
 {
+       tTID    tid;
+
+       tid = Proc_NewKThread( Proc_IdleThread, NULL );
+       gpIdleThread = Threads_GetThread(tid);
 }
 
 int GetCPUNum(void)
@@ -47,7 +61,13 @@ void Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **
 
 tTID Proc_SpawnWorker( void (*Fnc)(void*), void *Ptr )
 {
-       return 0;
+       return -1;
+}
+
+tTID Proc_NewKThread( void (*Fnc)(void*), void *Ptr )
+{
+       // TODO: Implement
+       return -1;
 }
 
 void Proc_CallFaultHandler(tThread *Thread)
@@ -57,7 +77,20 @@ void Proc_CallFaultHandler(tThread *Thread)
 
 void Proc_Reschedule(void)
 {
-       // TODO: Task switching!
+       tThread *cur, *next;
+
+       cur = gpCurrentThread;
+
+       next = Threads_GetNextToRun(0, cur);
+       if(!next)       next = gpIdleThread;
+       if(!next || next == cur)        return;
+       
+       SwitchTask(
+               next->SavedState.SP, &cur->SavedState.SP,
+               next->SavedState.IP, &cur->SavedState.IP,
+               next->MemState.Base
+               );
+       
 }
 
 void Proc_DumpThreadCPUState(tThread *Thread)