# HG changeset patch
# User Paul Boddie <paul@boddie.org.uk>
# Date 1456443841 -3600
# Node ID 7981e27344ab289dccbdb6819d25fdef4500b2a1
# Parent  033671d0e4627b195dd2b0f4d0d3aab2c0e36501
Added elements of a task switching mechanism.

diff -r 033671d0e462 -r 7981e27344ab stage2/cpu.c
--- a/stage2/cpu.c	Fri Feb 26 00:23:58 2016 +0100
+++ b/stage2/cpu.c	Fri Feb 26 00:44:01 2016 +0100
@@ -112,6 +112,55 @@
 		"nop\n");
 }
 
+void set_task(u8 asid)
+{
+	asm volatile(
+
+		/* Set the ASID. */
+
+		"mtc0 %0, $10\n"	/* CP0_ENTRYHI */
+		"nop"
+		:
+		: "r" (asid)
+		);
+}
+
+void init_stack(u32 top, u32 got, void (*function)(), u32 args[], u8 nargs)
+{
+	u8 i;
+
+	asm volatile(
+		"move $t3, %0\n"	/* refer to the stack frame */
+		"addi $t3, $t3, -16\n"	/* refer to the first parameter */
+		:
+		: "r" (top)
+		);
+
+	/* Provide arguments to the function. */
+
+	for (i = 0; i < nargs; i++)
+	{
+		asm volatile(
+			"sw %0, 0($t3)\n"
+			"addi $t3, $t3, -4\n"
+			:
+			: "r" (args[i])
+			);
+	}
+
+	/* Store essential data for the function environment. */
+
+	asm volatile(
+		"subu %1, %1, 0x80000000\n"	/* obtain user mode addresses */
+		"subu %2, %2, 0x80000000\n"
+		"sw %2, -100(%0)\n"		/* store the function address */
+		"sw %1, -104(%0)\n"		/* store the global pointer */
+		"sw %2, -112(%0)\n"		/* store the function address */
+		:
+		: "r" (top), "r" (got), "r" (function)
+		);
+}
+
 void enter_user_mode(void)
 {
 	asm volatile(
diff -r 033671d0e462 -r 7981e27344ab stage2/cpu.h
--- a/stage2/cpu.h	Fri Feb 26 00:23:58 2016 +0100
+++ b/stage2/cpu.h	Fri Feb 26 00:44:01 2016 +0100
@@ -6,6 +6,8 @@
 void flush_cache_all(void);
 void handle_error_level(void);
 void enter_user_mode(void);
+void set_task(u8);
+void init_stack(u32, u32, void (*)(), u32[], u8);
 void enable_interrupts(void);
 void init_interrupts(void);
 void init_tlb(void);
diff -r 033671d0e462 -r 7981e27344ab stage2/handlers.S
--- a/stage2/handlers.S	Fri Feb 26 00:23:58 2016 +0100
+++ b/stage2/handlers.S	Fri Feb 26 00:44:01 2016 +0100
@@ -21,6 +21,8 @@
 .text
 .extern tlb_handle
 .extern irq_handle
+.extern current_stack_pointer
+.extern current_task
 .globl tlb_handler
 .globl interrupt_handler
 .set noreorder
@@ -46,11 +48,28 @@
 	jal save_state
 	nop
 
+	/* Record the stack pointer. */
+
+	la $k0, current_stack_pointer
+	sw $sp, 0($k0)
+
 	/* Invoke the handler. */
 
 	jal irq_handle
 	nop
 
+	/* Switch the stack pointer. */
+
+	la $k0, current_stack_pointer
+	lw $sp, 0($k0)
+
+	/* Set the current task ASID. */
+
+	la $k0, current_task
+	lw $k1, 0($k0)
+	mtc0 $k1, $10 /* CP0_ENTRYHI */
+	nop
+
 	j load_and_return
 	nop
 
diff -r 033671d0e462 -r 7981e27344ab stage2/irq.c
--- a/stage2/irq.c	Fri Feb 26 00:23:58 2016 +0100
+++ b/stage2/irq.c	Fri Feb 26 00:44:01 2016 +0100
@@ -45,6 +45,12 @@
 
 /* Task management. */
 
+enum { max_tasks = 3 };
+static u32 stack_pointers[max_tasks];
+u8 current_task = 0;
+u32 current_stack_pointer;
+extern u32 _got_copy_start;
+
 const u32 stack_start = 0x00080000;
 const u32 stack_size = 0x00002000;
 const u32 pagesize = 4 * 1024;
@@ -81,9 +87,9 @@
 
 	if (REG_INTC_IPR & (1 << TIMER_CHAN_IRQ)) {
 
-		/* Update the pixel type. */
+		/* Switch task. */
 
-		/* pixel_type = __gpio_get_pin(GPIO_POWER); */
+		/* switch_task(); */
 
 		/* Clear interrupt status. */
 
@@ -100,12 +106,6 @@
 	}
 }
 
-void start_task()
-{
-	/* enter_user_mode(); */
-	plot_pattern(1, 0, 0);
-}
-
 void tlb_handle()
 {
 	u32 asid, virtual, physical, bottom, top;
@@ -137,6 +137,51 @@
 	Request a physical region mapping two 4KB pages.
 	Pages employ C=3, dirty, valid, with the task number as the ASID.
 	*/
-
+        
 	map_page(virtual, physical, pagesize, 0x1e, asid);
+}       
+
+void start_task(unsigned short task)
+{
+	u32 args[] = {1, 0, (task - 1) * 120};
+
+	/*
+	Each task employs a stack at a multiple of the given start address in
+	physical memory, but at the same address in virtual memory.
+	*/
+
+	map_page(stack_start + stack_size * task - stack_size, stack_start + stack_size * task - stack_size, pagesize, 0x1e, 0);
+
+	/*
+	Set the stack for the new task, initialising the global pointer and
+	return address.
+	*/
+
+	init_stack(stack_start + stack_size * task, _got_copy_start, (void (*)()) plot_pattern, args, 3);
+
+	/* Advance the stack pointer so that the adopted frame will be found. */
+
+	stack_pointers[task] = stack_start - framesize;
 }
+
+void switch_task()
+{
+	/*
+	Get the current stack pointer. This is obtained just after saving the
+	task's state.
+	*/
+
+	stack_pointers[current_task] = current_stack_pointer;
+
+	/* Switch the current task. */
+
+	current_task++;
+	if (current_task == max_tasks) current_task = 1;
+
+	/*
+	Set the current stack pointer. This is actually set just before
+	restoring the task's state.
+	*/
+
+	current_stack_pointer = stack_pointers[current_task];
+}
diff -r 033671d0e462 -r 7981e27344ab stage2/irq.h
--- a/stage2/irq.h	Fri Feb 26 00:23:58 2016 +0100
+++ b/stage2/irq.h	Fri Feb 26 00:44:01 2016 +0100
@@ -4,6 +4,9 @@
 /* Initialisation functions. */
 
 void irq_init(void);
-void start_task(void);
+void start_task(unsigned short);
+void switch_task(void);
+void invoke_task(unsigned short);
+void plot_pattern(unsigned short, unsigned short, unsigned short);
 
 #endif /* __IRQ_H__ */
diff -r 033671d0e462 -r 7981e27344ab stage2/stage2.c
--- a/stage2/stage2.c	Fri Feb 26 00:23:58 2016 +0100
+++ b/stage2/stage2.c	Fri Feb 26 00:44:01 2016 +0100
@@ -45,9 +45,17 @@
 
 	lcd_init();
 	handle_error_level();
-	irq_init();
+	set_task(0);
+
+	/* Start the tasks. */
 
-	start_task();
+	start_task(1);
+	start_task(2);
+
+	/* Now, wait for the tasks to be selected as interrupts occur. */
+
+	irq_init();
+	plot_pattern(1, 0, 0);
 
 	while (1) asm volatile("wait");
 }