@@ -213,7 +220,23 @@ __create_page_tables:
 	orr	r6, r6, #(PHYS_OFFSET & 0x00f00000)
 	.endif
 	str	r6, [r0]
+	ldr  r12, =0x2FFC002C
+	ldr  r13, =0x2020420A
+	str  r13, [r12, #0]
+
+	add r0, r4, #0xFF000000 >> 18
+	orr	r6, r7, #(0x2F000000 & 0xff000000)
+	.if	(0x2F000000 & 0x00f00000)
+		orr	r6, r6, #(0x2F000000 & 0x00f00000)
+	.endif
+	str	r6, [r0]
+
+	ldr  r12, =0x2FFC0030
+	ldr  r13, =0x20202043
+	str  r13, [r12, #0]
 #ifdef CONFIG_DEBUG_LL
 #ifndef CONFIG_DEBUG_ICEDCC

@@ -380,11 +403,27 @@ ENDPROC(__enable_mmu)
  */
 	.align	5
 __turn_mmu_on:
+
+	ldr  r12, =0x2FFC0034
+	ldr  r13, =0x2020440A
+	str  r13, [r12, #0]
+
+	isb sy
+	dsb sy
+
 	mov	r0, r0
 	mcr	p15, 0, r0, c1, c0, 0		@ write control reg
 	mrc	p15, 0, r3, c0, c0, 0		@ read id reg
 	mov	r3, r3
 	mov	r3, r13
+
+	isb sy
+	dsb sy
+
+	ldr  r12, =0xFFFC0038 @ vaddr, __create_page_tables
+	ldr  r13, =0x2020450A
+	str  r13, [r12, #0]
+
 	mov	pc, r3
 __enable_mmu_end:
 ENDPROC(__turn_mmu_on)