1.1 --- a/stage2/entry.S Fri Feb 26 01:13:51 2016 +0100
1.2 +++ b/stage2/entry.S Fri Feb 26 20:01:03 2016 +0100
1.3 @@ -18,17 +18,86 @@
1.4 */
1.5
1.6 .text
1.7 -.extern tlb_handler
1.8 .extern interrupt_handler
1.9 .globl _tlb_entry
1.10 .globl _irq_entry
1.11 .globl _end_entries
1.12 .set noreorder
1.13
1.14 +/* NOTE: Duplicated from cpu.h. */
1.15 +
1.16 +#define page_table_start 0x00040000
1.17 +#define page_table_task_size 0x00008000
1.18 +#define page_table_task_size_log2 15
1.19 +
1.20 _tlb_entry:
1.21 + /* Get the bad address. */
1.22 +
1.23 + mfc0 $k0, $10 /* CP0_ENTRYHI */
1.24 + nop
1.25 + andi $k1, $k0, 0xff /* ASID */
1.26 +
1.27 + /* For ASID == 0... */
1.28 +
1.29 + beqz $k1, _tlb_entry_direct
1.30 + nop
1.31 +
1.32 + /* For addresses over 0x00080000... */
1.33 +
1.34 + li $k1, 0xfff80000
1.35 + and $k1, $k0, $k1
1.36 + bnez $k1, _tlb_entry_direct
1.37 + nop
1.38 +
1.39 + /* Otherwise, load the page table entries. */
1.40 +
1.41 + andi $k1, $k0, 0xff /* ASID */
1.42 + sll $k1, $k1, page_table_task_size_log2 /* [ASID] */
1.43 + li $k0, page_table_start /* page_table */
1.44 + add $k0, $k0, $k1 /* page_table[ASID] */
1.45 +
1.46 + mfc0 $k1, $4 /* CP0_CONTEXT */
1.47 + nop
1.48 + srl $k1, $k1, 1 /* use 8 byte - not 16 byte - entries */
1.49 + add $k0, $k0, $k1 /* page_table[ASID][entry] */
1.50 +
1.51 + lw $k1, 0($k0) /* page_table[ASID][entry][0] */
1.52 + mtc0 $k1, $2 /* CP0_ENTRYLO0 */
1.53 + lw $k1, 4($k0) /* page_table[ASID][entry][1] */
1.54 + mtc0 $k1, $3 /* CP0_ENTRYLO1 */
1.55 + /* page size is 4KB */
1.56 + mtc0 $zero, $5 /* CP0_PAGEMASK */
1.57 + nop
1.58 +
1.59 + tlbwr
1.60 + nop
1.61 + eret
1.62 + nop
1.63 +
1.64 +_tlb_entry_direct:
1.65 + /* Otherwise, just translate the address directly. */
1.66 +
1.67 + li $k1, 0xffffe000
1.68 + and $k0, $k0, $k1 /* VPN2 (8KB resolution) */
1.69 + srl $k0, $k0, 6 /* PFN (maintain 8KB resolution, bit 6 remaining zero) */
1.70 + ori $k0, $k0, 0x1e /* flags */
1.71 +
1.72 + mtc0 $k0, $2 /* CP0_ENTRYLO0 */
1.73 + ori $k0, $k0, 0x40 /* page size is 4KB (bit 6 set) */
1.74 + mtc0 $k0, $3 /* CP0_ENTRYLO1 */
1.75 + nop /* page size is 4KB */
1.76 + mtc0 $zero, $5 /* CP0_PAGEMASK */
1.77 + nop
1.78 +
1.79 + tlbwr
1.80 + nop
1.81 + eret
1.82 + nop
1.83 +
1.84 +_irq_entry:
1.85 /* Save the status. */
1.86
1.87 - mfc0 $k0, $12 /* CP0_STATUS */
1.88 + mfc0 $k0, $12 /* CP0_STATUS */
1.89 nop
1.90 sw $k0, -120($sp)
1.91
1.92 @@ -36,32 +105,7 @@
1.93
1.94 li $k1, 0xffff03ff
1.95 and $k1, $k0, $k1
1.96 - mtc0 $k1, $12
1.97 -
1.98 - /* Save registers that the assembler wants to trash. */
1.99 -
1.100 - sw $t9, -100($sp)
1.101 - sw $gp, -104($sp)
1.102 - sw $ra, -112($sp)
1.103 -
1.104 - lui $gp, %hi(_GLOBAL_OFFSET_TABLE_)
1.105 - ori $gp, $gp, %lo(_GLOBAL_OFFSET_TABLE_)
1.106 - la $k0, tlb_handler
1.107 - jr $k0
1.108 - nop
1.109 -
1.110 -_irq_entry:
1.111 - /* Save the status. */
1.112 -
1.113 - mfc0 $k0, $12 /* CP0_STATUS */
1.114 - nop
1.115 - sw $k0, -120($sp)
1.116 -
1.117 - /* Mask interrupts. */
1.118 -
1.119 - li $k1, 0xffff03ff
1.120 - and $k1, $k0, $k1
1.121 - mtc0 $k1, $12
1.122 + mtc0 $k1, $12 /* CP0_STATUS */
1.123
1.124 /* Save registers that the assembler wants to trash. */
1.125