1.1 --- a/stage2/cpu.c Tue Feb 23 16:50:21 2016 +0100
1.2 +++ b/stage2/cpu.c Tue Feb 23 22:51:36 2016 +0100
1.3 @@ -80,18 +80,8 @@
1.4 void handle_error_level(void)
1.5 {
1.6 asm volatile(
1.7 - "mfc0 $t3, $12\n" /* CP0_STATUS */
1.8 - "li $t4, 0xfffffffb\n" /* ERL = 0 */
1.9 - "and $t3, $t3, $t4\n"
1.10 - "mtc0 $t3, $12\n"
1.11 - "nop\n");
1.12 -}
1.13 -
1.14 -void enter_user_mode(void)
1.15 -{
1.16 - asm volatile(
1.17 - "mfc0 $t3, $12\n" /* CP0_STATUS */
1.18 - "li $t4, 0xffffffef\n" /* KSU = 2 (UM = 1) */
1.19 + "mfc0 $t3, $12\n" /* CP0_STATUS */
1.20 + "li $t4, 0xfffffffb\n" /* ERL = 0 */
1.21 "and $t3, $t3, $t4\n"
1.22 "mtc0 $t3, $12\n"
1.23 "nop\n");
1.24 @@ -100,8 +90,8 @@
1.25 void enable_interrupts(void)
1.26 {
1.27 asm volatile(
1.28 - "mfc0 $t3, $12\n" /* CP0_STATUS */
1.29 - "li $t4, 0x0000fc01\n" /* IE = enable interrupts */
1.30 + "mfc0 $t3, $12\n" /* CP0_STATUS */
1.31 + "li $t4, 0x0000fc01\n" /* IE = enable interrupts */
1.32 "or $t3, $t3, $t4\n"
1.33 "mtc0 $t3, $12\n"
1.34 "nop\n");
1.35 @@ -122,39 +112,60 @@
1.36 "nop\n");
1.37 }
1.38
1.39 +void enter_user_mode(void)
1.40 +{
1.41 + asm volatile(
1.42 + "mfc0 $t3, $12\n" /* CP0_STATUS */
1.43 + "li $t4, 0xffffffef\n" /* KSU = 2 (UM = 1) */
1.44 + "and $t3, $t3, $t4\n"
1.45 + "mtc0 $t3, $12\n"
1.46 + "nop\n");
1.47 +}
1.48 +
1.49 void init_tlb(void)
1.50 {
1.51 asm volatile(
1.52 - "li $t0, 0x01ffe000\n" /* 16MB */
1.53 - "mtc0 $t0, $5\n" /* CP0_PAGEMASK */
1.54 "li $t1, 1\n" /* index of first randomly-replaced entry */
1.55 "mtc0 $t1, $6\n" /* CP0_WIRED */
1.56 + "nop\n");
1.57
1.58 - /* 0x80000000..0x82000000 -> 0x00000000..0x02000000 */
1.59 + map_page_index(0x00000000, 0x00000000, 16 * 1024 * 1024, 0x1f, 0, 0);
1.60 +}
1.61
1.62 - "mtc0 $zero, $0\n" /* CP0_INDEX */
1.63 +void map_page_index(u32 virtual, u32 physical, u32 pagesize, u8 flags, u8 asid, u32 index)
1.64 +{
1.65 + u32 start = virtual & 0xffffe000; /* VPN2 */
1.66 + u32 lower = ((physical & 0xfffff000) >> 6) | flags;
1.67 + u32 upper = (((physical + pagesize) & 0xfffff000) >> 6) | flags;
1.68 + u32 pagemask = ((pagesize - 1) & 0xfffff000) << 1;
1.69 +
1.70 + asm volatile(
1.71 + "mtc0 %3, $5\n" /* CP0_PAGEMASK */
1.72 +
1.73 + /* Set the index. */
1.74 +
1.75 + "mtc0 %4, $0\n" /* CP0_INDEX */
1.76
1.77 /* Set physical address. */
1.78
1.79 - "li $t0, 0x0000001f\n" /* 0x00000000, C=3, dirty, global, valid */
1.80 - "mtc0 $t0, $2\n" /* CP0_ENTRYLO0 */
1.81 - "li $t1, 0x0004001f\n" /* 0x01000000, C=3, dirty, global, valid */
1.82 - "mtc0 $t1, $3\n" /* CP0_ENTRYLO1 */
1.83 + "mtc0 %0, $2\n" /* CP0_ENTRYLO0 */
1.84 + "mtc0 %1, $3\n" /* CP0_ENTRYLO1 */
1.85
1.86 /* Set virtual address. */
1.87
1.88 - "li $t0, 0x80000000\n" /* 0x80000000, ASID=0 */
1.89 - "mtc0 $t0, $10\n" /* CP0_ENTRYHI */
1.90 + "mtc0 %2, $10\n" /* CP0_ENTRYHI */
1.91 "nop\n"
1.92
1.93 "tlbwi\n"
1.94 - "nop\n");
1.95 + "nop"
1.96 + :
1.97 + : "r" (lower), "r" (upper), "r" (start), "r" (pagemask), "r" (index)
1.98 + );
1.99 }
1.100
1.101 -void map_page(u32 virtual, u32 physical, u32 pagesize)
1.102 +void map_page(u32 virtual, u32 physical, u32 pagesize, u8 flags, u8 asid)
1.103 {
1.104 u32 start = virtual & 0xffffe000; /* VPN2 */
1.105 - u32 flags = 0x1f; /* C=3, dirty, global, valid */
1.106 u32 lower = ((physical & 0xfffff000) >> 6) | flags;
1.107 u32 upper = (((physical + pagesize) & 0xfffff000) >> 6) | flags;
1.108 u32 pagemask = ((pagesize - 1) & 0xfffff000) << 1;
1.109 @@ -179,6 +190,50 @@
1.110 );
1.111 }
1.112
1.113 +void unmap_page(u32 virtual, u32 physical, u32 pagesize, u8 flags, u8 asid)
1.114 +{
1.115 + u32 start = virtual & 0xffffe000; /* VPN2 */
1.116 + u32 lower = ((physical & 0xfffff000) >> 6) | flags;
1.117 + u32 upper = (((physical + pagesize) & 0xfffff000) >> 6) | flags;
1.118 + u32 pagemask = ((pagesize - 1) & 0xfffff000) << 1;
1.119 + u32 index = 0;
1.120 +
1.121 + asm volatile(
1.122 + "mtc0 %4, $5\n" /* CP0_PAGEMASK */
1.123 +
1.124 + /* Set physical address. */
1.125 +
1.126 + "mtc0 %1, $2\n" /* CP0_ENTRYLO0 */
1.127 + "mtc0 %2, $3\n" /* CP0_ENTRYLO1 */
1.128 +
1.129 + /* Set virtual address. */
1.130 +
1.131 + "mtc0 %3, $10\n" /* CP0_ENTRYHI */
1.132 + "nop\n"
1.133 +
1.134 + /* Find an existing mapping. */
1.135 +
1.136 + "tlbp\n"
1.137 + "nop\n"
1.138 +
1.139 + /* Read the index register to see if a match was found. */
1.140 +
1.141 + "mfc0 %0, $0\n" /* CP0_INDEX */
1.142 + "nop"
1.143 + : "=r" (index)
1.144 + : "r" (lower), "r" (upper), "r" (start), "r" (pagemask)
1.145 + );
1.146 +
1.147 + /* Return if the page is not mapped. */
1.148 +
1.149 + if (index & 0x80000000)
1.150 + return;
1.151 +
1.152 + /* Otherwise, invalidate the mapping. */
1.153 +
1.154 + map_page_index(virtual, physical, pagesize, flags & 0xfd, asid, index);
1.155 +}
1.156 +
1.157 void tlb_handle(void)
1.158 {
1.159 u32 context, virtual;
1.160 @@ -196,5 +251,5 @@
1.161
1.162 /* Request a physical region mapping two 4KB pages. */
1.163
1.164 - map_page(virtual, virtual, 4 * 1024);
1.165 + map_page(virtual, virtual, 4 * 1024, 0x1f, 0);
1.166 }
2.1 --- a/stage2/cpu.h Tue Feb 23 16:50:21 2016 +0100
2.2 +++ b/stage2/cpu.h Tue Feb 23 22:51:36 2016 +0100
2.3 @@ -9,7 +9,9 @@
2.4 void enable_interrupts(void);
2.5 void init_interrupts(void);
2.6 void init_tlb(void);
2.7 -void map_page(u32, u32, u32);
2.8 +void map_page(u32, u32, u32, u8, u8);
2.9 +void map_page_index(u32, u32, u32, u8, u8, u32);
2.10 +void unmap_page(u32, u32, u32, u8, u8);
2.11 void tlb_handle(void);
2.12
2.13 #endif /* __CPU_H__ */