1.1 --- a/stage2/cpu.c Mon Feb 22 18:01:27 2016 +0100
1.2 +++ b/stage2/cpu.c Mon Feb 22 20:31:50 2016 +0100
1.3 @@ -22,7 +22,7 @@
1.4 * Boston, MA 02110-1301, USA
1.5 */
1.6
1.7 -#include "xburst_types.h"
1.8 +#include "cpu.h"
1.9 #include "sdram.h"
1.10
1.11 void flush_icache_all(void)
1.12 @@ -138,27 +138,37 @@
1.13 "nop\n"
1.14
1.15 "tlbwi\n"
1.16 - "nop\n"
1.17 + "nop\n");
1.18
1.19 - /* 0x00000000..0x02000000 -> 0x00000000..0x02000000 */
1.20 + map_page(0x00000000, 0x00000000, 16 * 1024 * 1024);
1.21 +}
1.22
1.23 - "li $t0, 1\n"
1.24 - "mtc0 $t0, $0\n" /* CP0_INDEX */
1.25 - "nop\n"
1.26 +void map_page(u32 virtual, u32 physical, u32 pagesize)
1.27 +{
1.28 + u32 start = virtual & 0xffffe000;
1.29 + u32 flags = 0x1f; /* C=3, dirty, global, valid */
1.30 + u32 lower = ((physical & 0xffffc000) >> 6) | flags;
1.31 + u32 upper = (((physical + pagesize) & 0xffffc000) >> 6) | flags;
1.32 + u32 pagemask = ((pagesize - 1) & 0xfffff000) << 1;
1.33 +
1.34 + asm volatile(
1.35 + "mtc0 %3, $5\n" /* CP0_PAGEMASK */
1.36 + "li $t1, 2\n" /* index of first randomly-replaced entry */
1.37 + "mtc0 $t1, $6\n" /* CP0_WIRED */
1.38
1.39 /* Set physical address. */
1.40
1.41 - "li $t0, 0x0000001f\n" /* 0x00000000, C=3, dirty, global, valid */
1.42 - "mtc0 $t0, $2\n" /* CP0_ENTRYLO0 */
1.43 - "li $t1, 0x0004001f\n" /* 0x01000000, C=3, dirty, global, valid */
1.44 - "mtc0 $t1, $3\n" /* CP0_ENTRYLO1 */
1.45 + "mtc0 %0, $2\n" /* CP0_ENTRYLO0 */
1.46 + "mtc0 %1, $3\n" /* CP0_ENTRYLO1 */
1.47
1.48 /* Set virtual address. */
1.49
1.50 - "li $t0, 0x00000000\n" /* 0x00000000, ASID=0 */
1.51 - "mtc0 $t0, $10\n" /* CP0_ENTRYHI */
1.52 + "mtc0 %2, $10\n" /* CP0_ENTRYHI */
1.53 "nop\n"
1.54
1.55 - "tlbwi\n"
1.56 - "nop");
1.57 + "tlbwr\n"
1.58 + "nop"
1.59 + :
1.60 + : "r" (lower), "r" (upper), "r" (start), "r" (pagemask)
1.61 + );
1.62 }