From 2bc4a1c1fc602a92ace9fbeea5664c6977f8ae5b Mon Sep 17 00:00:00 2001
From: John Hodge <tpg@mutabah.net>
Date: Mon, 26 Sep 2011 22:13:58 +0800
Subject: [PATCH] Kernel/arm7 - Working on threading

---
 Kernel/arch/arm7/proc.S | 107 +++++++++++++++++++++++++++-------------
 Kernel/arch/arm7/proc.c |  37 +++++++++++++-
 2 files changed, 107 insertions(+), 37 deletions(-)

diff --git a/Kernel/arch/arm7/proc.S b/Kernel/arch/arm7/proc.S
index 7f9b88ef..841764b6 100644
--- 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 28b0af03..30279cc1 100644
--- 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)
-- 
2.20.1