1.1 --- a/stage2/cpu.c Tue Jun 30 16:09:27 2015 +0200
1.2 +++ b/stage2/cpu.c Tue Jun 30 16:10:40 2015 +0200
1.3 @@ -76,13 +76,71 @@
1.4 flush_icache_all();
1.5 }
1.6
1.7 +void handle_error_level(void)
1.8 +{
1.9 + asm volatile(
1.10 + "mfc0 $t3, $12\n" /* CP0_STATUS */
1.11 + "nop\n"
1.12 + "li $t4, 0xfffffffb\n" /* ERL = 0 */
1.13 + "and $t3, $t3, $t4\n"
1.14 + "mtc0 $t3, $12\n"
1.15 + "nop\n");
1.16 +}
1.17 +
1.18 void enable_interrupts(void)
1.19 {
1.20 asm volatile(
1.21 "mfc0 $t3, $12\n" /* CP0_STATUS */
1.22 "nop\n"
1.23 - "li $t4, 0x00000001\n" /* IE = enable interrupts */
1.24 + "li $t4, 0x0000fc01\n" /* IE = enable interrupts */
1.25 "or $t3, $t3, $t4\n"
1.26 "mtc0 $t3, $12\n"
1.27 "nop\n");
1.28 }
1.29 +
1.30 +void init_interrupts(void)
1.31 +{
1.32 + /* Set exception registers. */
1.33 +
1.34 + asm volatile(
1.35 + "mtc0 $zero, $18\n" /* CP0_WATCHLO */
1.36 + "nop\n"
1.37 + "li $t3, 0x00800000\n" /* IV = 1 (use 0x80000200 for interrupts) */
1.38 + "mtc0 $t3, $13\n" /* CP0_CAUSE */
1.39 + "nop\n"
1.40 + "mtc0 $zero, $12\n" /* CP0_STATUS */
1.41 + "nop\n"
1.42 + "mtc0 $zero, $15\n" /* CP0_EBASE (should be zero anyway) */
1.43 + "nop\n");
1.44 +}
1.45 +
1.46 +void init_tlb(void)
1.47 +{
1.48 + asm volatile(
1.49 + "li $t0, 0x001fe000\n" /* 1MB */
1.50 + "mtc0 $t0, $5\n" /* CP0_PAGEMASK */
1.51 + "nop\n"
1.52 + "mtc0 $zero, $6\n" /* CP0_WIRED */
1.53 + "nop\n"
1.54 +
1.55 + /* Set physical address. */
1.56 +
1.57 + "li $t0, 0x00000003\n" /* 0x000000.. global, valid */
1.58 + "mtc0 $t0, $2\n" /* CP0_ENTRYLO0 */
1.59 + "nop\n"
1.60 + "li $t0, 0x00000003\n" /* 0x000000.. global, valid */
1.61 + "mtc0 $zero, $3\n" /* CP0_ENTRYLO1 */
1.62 + "nop\n"
1.63 +
1.64 + /* Set virtual address. */
1.65 +
1.66 + "li $t0, 0x80000000\n" /* 0x80000... */
1.67 + "mtc0 $t0, $10\n" /* CP0_ENTRYHI */
1.68 + "nop\n"
1.69 +
1.70 + /* Set index. */
1.71 +
1.72 + "mtc0 $zero, $0\n" /* CP0_INDEX */
1.73 + "nop\n"
1.74 + "tlbwi\n");
1.75 +}