1.1 --- a/stage2/cpu.c Mon Feb 29 19:09:10 2016 +0100
1.2 +++ b/stage2/cpu.c Mon Feb 29 21:29:44 2016 +0100
1.3 @@ -155,18 +155,31 @@
1.4
1.5 void init_tlb(void)
1.6 {
1.7 - unsigned short first_random = 0;
1.8 + unsigned short first_random = 0, i, limit;
1.9
1.10 asm volatile(
1.11 "mtc0 $zero, $4\n" /* CP0_CONTEXT */
1.12 "mtc0 $zero, $10\n" /* CP0_ENTRYHI */
1.13 "mtc0 $zero, $2\n" /* CP0_ENTRYLO0 */
1.14 "mtc0 $zero, $3\n" /* CP0_ENTRYLO1 */
1.15 - "mtc0 %0, $6\n" /* CP0_WIRED */
1.16 + "mtc0 %1, $6\n" /* CP0_WIRED */
1.17 + "mfc0 %0, $16\n" /* CP0_CONFIG1 */
1.18 "nop"
1.19 - :
1.20 + : "=r" (limit)
1.21 : "r" (first_random)
1.22 );
1.23 +
1.24 + /* Reset the mappings. The total number is bits 30..25 of Config1. */
1.25 +
1.26 + for (i = 0; i < ((limit >> 25) & 0x3f); i++)
1.27 + {
1.28 + asm volatile(
1.29 + "mtc0 %0, $0\n" /* CP0_INDEX */
1.30 + "nop"
1.31 + :
1.32 + : "r" (i)
1.33 + );
1.34 + }
1.35 }
1.36
1.37 void map_page_index(u32 virtual, u32 physical, u32 pagesize, u8 flags, u8 asid, u32 index)