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(