1.1 --- a/stage2/cpu.c Sat Feb 27 23:41:17 2016 +0100
1.2 +++ b/stage2/cpu.c Sun Feb 28 01:18:09 2016 +0100
1.3 @@ -125,41 +125,22 @@
1.4 );
1.5 }
1.6
1.7 -void init_stack(u32 top, u32 got, void (*function)(), u32 args[], u8 nargs)
1.8 +void init_registers(u32 *base, u32 got, void (*function)(), u32 args[], u8 nargs)
1.9 {
1.10 u8 i;
1.11
1.12 - asm volatile(
1.13 - "move $t3, %0\n" /* refer to the stack frame */
1.14 - "addi $t3, $t3, -16\n" /* refer to the first parameter */
1.15 - :
1.16 - : "r" (top)
1.17 - );
1.18 -
1.19 /* Provide arguments to the function. */
1.20
1.21 for (i = 0; i < nargs; i++)
1.22 {
1.23 - asm volatile(
1.24 - "sw %0, 0($t3)\n"
1.25 - "addi $t3, $t3, -4\n"
1.26 - :
1.27 - : "r" (args[i])
1.28 - );
1.29 + base[i+4] = args[i];
1.30 }
1.31
1.32 /* Store essential data for the function environment. */
1.33
1.34 - asm volatile(
1.35 - "subu %1, %1, 0x80000000\n" /* obtain user mode addresses */
1.36 - "subu %2, %2, 0x80000000\n"
1.37 - "sw %2, -100(%0)\n" /* store the function address as t9 */
1.38 - "sw %1, -104(%0)\n" /* store the global pointer */
1.39 - "sw %2, -112(%0)\n" /* store the function address as ra (for invoke_task) */
1.40 - "sw %2, -116(%0)\n" /* store the function address as EPC (for the handler) */
1.41 - :
1.42 - : "r" (top), "r" (got), "r" (function)
1.43 - );
1.44 + base[25] = (u32) function - 0x80000000; /* store the function address as t9 */
1.45 + base[26] = got - 0x80000000; /* store the global pointer */
1.46 + base[29] = (u32) function - 0x80000000; /* store the function address as EPC (for the handler) */
1.47 }
1.48
1.49 void enter_user_mode(void)
1.50 @@ -174,14 +155,18 @@
1.51
1.52 void init_tlb(void)
1.53 {
1.54 - /* Wire in the kseg0 mapping and the page tables. */
1.55 + unsigned short first_random = 0;
1.56
1.57 asm volatile(
1.58 - "li $t1, 1\n" /* index of first randomly-replaced entry */
1.59 - "mtc0 $t1, $6\n" /* CP0_WIRED */
1.60 "mtc0 $zero, $4\n" /* CP0_CONTEXT */
1.61 "mtc0 $zero, $10\n" /* CP0_ENTRYHI */
1.62 - "nop\n");
1.63 + "mtc0 $zero, $2\n" /* CP0_ENTRYLO0 */
1.64 + "mtc0 $zero, $3\n" /* CP0_ENTRYLO1 */
1.65 + "mtc0 %0, $6\n" /* CP0_WIRED */
1.66 + "nop"
1.67 + :
1.68 + : "r" (first_random)
1.69 + );
1.70 }
1.71
1.72 void map_page_index(u32 virtual, u32 physical, u32 pagesize, u8 flags, u8 asid, u32 index)
2.1 --- a/stage2/cpu.h Sat Feb 27 23:41:17 2016 +0100
2.2 +++ b/stage2/cpu.h Sun Feb 28 01:18:09 2016 +0100
2.3 @@ -7,7 +7,7 @@
2.4 void handle_error_level(void);
2.5 void enter_user_mode(void);
2.6 void set_task(u8);
2.7 -void init_stack(u32, u32, void (*)(), u32[], u8);
2.8 +void init_registers(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/entry.S Sat Feb 27 23:41:17 2016 +0100
3.2 +++ b/stage2/entry.S Sun Feb 28 01:18:09 2016 +0100
3.3 @@ -19,6 +19,8 @@
3.4
3.5 .text
3.6 .extern interrupt_handler
3.7 +.extern current_registers
3.8 +.extern current_stack_pointer
3.9 .globl _tlb_entry
3.10 .globl _exc_entry
3.11 .globl _irq_entry
3.12 @@ -111,14 +113,40 @@
3.13 nop
3.14
3.15 _irq_entry:
3.16 + /* Set the ASID. */
3.17 +
3.18 + mtc0 $zero, $10 /* CP0_ENTRYHI */
3.19 + nop
3.20 +
3.21 + /* Obtain the kernel global offset table. */
3.22 +
3.23 + move $k0, $gp
3.24 + lui $gp, %hi(_GLOBAL_OFFSET_TABLE_)
3.25 + ori $gp, $gp, %lo(_GLOBAL_OFFSET_TABLE_)
3.26 +
3.27 + /* Obtain a store of registers for the current task. */
3.28 +
3.29 + la $k1, current_registers
3.30 + lw $k1, 0($k1)
3.31 +
3.32 /* Save registers that the assembler wants to trash. */
3.33
3.34 - sw $t9, -100($sp)
3.35 - sw $gp, -104($sp)
3.36 - sw $ra, -112($sp)
3.37 + sw $t9, 100($k1)
3.38 + sw $k0, 104($k1) /* old gp */
3.39 + sw $ra, 112($k1)
3.40 +
3.41 + /* Record the stack pointer. */
3.42
3.43 - lui $gp, %hi(_GLOBAL_OFFSET_TABLE_)
3.44 - ori $gp, $gp, %lo(_GLOBAL_OFFSET_TABLE_)
3.45 + la $k1, current_stack_pointer
3.46 + lw $k1, 0($k1) /* &stack_pointers[current_task] */
3.47 + sw $sp, 0($k1)
3.48 +
3.49 + /* Switch to the kernel stack. */
3.50 +
3.51 + li $sp, 0x80070000
3.52 +
3.53 + /* Invoke the rest of the interrupt handling process. */
3.54 +
3.55 la $k0, interrupt_handler
3.56 jr $k0
3.57 nop
4.1 --- a/stage2/handlers.S Sat Feb 27 23:41:17 2016 +0100
4.2 +++ b/stage2/handlers.S Sun Feb 28 01:18:09 2016 +0100
4.3 @@ -20,6 +20,7 @@
4.4
4.5 .text
4.6 .extern irq_handle
4.7 +.extern current_registers
4.8 .extern current_stack_pointer
4.9 .extern current_task
4.10 .globl interrupt_handler
4.11 @@ -32,11 +33,6 @@
4.12 jal save_state
4.13 nop
4.14
4.15 - /* Record the stack pointer. */
4.16 -
4.17 - la $k0, current_stack_pointer
4.18 - sw $sp, 0($k0)
4.19 -
4.20 /* Invoke the handler. */
4.21
4.22 jal irq_handle
4.23 @@ -45,7 +41,8 @@
4.24 /* Switch the stack pointer. */
4.25
4.26 la $k0, current_stack_pointer
4.27 - lw $sp, 0($k0)
4.28 + lw $k1, 0($k0) /* &stack_pointers[current_task] */
4.29 + lw $sp, 0($k1)
4.30
4.31 /* Set the current task ASID. */
4.32
4.33 @@ -54,81 +51,89 @@
4.34 mtc0 $k1, $10 /* CP0_ENTRYHI */
4.35 nop
4.36
4.37 + /* Obtain the current task's registers. */
4.38 +
4.39 j load_and_return
4.40 nop
4.41
4.42 save_state:
4.43 - sw $at, -4($sp)
4.44 - sw $v0, -8($sp)
4.45 - sw $v1, -12($sp)
4.46 - sw $a0, -16($sp)
4.47 - sw $a1, -20($sp)
4.48 - sw $a2, -24($sp)
4.49 - sw $a3, -28($sp)
4.50 - sw $t0, -32($sp)
4.51 - sw $t1, -36($sp)
4.52 - sw $t2, -40($sp)
4.53 - sw $t3, -44($sp)
4.54 - sw $t4, -48($sp)
4.55 - sw $t5, -52($sp)
4.56 - sw $t6, -56($sp)
4.57 - sw $t7, -60($sp)
4.58 - sw $s0, -64($sp)
4.59 - sw $s1, -68($sp)
4.60 - sw $s2, -72($sp)
4.61 - sw $s3, -76($sp)
4.62 - sw $s4, -80($sp)
4.63 - sw $s5, -84($sp)
4.64 - sw $s6, -88($sp)
4.65 - sw $s7, -92($sp)
4.66 - sw $t8, -96($sp)
4.67 - /* sw $t9, -100($sp) */
4.68 - /* sw $gp, -104($sp) */
4.69 - sw $fp, -108($sp)
4.70 - /* sw $ra, -112($sp) */
4.71 + /* Obtain a store of registers for the current task. */
4.72 +
4.73 + la $k0, current_registers
4.74 + lw $k1, 0($k0)
4.75 +
4.76 + sw $at, 4($k1)
4.77 + sw $v0, 8($k1)
4.78 + sw $v1, 12($k1)
4.79 + sw $a0, 16($k1)
4.80 + sw $a1, 20($k1)
4.81 + sw $a2, 24($k1)
4.82 + sw $a3, 28($k1)
4.83 + sw $t0, 32($k1)
4.84 + sw $t1, 36($k1)
4.85 + sw $t2, 40($k1)
4.86 + sw $t3, 44($k1)
4.87 + sw $t4, 48($k1)
4.88 + sw $t5, 52($k1)
4.89 + sw $t6, 56($k1)
4.90 + sw $t7, 60($k1)
4.91 + sw $s0, 64($k1)
4.92 + sw $s1, 68($k1)
4.93 + sw $s2, 72($k1)
4.94 + sw $s3, 76($k1)
4.95 + sw $s4, 80($k1)
4.96 + sw $s5, 84($k1)
4.97 + sw $s6, 88($k1)
4.98 + sw $s7, 92($k1)
4.99 + sw $t8, 96($k1)
4.100 +
4.101 + /* t9, gp and ra are already saved */
4.102 +
4.103 + sw $fp, 108($k1)
4.104
4.105 mfc0 $k0, $14 /* CP0_EPC */
4.106 nop
4.107 - sw $k0, -116($sp)
4.108 + sw $k0, 116($k1)
4.109
4.110 - addi $sp, $sp, -120
4.111 j $ra
4.112 nop
4.113
4.114 load_and_return:
4.115 + /* Obtain a store of registers for the current task. */
4.116
4.117 - addi $sp, $sp, 120
4.118 + la $k0, current_registers
4.119 + lw $k1, 0($k0)
4.120
4.121 - lw $at, -4($sp)
4.122 - lw $v0, -8($sp)
4.123 - lw $v1, -12($sp)
4.124 - lw $a0, -16($sp)
4.125 - lw $a1, -20($sp)
4.126 - lw $a2, -24($sp)
4.127 - lw $a3, -28($sp)
4.128 - lw $t0, -32($sp)
4.129 - lw $t1, -36($sp)
4.130 - lw $t2, -40($sp)
4.131 - lw $t3, -44($sp)
4.132 - lw $t4, -48($sp)
4.133 - lw $t5, -52($sp)
4.134 - lw $t6, -56($sp)
4.135 - lw $t7, -60($sp)
4.136 - lw $s0, -64($sp)
4.137 - lw $s1, -68($sp)
4.138 - lw $s2, -72($sp)
4.139 - lw $s3, -76($sp)
4.140 - lw $s4, -80($sp)
4.141 - lw $s5, -84($sp)
4.142 - lw $s6, -88($sp)
4.143 - lw $s7, -92($sp)
4.144 - lw $t8, -96($sp)
4.145 - lw $t9, -100($sp)
4.146 - lw $gp, -104($sp)
4.147 - lw $fp, -108($sp)
4.148 - lw $ra, -112($sp)
4.149 + lw $at, 4($k1)
4.150 + lw $v0, 8($k1)
4.151 + lw $v1, 12($k1)
4.152 + lw $a0, 16($k1)
4.153 + lw $a1, 20($k1)
4.154 + lw $a2, 24($k1)
4.155 + lw $a3, 28($k1)
4.156 + lw $t0, 32($k1)
4.157 + lw $t1, 36($k1)
4.158 + lw $t2, 40($k1)
4.159 + lw $t3, 44($k1)
4.160 + lw $t4, 48($k1)
4.161 + lw $t5, 52($k1)
4.162 + lw $t6, 56($k1)
4.163 + lw $t7, 60($k1)
4.164 + lw $s0, 64($k1)
4.165 + lw $s1, 68($k1)
4.166 + lw $s2, 72($k1)
4.167 + lw $s3, 76($k1)
4.168 + lw $s4, 80($k1)
4.169 + lw $s5, 84($k1)
4.170 + lw $s6, 88($k1)
4.171 + lw $s7, 92($k1)
4.172 + lw $t8, 96($k1)
4.173 + lw $t9, 100($k1)
4.174 + lw $gp, 104($k1)
4.175 + lw $fp, 108($k1)
4.176 + lw $ra, 112($k1)
4.177
4.178 - lw $k0, -116($sp)
4.179 + lw $k0, 116($k1)
4.180 mtc0 $k0, $14 /* CP0_EPC */
4.181 nop
4.182
5.1 --- a/stage2/irq.c Sat Feb 27 23:41:17 2016 +0100
5.2 +++ b/stage2/irq.c Sun Feb 28 01:18:09 2016 +0100
5.3 @@ -47,14 +47,17 @@
5.4
5.5 enum { max_tasks = 3 };
5.6 static u32 stack_pointers[max_tasks];
5.7 -u8 current_task = 0;
5.8 -u32 current_stack_pointer;
5.9 +static u32 registers[max_tasks][32];
5.10 +
5.11 +u8 current_task;
5.12 +u32 *current_stack_pointer;
5.13 +u32 *current_registers;
5.14 +
5.15 extern u32 _got_copy_start;
5.16
5.17 const u32 stack_start = 0x80080000;
5.18 const u32 stack_size = 0x00002000;
5.19 const u32 pagesize = 4 * 1024;
5.20 -const u32 framesize = 120; /* see the handlers */
5.21
5.22 /* Tasks. */
5.23
5.24 @@ -106,6 +109,13 @@
5.25 }
5.26 }
5.27
5.28 +void init_tasks()
5.29 +{
5.30 + current_task = 0;
5.31 + current_stack_pointer = &stack_pointers[current_task];
5.32 + current_registers = registers[current_task];
5.33 +}
5.34 +
5.35 void start_task(unsigned short task)
5.36 {
5.37 u32 args[] = {task, 0, (task - 1) * 60};
5.38 @@ -116,89 +126,30 @@
5.39 physical memory, but at the same address in virtual memory.
5.40 */
5.41
5.42 - physical = stack_start + stack_size * task;
5.43 + physical = stack_start - stack_size * task;
5.44 virtual = physical;
5.45
5.46 init_page_table(page_table_start, virtual - pagesize * 2, physical - pagesize * 2, pagesize, 0x1e, task);
5.47
5.48 + stack_pointers[task] = virtual;
5.49 +
5.50 /*
5.51 - Set the stack for the new task, initialising the global pointer and
5.52 + Set the registers for the new task, initialising the global pointer and
5.53 return address.
5.54 */
5.55
5.56 - init_stack(physical, _got_copy_start, (void (*)()) plot_pattern, args, 3);
5.57 -
5.58 - /* Advance the stack pointer so that the adopted frame will be found. */
5.59 -
5.60 - stack_pointers[task] = virtual - framesize;
5.61 + init_registers(registers[task], _got_copy_start, (void (*)()) plot_pattern, args, 3);
5.62 }
5.63
5.64 void switch_task()
5.65 {
5.66 - /*
5.67 - Get the current stack pointer. This is obtained just after saving the
5.68 - task's state.
5.69 - */
5.70 -
5.71 - stack_pointers[current_task] = current_stack_pointer;
5.72 -
5.73 /* Switch the current task. */
5.74
5.75 current_task++;
5.76 if (current_task == max_tasks) current_task = 1;
5.77
5.78 - /*
5.79 - Set the current stack pointer. This is actually set just before
5.80 - restoring the task's state.
5.81 - */
5.82 -
5.83 - current_stack_pointer = stack_pointers[current_task];
5.84 -}
5.85 -
5.86 -void invoke_task(unsigned short task)
5.87 -{
5.88 - current_task = task;
5.89 - current_stack_pointer = stack_pointers[current_task];
5.90 - set_task(current_task);
5.91 + /* Indicate the current stack pointer and task registers. */
5.92
5.93 - asm volatile(
5.94 - ".set noat\n"
5.95 - "move $sp, %0\n"
5.96 - "addi $sp, $sp, 120\n"
5.97 - "lw $at, -4($sp)\n"
5.98 - "lw $v0, -8($sp)\n"
5.99 - "lw $v1, -12($sp)\n"
5.100 - "lw $a0, -16($sp)\n"
5.101 - "lw $a1, -20($sp)\n"
5.102 - "lw $a2, -24($sp)\n"
5.103 - "lw $a3, -28($sp)\n"
5.104 - "lw $t0, -32($sp)\n"
5.105 - "lw $t1, -36($sp)\n"
5.106 - "lw $t2, -40($sp)\n"
5.107 - "lw $t3, -44($sp)\n"
5.108 - "lw $t4, -48($sp)\n"
5.109 - "lw $t5, -52($sp)\n"
5.110 - "lw $t6, -56($sp)\n"
5.111 - "lw $t7, -60($sp)\n"
5.112 - "lw $s0, -64($sp)\n"
5.113 - "lw $s1, -68($sp)\n"
5.114 - "lw $s2, -72($sp)\n"
5.115 - "lw $s3, -76($sp)\n"
5.116 - "lw $s4, -80($sp)\n"
5.117 - "lw $s5, -84($sp)\n"
5.118 - "lw $s6, -88($sp)\n"
5.119 - "lw $s7, -92($sp)\n"
5.120 - "lw $t8, -96($sp)\n"
5.121 - "lw $t9, -100($sp)\n"
5.122 - "lw $gp, -104($sp)\n"
5.123 - "lw $fp, -108($sp)\n"
5.124 - "lw $ra, -112($sp)\n"
5.125 - "lw $k0, -116($sp)\n"
5.126 - "mtc0 $k0, $14\n" /* CP0_EPC */
5.127 - "nop\n"
5.128 - "jr $ra\n"
5.129 - "nop"
5.130 - :
5.131 - : "r" (current_stack_pointer)
5.132 - );
5.133 + current_stack_pointer = &stack_pointers[current_task];
5.134 + current_registers = registers[current_task];
5.135 }
6.1 --- a/stage2/irq.h Sat Feb 27 23:41:17 2016 +0100
6.2 +++ b/stage2/irq.h Sun Feb 28 01:18:09 2016 +0100
6.3 @@ -4,9 +4,9 @@
6.4 /* Initialisation functions. */
6.5
6.6 void irq_init(void);
6.7 +void init_tasks(void);
6.8 void start_task(unsigned short);
6.9 void switch_task(void);
6.10 -void invoke_task(unsigned short);
6.11 void plot_pattern(unsigned short, unsigned short, unsigned short);
6.12
6.13 #endif /* __IRQ_H__ */
7.1 --- a/stage2/stage2.c Sat Feb 27 23:41:17 2016 +0100
7.2 +++ b/stage2/stage2.c Sun Feb 28 01:18:09 2016 +0100
7.3 @@ -48,6 +48,7 @@
7.4
7.5 /* Start the tasks. */
7.6
7.7 + init_tasks();
7.8 start_task(1);
7.9 start_task(2);
7.10