1.1 --- a/stage2/cpu.c Fri Feb 26 00:23:58 2016 +0100
1.2 +++ b/stage2/cpu.c Fri Feb 26 00:44:01 2016 +0100
1.3 @@ -112,6 +112,55 @@
1.4 "nop\n");
1.5 }
1.6
1.7 +void set_task(u8 asid)
1.8 +{
1.9 + asm volatile(
1.10 +
1.11 + /* Set the ASID. */
1.12 +
1.13 + "mtc0 %0, $10\n" /* CP0_ENTRYHI */
1.14 + "nop"
1.15 + :
1.16 + : "r" (asid)
1.17 + );
1.18 +}
1.19 +
1.20 +void init_stack(u32 top, u32 got, void (*function)(), u32 args[], u8 nargs)
1.21 +{
1.22 + u8 i;
1.23 +
1.24 + asm volatile(
1.25 + "move $t3, %0\n" /* refer to the stack frame */
1.26 + "addi $t3, $t3, -16\n" /* refer to the first parameter */
1.27 + :
1.28 + : "r" (top)
1.29 + );
1.30 +
1.31 + /* Provide arguments to the function. */
1.32 +
1.33 + for (i = 0; i < nargs; i++)
1.34 + {
1.35 + asm volatile(
1.36 + "sw %0, 0($t3)\n"
1.37 + "addi $t3, $t3, -4\n"
1.38 + :
1.39 + : "r" (args[i])
1.40 + );
1.41 + }
1.42 +
1.43 + /* Store essential data for the function environment. */
1.44 +
1.45 + asm volatile(
1.46 + "subu %1, %1, 0x80000000\n" /* obtain user mode addresses */
1.47 + "subu %2, %2, 0x80000000\n"
1.48 + "sw %2, -100(%0)\n" /* store the function address */
1.49 + "sw %1, -104(%0)\n" /* store the global pointer */
1.50 + "sw %2, -112(%0)\n" /* store the function address */
1.51 + :
1.52 + : "r" (top), "r" (got), "r" (function)
1.53 + );
1.54 +}
1.55 +
1.56 void enter_user_mode(void)
1.57 {
1.58 asm volatile(
2.1 --- a/stage2/cpu.h Fri Feb 26 00:23:58 2016 +0100
2.2 +++ b/stage2/cpu.h Fri Feb 26 00:44:01 2016 +0100
2.3 @@ -6,6 +6,8 @@
2.4 void flush_cache_all(void);
2.5 void handle_error_level(void);
2.6 void enter_user_mode(void);
2.7 +void set_task(u8);
2.8 +void init_stack(u32, u32, void (*)(), u32[], u8);
2.9 void enable_interrupts(void);
2.10 void init_interrupts(void);
2.11 void init_tlb(void);
3.1 --- a/stage2/handlers.S Fri Feb 26 00:23:58 2016 +0100
3.2 +++ b/stage2/handlers.S Fri Feb 26 00:44:01 2016 +0100
3.3 @@ -21,6 +21,8 @@
3.4 .text
3.5 .extern tlb_handle
3.6 .extern irq_handle
3.7 +.extern current_stack_pointer
3.8 +.extern current_task
3.9 .globl tlb_handler
3.10 .globl interrupt_handler
3.11 .set noreorder
3.12 @@ -46,11 +48,28 @@
3.13 jal save_state
3.14 nop
3.15
3.16 + /* Record the stack pointer. */
3.17 +
3.18 + la $k0, current_stack_pointer
3.19 + sw $sp, 0($k0)
3.20 +
3.21 /* Invoke the handler. */
3.22
3.23 jal irq_handle
3.24 nop
3.25
3.26 + /* Switch the stack pointer. */
3.27 +
3.28 + la $k0, current_stack_pointer
3.29 + lw $sp, 0($k0)
3.30 +
3.31 + /* Set the current task ASID. */
3.32 +
3.33 + la $k0, current_task
3.34 + lw $k1, 0($k0)
3.35 + mtc0 $k1, $10 /* CP0_ENTRYHI */
3.36 + nop
3.37 +
3.38 j load_and_return
3.39 nop
3.40
4.1 --- a/stage2/irq.c Fri Feb 26 00:23:58 2016 +0100
4.2 +++ b/stage2/irq.c Fri Feb 26 00:44:01 2016 +0100
4.3 @@ -45,6 +45,12 @@
4.4
4.5 /* Task management. */
4.6
4.7 +enum { max_tasks = 3 };
4.8 +static u32 stack_pointers[max_tasks];
4.9 +u8 current_task = 0;
4.10 +u32 current_stack_pointer;
4.11 +extern u32 _got_copy_start;
4.12 +
4.13 const u32 stack_start = 0x00080000;
4.14 const u32 stack_size = 0x00002000;
4.15 const u32 pagesize = 4 * 1024;
4.16 @@ -81,9 +87,9 @@
4.17
4.18 if (REG_INTC_IPR & (1 << TIMER_CHAN_IRQ)) {
4.19
4.20 - /* Update the pixel type. */
4.21 + /* Switch task. */
4.22
4.23 - /* pixel_type = __gpio_get_pin(GPIO_POWER); */
4.24 + /* switch_task(); */
4.25
4.26 /* Clear interrupt status. */
4.27
4.28 @@ -100,12 +106,6 @@
4.29 }
4.30 }
4.31
4.32 -void start_task()
4.33 -{
4.34 - /* enter_user_mode(); */
4.35 - plot_pattern(1, 0, 0);
4.36 -}
4.37 -
4.38 void tlb_handle()
4.39 {
4.40 u32 asid, virtual, physical, bottom, top;
4.41 @@ -137,6 +137,51 @@
4.42 Request a physical region mapping two 4KB pages.
4.43 Pages employ C=3, dirty, valid, with the task number as the ASID.
4.44 */
4.45 -
4.46 +
4.47 map_page(virtual, physical, pagesize, 0x1e, asid);
4.48 +}
4.49 +
4.50 +void start_task(unsigned short task)
4.51 +{
4.52 + u32 args[] = {1, 0, (task - 1) * 120};
4.53 +
4.54 + /*
4.55 + Each task employs a stack at a multiple of the given start address in
4.56 + physical memory, but at the same address in virtual memory.
4.57 + */
4.58 +
4.59 + map_page(stack_start + stack_size * task - stack_size, stack_start + stack_size * task - stack_size, pagesize, 0x1e, 0);
4.60 +
4.61 + /*
4.62 + Set the stack for the new task, initialising the global pointer and
4.63 + return address.
4.64 + */
4.65 +
4.66 + init_stack(stack_start + stack_size * task, _got_copy_start, (void (*)()) plot_pattern, args, 3);
4.67 +
4.68 + /* Advance the stack pointer so that the adopted frame will be found. */
4.69 +
4.70 + stack_pointers[task] = stack_start - framesize;
4.71 }
4.72 +
4.73 +void switch_task()
4.74 +{
4.75 + /*
4.76 + Get the current stack pointer. This is obtained just after saving the
4.77 + task's state.
4.78 + */
4.79 +
4.80 + stack_pointers[current_task] = current_stack_pointer;
4.81 +
4.82 + /* Switch the current task. */
4.83 +
4.84 + current_task++;
4.85 + if (current_task == max_tasks) current_task = 1;
4.86 +
4.87 + /*
4.88 + Set the current stack pointer. This is actually set just before
4.89 + restoring the task's state.
4.90 + */
4.91 +
4.92 + current_stack_pointer = stack_pointers[current_task];
4.93 +}
5.1 --- a/stage2/irq.h Fri Feb 26 00:23:58 2016 +0100
5.2 +++ b/stage2/irq.h Fri Feb 26 00:44:01 2016 +0100
5.3 @@ -4,6 +4,9 @@
5.4 /* Initialisation functions. */
5.5
5.6 void irq_init(void);
5.7 -void start_task(void);
5.8 +void start_task(unsigned short);
5.9 +void switch_task(void);
5.10 +void invoke_task(unsigned short);
5.11 +void plot_pattern(unsigned short, unsigned short, unsigned short);
5.12
5.13 #endif /* __IRQ_H__ */
6.1 --- a/stage2/stage2.c Fri Feb 26 00:23:58 2016 +0100
6.2 +++ b/stage2/stage2.c Fri Feb 26 00:44:01 2016 +0100
6.3 @@ -45,9 +45,17 @@
6.4
6.5 lcd_init();
6.6 handle_error_level();
6.7 - irq_init();
6.8 + set_task(0);
6.9 +
6.10 + /* Start the tasks. */
6.11
6.12 - start_task();
6.13 + start_task(1);
6.14 + start_task(2);
6.15 +
6.16 + /* Now, wait for the tasks to be selected as interrupts occur. */
6.17 +
6.18 + irq_init();
6.19 + plot_pattern(1, 0, 0);
6.20
6.21 while (1) asm volatile("wait");
6.22 }