Merge branch 'for-next/arm64'

12 files changed, 920 insertions, 10 deletions

diff --git a/arch/arm/cpu/Kconfig b/arch/arm/cpu/Kconfig
index 4f5d9b6e16..450a6d593a 100644
--- a/arch/arm/cpu/Kconfig
+++ b/arch/arm/cpu/Kconfig
@@ -1,8 +1,14 @@
 comment "Processor Type"
 
+config PHYS_ADDR_T_64BIT
+	bool
+
 config CPU_32
 	bool
-	default y
+
+config CPU_64
+	bool
+	select PHYS_ADDR_T_64BIT
 
 # Select CPU types depending on the architecture selected. This selects
 # which CPUs we support in the kernel image, and the compiler instruction
@@ -69,6 +75,12 @@ config CPU_V7
 	bool
 	select CPU_32v7
 
+# ARMv8
+config CPU_V8
+	bool
+	select CPU_64v8
+	select CPU_SUPPORTS_64BIT_KERNEL
+
 config CPU_XSC3
         bool
         select CPU_32v4T
@@ -84,15 +96,23 @@ config CPU_XSCALE
 # This defines the compiler instruction set which depends on the machine type.
 config CPU_32v4T
 	bool
+	select CPU_32
 
 config CPU_32v5
 	bool
+	select CPU_32
 
 config CPU_32v6
 	bool
+	select CPU_32
 
 config CPU_32v7
 	bool
+	select CPU_32
+
+config CPU_64v8
+	bool
+	select CPU_64
 
 comment "processor features"
 
@@ -124,3 +144,14 @@ config CACHE_L2X0
 	bool "Enable L2x0 PrimeCell"
 	depends on MMU && ARCH_HAS_L2X0
 
+config SYS_SUPPORTS_32BIT_KERNEL
+	bool
+
+config SYS_SUPPORTS_64BIT_KERNEL
+	bool
+
+config CPU_SUPPORTS_32BIT_KERNEL
+	bool
+
+config CPU_SUPPORTS_64BIT_KERNEL
+	bool
diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index 854df60ebb..331c1cd8bc 100644
--- a/arch/arm/cpu/Makefile
+++ b/arch/arm/cpu/Makefile
@@ -1,7 +1,24 @@
 obj-y += cpu.o
+
+ifeq ($(CONFIG_CPU_64v8), y)
+obj-$(CONFIG_ARM_EXCEPTIONS) += exceptions_64.o
+obj-$(CONFIG_MMU) += mmu_64.o
+lwl-y += lowlevel_64.o
+else
 obj-$(CONFIG_ARM_EXCEPTIONS) += exceptions.o
+obj-$(CONFIG_MMU) += mmu.o mmu-early.o
+pbl-$(CONFIG_MMU) += mmu-early.o
+lwl-y += lowlevel.o
+endif
+
 obj-$(CONFIG_ARM_EXCEPTIONS) += interrupts.o
-obj-y += start.o setupc.o entry.o
+obj-y += start.o entry.o
+
+ifeq ($(CONFIG_CPU_64v8), y)
+obj-y += setupc_64.o
+else
+obj-y += setupc.o
+endif
 
 #
 # Any variants can be called as start-armxyz.S
@@ -9,8 +26,7 @@ obj-y += start.o setupc.o entry.o
 obj-$(CONFIG_CMD_ARM_CPUINFO) += cpuinfo.o
 obj-$(CONFIG_CMD_ARM_MMUINFO) += mmuinfo.o
 obj-$(CONFIG_OFDEVICE) += dtb.o
-obj-$(CONFIG_MMU) += mmu.o cache.o mmu-early.o
-pbl-$(CONFIG_MMU) += mmu-early.o
+obj-$(CONFIG_MMU) += cache.o
 
 ifeq ($(CONFIG_MMU),)
 obj-y += no-mmu.o
@@ -27,6 +43,10 @@ obj-$(CONFIG_CPU_32v7) += cache-armv7.o
 AFLAGS_pbl-cache-armv7.o       :=-Wa,-march=armv7-a
 pbl-$(CONFIG_CPU_32v7) += cache-armv7.o
 obj-$(CONFIG_CACHE_L2X0) += cache-l2x0.o
+AFLAGS_cache-armv8.o       :=-Wa,-march=armv8-a
+obj-$(CONFIG_CPU_64v8) += cache-armv8.o
+AFLAGS_pbl-cache-armv8.o       :=-Wa,-march=armv8-a
+pbl-$(CONFIG_CPU_64v8) += cache-armv8.o
 
 pbl-y += setupc.o entry.o
 pbl-$(CONFIG_PBL_SINGLE_IMAGE) += start-pbl.o
@@ -34,5 +54,3 @@ pbl-$(CONFIG_PBL_MULTI_IMAGES) += uncompress.o
 
 obj-y += common.o cache.o
 pbl-y += common.o cache.o
-
-lwl-y += lowlevel.o
diff --git a/arch/arm/cpu/cache-armv8.S b/arch/arm/cpu/cache-armv8.S
new file mode 100644
index 0000000000..82b2f81778
--- /dev/null
+++ b/arch/arm/cpu/cache-armv8.S
@@ -0,0 +1,168 @@
+/*
+ * (C) Copyright 2013
+ * David Feng <fenghua@phytium.com.cn>
+ *
+ * This file is based on sample code from ARMv8 ARM.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <init.h>
+
+/*
+ * void v8_flush_dcache_level(level)
+ *
+ * clean and invalidate one level cache.
+ *
+ * x0: cache level
+ * x1: 0 flush & invalidate, 1 invalidate only
+ * x2~x9: clobbered
+ */
+.section .text.v8_flush_dcache_level
+ENTRY(v8_flush_dcache_level)
+	lsl	x12, x0, #1
+	msr	csselr_el1, x12		/* select cache level */
+	isb				/* sync change of cssidr_el1 */
+	mrs	x6, ccsidr_el1		/* read the new cssidr_el1 */
+	and	x2, x6, #7		/* x2 <- log2(cache line size)-4 */
+	add	x2, x2, #4		/* x2 <- log2(cache line size) */
+	mov	x3, #0x3ff
+	and	x3, x3, x6, lsr #3	/* x3 <- max number of #ways */
+	clz	w5, w3			/* bit position of #ways */
+	mov	x4, #0x7fff
+	and	x4, x4, x6, lsr #13	/* x4 <- max number of #sets */
+	/* x12 <- cache level << 1 */
+	/* x2 <- line length offset */
+	/* x3 <- number of cache ways - 1 */
+	/* x4 <- number of cache sets - 1 */
+	/* x5 <- bit position of #ways */
+
+loop_set:
+	mov	x6, x3			/* x6 <- working copy of #ways */
+loop_way:
+	lsl	x7, x6, x5
+	orr	x9, x12, x7		/* map way and level to cisw value */
+	lsl	x7, x4, x2
+	orr	x9, x9, x7		/* map set number to cisw value */
+	tbz	w1, #0, 1f
+	dc	isw, x9
+	b	2f
+1:	dc	cisw, x9		/* clean & invalidate by set/way */
+2:	subs	x6, x6, #1		/* decrement the way */
+	b.ge	loop_way
+	subs	x4, x4, #1		/* decrement the set */
+	b.ge	loop_set
+
+	ret
+ENDPROC(v8_flush_dcache_level)
+
+/*
+ * void v8_flush_dcache_all(int invalidate_only)
+ *
+ * x0: 0 flush & invalidate, 1 invalidate only
+ *
+ * clean and invalidate all data cache by SET/WAY.
+ */
+.section .text.v8_dcache_all
+ENTRY(v8_dcache_all)
+	mov	x1, x0
+	dsb	sy
+	mrs	x10, clidr_el1		/* read clidr_el1 */
+	lsr	x11, x10, #24
+	and	x11, x11, #0x7		/* x11 <- loc */
+	cbz	x11, finished		/* if loc is 0, exit */
+	mov	x15, x30
+	mov	x0, #0			/* start flush at cache level 0 */
+	/* x0  <- cache level */
+	/* x10 <- clidr_el1 */
+	/* x11 <- loc */
+	/* x15 <- return address */
+
+loop_level:
+	lsl	x12, x0, #1
+	add	x12, x12, x0		/* x0 <- tripled cache level */
+	lsr	x12, x10, x12
+	and	x12, x12, #7		/* x12 <- cache type */
+	cmp	x12, #2
+	b.lt	skip			/* skip if no cache or icache */
+	bl	v8_flush_dcache_level	/* x1 = 0 flush, 1 invalidate */
+skip:
+	add	x0, x0, #1		/* increment cache level */
+	cmp	x11, x0
+	b.gt	loop_level
+
+	mov	x0, #0
+	msr	csselr_el1, x0		/* restore csselr_el1 */
+	dsb	sy
+	isb
+	mov	x30, x15
+
+finished:
+	ret
+ENDPROC(v8_dcache_all)
+
+.section .text.v8_flush_dcache_all
+ENTRY(v8_flush_dcache_all)
+	mov	x16, x30
+	mov	x0, #0
+	bl	v8_dcache_all
+	mov	x30, x16
+	ret
+ENDPROC(v8_flush_dcache_all)
+
+.section .text.v8_invalidate_dcache_all
+ENTRY(v8_invalidate_dcache_all)
+	mov	x16, x30
+	mov	x0, #0x1
+	bl	v8_dcache_all
+	mov	x30, x16
+	ret
+ENDPROC(v8_invalidate_dcache_all)
+
+/*
+ * void v8_flush_dcache_range(start, end)
+ *
+ * clean & invalidate data cache in the range
+ *
+ * x0: start address
+ * x1: end address
+ */
+.section .text.v8_flush_dcache_range
+ENTRY(v8_flush_dcache_range)
+	mrs	x3, ctr_el0
+	lsr	x3, x3, #16
+	and	x3, x3, #0xf
+	mov	x2, #4
+	lsl	x2, x2, x3		/* cache line size */
+
+	/* x2 <- minimal cache line size in cache system */
+	sub	x3, x2, #1
+	bic	x0, x0, x3
+1:	dc	civac, x0	/* clean & invalidate data or unified cache */
+	add	x0, x0, x2
+	cmp	x0, x1
+	b.lo	1b
+	dsb	sy
+	ret
+ENDPROC(v8_flush_dcache_range)
+
+/*
+ * void v8_invalidate_icache_all(void)
+ *
+ * invalidate all tlb entries.
+ */
+.section .text.v8_invalidate_icache_all
+ENTRY(v8_invalidate_icache_all)
+	ic	ialluis
+	isb	sy
+	ret
+ENDPROC(v8_invalidate_icache_all)
+
+.section .text.v8_flush_l3_cache
+ENTRY(v8_flush_l3_cache)
+	mov	x0, #0			/* return status as success */
+	ret
+ENDPROC(v8_flush_l3_cache)
+	.weak	v8_flush_l3_cache
diff --git a/arch/arm/cpu/cache.c b/arch/arm/cpu/cache.c
index 27ead1c177..929c385df5 100644
--- a/arch/arm/cpu/cache.c
+++ b/arch/arm/cpu/cache.c
@@ -36,6 +36,7 @@ DEFINE_CPU_FNS(v4)
 DEFINE_CPU_FNS(v5)
 DEFINE_CPU_FNS(v6)
 DEFINE_CPU_FNS(v7)
+DEFINE_CPU_FNS(v8)
 
 void __dma_clean_range(unsigned long start, unsigned long end)
 {
@@ -101,6 +102,11 @@ int arm_set_cache_functions(void)
 		cache_fns = &cache_fns_armv7;
 		break;
 #endif
+#ifdef CONFIG_CPU_64v8
+	case CPU_ARCH_ARMv8:
+		cache_fns = &cache_fns_armv8;
+		break;
+#endif
 	default:
 		while(1);
 	}
@@ -138,6 +144,11 @@ void arm_early_mmu_cache_flush(void)
 		v7_mmu_cache_flush();
 		return;
 #endif
+#ifdef CONFIG_CPU_64v8
+	case CPU_ARCH_ARMv8:
+		v8_dcache_all();
+		return;
+#endif
 	}
 }
 
@@ -146,6 +157,7 @@ void v7_mmu_cache_invalidate(void);
 void arm_early_mmu_cache_invalidate(void)
 {
 	switch (arm_early_get_cpu_architecture()) {
+#if __LINUX_ARM_ARCH__ <= 7
 	case CPU_ARCH_ARMv4T:
 	case CPU_ARCH_ARMv5:
 	case CPU_ARCH_ARMv5T:
@@ -159,5 +171,12 @@ void arm_early_mmu_cache_invalidate(void)
 		v7_mmu_cache_invalidate();
 		return;
 #endif
+#else
+#ifdef CONFIG_CPU_64v8
+	case CPU_ARCH_ARMv8:
+		v8_invalidate_icache_all();
+		return;
+#endif
+#endif
 	}
 }
diff --git a/arch/arm/cpu/cpu.c b/arch/arm/cpu/cpu.c
index eb12166c16..b4804634b7 100644
--- a/arch/arm/cpu/cpu.c
+++ b/arch/arm/cpu/cpu.c
@@ -68,6 +68,7 @@ int icache_status(void)
 	return (get_cr () & CR_I) != 0;
 }
 
+#if __LINUX_ARM_ARCH__ <= 7
 /*
  * SoC like the ux500 have the l2x0 always enable
  * with or without MMU enable
@@ -86,6 +87,7 @@ void mmu_disable(void)
 	}
 	__mmu_cache_off();
 }
+#endif
 
 /**
  * Disable MMU and D-cache, flush caches
@@ -98,8 +100,12 @@ static void arch_shutdown(void)
 {
 	uint32_t r;
 
+#ifdef CONFIG_MMU
 	mmu_disable();
+#endif
 	flush_icache();
+
+#if __LINUX_ARM_ARCH__ <= 7
 	/*
 	 * barebox normally does not use interrupts, but some functionalities
 	 * (eg. OMAP4_USBBOOT) require them enabled. So be sure interrupts are
@@ -108,6 +114,7 @@ static void arch_shutdown(void)
 	__asm__ __volatile__("mrs %0, cpsr" : "=r"(r));
 	r |= PSR_I_BIT;
 	__asm__ __volatile__("msr cpsr, %0" : : "r"(r));
+#endif
 }
 archshutdown_exitcall(arch_shutdown);
 
diff --git a/arch/arm/cpu/cpuinfo.c b/arch/arm/cpu/cpuinfo.c
index 8b22e9bb50..86e19d9780 100644
--- a/arch/arm/cpu/cpuinfo.c
+++ b/arch/arm/cpu/cpuinfo.c
@@ -30,12 +30,15 @@
 #define CPU_ARCH_ARMv5TEJ	7
 #define CPU_ARCH_ARMv6		8
 #define CPU_ARCH_ARMv7		9
+#define CPU_ARCH_ARMv8		10
 
 #define ARM_CPU_PART_CORTEX_A5      0xC050
 #define ARM_CPU_PART_CORTEX_A7      0xC070
 #define ARM_CPU_PART_CORTEX_A8      0xC080
 #define ARM_CPU_PART_CORTEX_A9      0xC090
 #define ARM_CPU_PART_CORTEX_A15     0xC0F0
+#define ARM_CPU_PART_CORTEX_A53	    0xD030
+#define ARM_CPU_PART_CORTEX_A57	    0xD070
 
 static void decode_cache(unsigned long size)
 {
@@ -60,6 +63,25 @@ static int do_cpuinfo(int argc, char *argv[])
 	int i;
 	int cpu_arch;
 
+#ifdef CONFIG_CPU_64v8
+	__asm__ __volatile__(
+		"mrs	%0, midr_el1\n"
+		: "=r" (mainid)
+		:
+		: "memory");
+
+	__asm__ __volatile__(
+		"mrs	%0, ctr_el0\n"
+		: "=r" (cache)
+		:
+		: "memory");
+
+	__asm__ __volatile__(
+		"mrs	%0, sctlr_el1\n"
+		: "=r" (cr)
+		:
+		: "memory");
+#else
 	__asm__ __volatile__(
 		"mrc    p15, 0, %0, c0, c0, 0   @ read control reg\n"
 		: "=r" (mainid)
@@ -74,9 +96,10 @@ static int do_cpuinfo(int argc, char *argv[])
 
 	__asm__ __volatile__(
 		"mrc    p15, 0, %0, c1, c0, 0   @ read control reg\n"
-			: "=r" (cr)
-			:
-			: "memory");
+		: "=r" (cr)
+		:
+		: "memory");
+#endif
 
 	switch (mainid >> 24) {
 	case 0x41:
@@ -107,6 +130,23 @@ static int do_cpuinfo(int argc, char *argv[])
 		if (cpu_arch)
 			cpu_arch += CPU_ARCH_ARMv3;
 	} else if ((mainid & 0x000f0000) == 0x000f0000) {
+#ifdef CONFIG_CPU_64v8
+		unsigned int isar2;
+
+		__asm__ __volatile__(
+			"mrs	%0, id_isar2_el1\n"
+			: "=r" (isar2)
+			:
+			: "memory");
+
+
+		/* Check Load/Store acquire to check if ARMv8 or not */
+
+		if (isar2 & 0x2)
+			cpu_arch = CPU_ARCH_ARMv8;
+		else
+			cpu_arch = CPU_ARCH_UNKNOWN;
+#else
 		unsigned int mmfr0;
 
 		/* Revised CPUID format. Read the Memory Model Feature
@@ -121,6 +161,7 @@ static int do_cpuinfo(int argc, char *argv[])
 			cpu_arch = CPU_ARCH_ARMv6;
 		else
 			cpu_arch = CPU_ARCH_UNKNOWN;
+#endif
 	} else
 		cpu_arch = CPU_ARCH_UNKNOWN;
 
@@ -152,6 +193,9 @@ static int do_cpuinfo(int argc, char *argv[])
 	case CPU_ARCH_ARMv7:
 		architecture = "v7";
 		break;
+	case CPU_ARCH_ARMv8:
+		architecture = "v8";
+		break;
 	case CPU_ARCH_UNKNOWN:
 	default:
 		architecture = "Unknown";
@@ -160,7 +204,7 @@ static int do_cpuinfo(int argc, char *argv[])
 	printf("implementer: %s\narchitecture: %s\n",
 			implementer, architecture);
 
-	if (cpu_arch == CPU_ARCH_ARMv7) {
+	if (cpu_arch >= CPU_ARCH_ARMv7) {
 		unsigned int major, minor;
 		char *part;
 		major = (mainid >> 20) & 0xf;
@@ -181,6 +225,12 @@ static int do_cpuinfo(int argc, char *argv[])
 		case ARM_CPU_PART_CORTEX_A15:
 			part = "Cortex-A15";
 			break;
+		case ARM_CPU_PART_CORTEX_A53:
+			part = "Cortex-A53";
+			break;
+		case ARM_CPU_PART_CORTEX_A57:
+			part = "Cortex-A57";
+			break;
 		default:
 			part = "unknown";
 		}
diff --git a/arch/arm/cpu/exceptions_64.S b/arch/arm/cpu/exceptions_64.S
new file mode 100644
index 0000000000..58120253a1
--- /dev/null
+++ b/arch/arm/cpu/exceptions_64.S
@@ -0,0 +1,127 @@
+/*
+ * (C) Copyright 2013
+ * David Feng <fenghua@phytium.com.cn>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <asm/ptrace.h>
+#include <linux/linkage.h>
+
+/*
+ * Enter Exception.
+ * This will save the processor state that is ELR/X0~X30
+ * to the stack frame.
+ */
+.macro	exception_entry
+	stp	x29, x30, [sp, #-16]!
+	stp	x27, x28, [sp, #-16]!
+	stp	x25, x26, [sp, #-16]!
+	stp	x23, x24, [sp, #-16]!
+	stp	x21, x22, [sp, #-16]!
+	stp	x19, x20, [sp, #-16]!
+	stp	x17, x18, [sp, #-16]!
+	stp	x15, x16, [sp, #-16]!
+	stp	x13, x14, [sp, #-16]!
+	stp	x11, x12, [sp, #-16]!
+	stp	x9, x10, [sp, #-16]!
+	stp	x7, x8, [sp, #-16]!
+	stp	x5, x6, [sp, #-16]!
+	stp	x3, x4, [sp, #-16]!
+	stp	x1, x2, [sp, #-16]!
+
+	/* Could be running at EL3/EL2/EL1 */
+	mrs	x11, CurrentEL
+	cmp	x11, #0xC		/* Check EL3 state */
+	b.eq	1f
+	cmp	x11, #0x8		/* Check EL2 state */
+	b.eq	2f
+	cmp	x11, #0x4		/* Check EL1 state */
+	b.eq	3f
+3:	mrs	x1, esr_el3
+	mrs	x2, elr_el3
+	b	0f
+2:	mrs	x1, esr_el2
+	mrs	x2, elr_el2
+	b	0f
+1:	mrs	x1, esr_el1
+	mrs	x2, elr_el1
+0:
+	stp	x2, x0, [sp, #-16]!
+	mov	x0, sp
+.endm
+
+/*
+ * Exception vectors.
+ */
+	.align	11
+	.globl	vectors
+vectors:
+	.align	7
+	b	_do_bad_sync	/* Current EL Synchronous Thread */
+
+	.align	7
+	b	_do_bad_irq	/* Current EL IRQ Thread */
+
+	.align	7
+	b	_do_bad_fiq	/* Current EL FIQ Thread */
+
+	.align	7
+	b	_do_bad_error	/* Current EL Error Thread */
+
+	.align	7
+	b	_do_sync	/* Current EL Synchronous Handler */
+
+	.align	7
+	b	_do_irq		/* Current EL IRQ Handler */
+
+	.align	7
+	b	_do_fiq		/* Current EL FIQ Handler */
+
+	.align	7
+	b	_do_error	/* Current EL Error Handler */
+
+
+_do_bad_sync:
+	exception_entry
+	bl	do_bad_sync
+
+_do_bad_irq:
+	exception_entry
+	bl	do_bad_irq
+
+_do_bad_fiq:
+	exception_entry
+	bl	do_bad_fiq
+
+_do_bad_error:
+	exception_entry
+	bl	do_bad_error
+
+_do_sync:
+	exception_entry
+	bl	do_sync
+
+_do_irq:
+	exception_entry
+	bl	do_irq
+
+_do_fiq:
+	exception_entry
+	bl	do_fiq
+
+_do_error:
+	exception_entry
+	bl	do_error
+
+.section .data
+.align 4
+.global arm_ignore_data_abort
+arm_ignore_data_abort:
+.word 0 /* When != 0 data aborts are ignored */
+.global arm_data_abort_occurred
+arm_data_abort_occurred:
+.word 0 /* set != 0 by the data abort handler */
+abort_stack:
+.space 8
diff --git a/arch/arm/cpu/interrupts.c b/arch/arm/cpu/interrupts.c
index fb4bb78dae..c34108a4f8 100644
--- a/arch/arm/cpu/interrupts.c
+++ b/arch/arm/cpu/interrupts.c
@@ -27,6 +27,8 @@
 #include <asm/ptrace.h>
 #include <asm/unwind.h>
 
+
+#if __LINUX_ARM_ARCH__ <= 7
 /**
  * Display current register set content
  * @param[in] regs Guess what
@@ -70,10 +72,13 @@ void show_regs (struct pt_regs *regs)
 	unwind_backtrace(regs);
 #endif
 }
+#endif
 
 static void __noreturn do_exception(struct pt_regs *pt_regs)
 {
+#if __LINUX_ARM_ARCH__ <= 7
 	show_regs(pt_regs);
+#endif
 
 	panic("");
 }
@@ -121,6 +126,8 @@ void do_prefetch_abort (struct pt_regs *pt_regs)
  */
 void do_data_abort (struct pt_regs *pt_regs)
 {
+
+#if __LINUX_ARM_ARCH__ <= 7
 	u32 far;
 
 	asm volatile ("mrc     p15, 0, %0, c6, c0, 0" : "=r" (far) : : "cc");
@@ -128,6 +135,7 @@ void do_data_abort (struct pt_regs *pt_regs)
 	printf("unable to handle %s at address 0x%08x\n",
 			far < PAGE_SIZE ? "NULL pointer dereference" :
 			"paging request", far);
+#endif
 
 	do_exception(pt_regs);
 }
@@ -156,6 +164,45 @@ void do_irq (struct pt_regs *pt_regs)
 	do_exception(pt_regs);
 }
 
+#ifdef CONFIG_CPU_64v8
+void do_bad_sync(struct pt_regs *pt_regs)
+{
+	printf("bad sync\n");
+	do_exception(pt_regs);
+}
+
+void do_bad_irq(struct pt_regs *pt_regs)
+{
+	printf("bad irq\n");
+	do_exception(pt_regs);
+}
+
+void do_bad_fiq(struct pt_regs *pt_regs)
+{
+	printf("bad fiq\n");
+	do_exception(pt_regs);
+}
+
+void do_bad_error(struct pt_regs *pt_regs)
+{
+	printf("bad error\n");
+	do_exception(pt_regs);
+}
+
+void do_sync(struct pt_regs *pt_regs)
+{
+	printf("sync exception\n");
+	do_exception(pt_regs);
+}
+
+
+void do_error(struct pt_regs *pt_regs)
+{
+	printf("error exception\n");
+	do_exception(pt_regs);
+}
+#endif
+
 extern volatile int arm_ignore_data_abort;
 extern volatile int arm_data_abort_occurred;
 
diff --git a/arch/arm/cpu/lowlevel_64.S b/arch/arm/cpu/lowlevel_64.S
new file mode 100644
index 0000000000..4850895169
--- /dev/null
+++ b/arch/arm/cpu/lowlevel_64.S
@@ -0,0 +1,40 @@
+#include <linux/linkage.h>
+#include <init.h>
+#include <asm/system.h>
+
+.section ".text_bare_init_","ax"
+ENTRY(arm_cpu_lowlevel_init)
+	adr	x0, vectors
+	mrs	x1, CurrentEL
+	cmp	x1, #0xC		/* Check EL3 state */
+	b.eq	1f
+	cmp	x1, #0x8		/* Check EL2 state */
+	b.eq	2f
+	cmp	x1, #0x4		/* Check EL1 state */
+	b.eq	3f
+
+1:
+	msr	vbar_el3, x0
+	mov	x0, #1			/* Non-Secure EL0/1 */
+	orr	x0, x0, #(1 << 10)	/* 64-bit EL2 */
+	msr	scr_el3, x0
+	msr	cptr_el3, xzr
+	b	done
+
+2:
+	msr	vbar_el2, x0
+	mov	x0, #0x33ff		/* Enable FP/SIMD */
+	msr	cptr_el2, x0
+	b	done
+
+
+3:
+	msr	vbar_el1, x0
+	mov	x0, #(3 << 20)		/* Enable FP/SIMD */
+	msr	cpacr_el1, x0
+	b	done
+
+done:
+	ret
+
+ENDPROC(arm_cpu_lowlevel_init)
diff --git a/arch/arm/cpu/mmu.h b/arch/arm/cpu/mmu.h
index 79ebc80d7d..186d408ead 100644
--- a/arch/arm/cpu/mmu.h
+++ b/arch/arm/cpu/mmu.h
@@ -1,6 +1,60 @@
 #ifndef __ARM_MMU_H
 #define __ARM_MMU_H
 
+#ifdef CONFIG_CPU_64v8
+
+#define TCR_FLAGS		(TCR_TG0_4K | \
+		TCR_SHARED_OUTER | \
+		TCR_SHARED_INNER | \
+		TCR_IRGN_WBWA | \
+		TCR_ORGN_WBWA | \
+		TCR_T0SZ(BITS_PER_VA))
+
+#ifndef __ASSEMBLY__
+
+static inline void set_ttbr_tcr_mair(int el, uint64_t table, uint64_t tcr, uint64_t attr)
+{
+	asm volatile("dsb sy");
+	if (el == 1) {
+		asm volatile("msr ttbr0_el1, %0" : : "r" (table) : "memory");
+		asm volatile("msr tcr_el1, %0" : : "r" (tcr) : "memory");
+		asm volatile("msr mair_el1, %0" : : "r" (attr) : "memory");
+	} else if (el == 2) {
+		asm volatile("msr ttbr0_el2, %0" : : "r" (table) : "memory");
+		asm volatile("msr tcr_el2, %0" : : "r" (tcr) : "memory");
+		asm volatile("msr mair_el2, %0" : : "r" (attr) : "memory");
+	} else if (el == 3) {
+		asm volatile("msr ttbr0_el3, %0" : : "r" (table) : "memory");
+		asm volatile("msr tcr_el3, %0" : : "r" (tcr) : "memory");
+		asm volatile("msr mair_el3, %0" : : "r" (attr) : "memory");
+	} else {
+		hang();
+	}
+	asm volatile("isb");
+}
+
+static inline uint64_t get_ttbr(int el)
+{
+	uint64_t val;
+	if (el == 1) {
+		asm volatile("mrs %0, ttbr0_el1" : "=r" (val));
+	} else if (el == 2) {
+		asm volatile("mrs %0, ttbr0_el2" : "=r" (val));
+	} else if (el == 3) {
+		asm volatile("mrs %0, ttbr0_el3" : "=r" (val));
+	} else {
+		hang();
+	}
+
+	return val;
+}
+
+void mmu_early_enable(uint64_t membase, uint64_t memsize, uint64_t _ttb);
+
+#endif
+
+#endif /* CONFIG_CPU_64v8 */
+
 #ifdef CONFIG_MMU
 void __mmu_cache_on(void);
 void __mmu_cache_off(void);
diff --git a/arch/arm/cpu/mmu_64.c b/arch/arm/cpu/mmu_64.c
new file mode 100644
index 0000000000..bfd80c0913
--- /dev/null
+++ b/arch/arm/cpu/mmu_64.c
@@ -0,0 +1,331 @@
+/*
+ * Copyright (c) 2009-2013 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ * Copyright (c) 2016 Raphaël Poggi <poggi.raph@gmail.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#define pr_fmt(fmt)	"mmu: " fmt
+
+#include <common.h>
+#include <dma-dir.h>
+#include <init.h>
+#include <mmu.h>
+#include <errno.h>
+#include <linux/sizes.h>
+#include <asm/memory.h>
+#include <asm/barebox-arm.h>
+#include <asm/system.h>
+#include <asm/cache.h>
+#include <memory.h>
+#include <asm/system_info.h>
+
+#include "mmu.h"
+
+static uint64_t *ttb;
+static int free_idx;
+
+static void arm_mmu_not_initialized_error(void)
+{
+	/*
+	 * This means:
+	 * - one of the MMU functions like dma_alloc_coherent
+	 *   or remap_range is called too early, before the MMU is initialized
+	 * - Or the MMU initialization has failed earlier
+	 */
+	panic("MMU not initialized\n");
+}
+
+
+/*
+ * Do it the simple way for now and invalidate the entire
+ * tlb
+ */
+static inline void tlb_invalidate(void)
+{
+	unsigned int el = current_el();
+
+	dsb();
+
+	if (el == 1)
+		__asm__ __volatile__("tlbi vmalle1\n\t" : : : "memory");
+	else if (el == 2)
+		__asm__ __volatile__("tlbi alle2\n\t" : : : "memory");
+	else if (el == 3)
+		__asm__ __volatile__("tlbi alle3\n\t" : : : "memory");
+
+	dsb();
+	isb();
+}
+
+static int level2shift(int level)
+{
+	/* Page is 12 bits wide, every level translates 9 bits */
+	return (12 + 9 * (3 - level));
+}
+
+static uint64_t level2mask(int level)
+{
+	uint64_t mask = -EINVAL;
+
+	if (level == 1)
+		mask = L1_ADDR_MASK;
+	else if (level == 2)
+		mask = L2_ADDR_MASK;
+	else if (level == 3)
+		mask = L3_ADDR_MASK;
+
+	return mask;
+}
+
+static int pte_type(uint64_t *pte)
+{
+	return *pte & PMD_TYPE_MASK;
+}
+
+static void set_table(uint64_t *pt, uint64_t *table_addr)
+{
+	uint64_t val;
+
+	val = PMD_TYPE_TABLE | (uint64_t)table_addr;
+	*pt = val;
+}
+
+static uint64_t *create_table(void)
+{
+	uint64_t *new_table = ttb + free_idx * GRANULE_SIZE;
+
+	/* Mark all entries as invalid */
+	memset(new_table, 0, GRANULE_SIZE);
+
+	free_idx++;
+
+	return new_table;
+}
+
+static uint64_t *get_level_table(uint64_t *pte)
+{
+	uint64_t *table = (uint64_t *)(*pte & XLAT_ADDR_MASK);
+
+	if (pte_type(pte) != PMD_TYPE_TABLE) {
+		table = create_table();
+		set_table(pte, table);
+	}
+
+	return table;
+}
+
+static uint64_t *find_pte(uint64_t addr)
+{
+	uint64_t *pte;
+	uint64_t block_shift;
+	uint64_t idx;
+	int i;
+
+	pte = ttb;
+
+	for (i = 1; i < 4; i++) {
+		block_shift = level2shift(i);
+		idx = (addr & level2mask(i)) >> block_shift;
+		pte += idx;
+
+		if ((pte_type(pte) != PMD_TYPE_TABLE) || (block_shift <= GRANULE_SIZE_SHIFT))
+			break;
+		else
+			pte = (uint64_t *)(*pte & XLAT_ADDR_MASK);
+	}
+
+	return pte;
+}
+
+static void map_region(uint64_t virt, uint64_t phys, uint64_t size, uint64_t attr)
+{
+	uint64_t block_size;
+	uint64_t block_shift;
+	uint64_t *pte;
+	uint64_t idx;
+	uint64_t addr;
+	uint64_t *table;
+	int level;
+
+	if (!ttb)
+		arm_mmu_not_initialized_error();
+
+	addr = virt;
+
+	attr &= ~(PMD_TYPE_SECT);
+
+	while (size) {
+		table = ttb;
+		for (level = 1; level < 4; level++) {
+			block_shift = level2shift(level);
+			idx = (addr & level2mask(level)) >> block_shift;
+			block_size = (1 << block_shift);
+
+			pte = table + idx;
+
+			if (level == 3)
+				attr |= PTE_TYPE_PAGE;
+			else
+				attr |= PMD_TYPE_SECT;
+
+			if (size >= block_size && IS_ALIGNED(addr, block_size)) {
+				*pte = phys | attr;
+				addr += block_size;
+				phys += block_size;
+				size -= block_size;
+				break;
+
+			}
+
+			table = get_level_table(pte);
+		}
+
+	}
+}
+
+static void create_sections(uint64_t virt, uint64_t phys, uint64_t size_m, uint64_t flags)
+{
+
+	map_region(virt, phys, size_m, flags);
+	tlb_invalidate();
+}
+
+void *map_io_sections(unsigned long phys, void *_start, size_t size)
+{
+
+	map_region((uint64_t)_start, phys, (uint64_t)size, UNCACHED_MEM);
+
+	tlb_invalidate();
+	return _start;
+}
+
+
+int arch_remap_range(void *_start, size_t size, unsigned flags)
+{
+	map_region((uint64_t)_start, (uint64_t)_start, (uint64_t)size, flags);
+	tlb_invalidate();
+
+	return 0;
+}
+
+/*
+ * Prepare MMU for usage enable it.
+ */
+static int mmu_init(void)
+{
+	struct memory_bank *bank;
+
+	if (list_empty(&memory_banks))
+		/*
+		 * If you see this it means you have no memory registered.
+		 * This can be done either with arm_add_mem_device() in an
+		 * initcall prior to mmu_initcall or via devicetree in the
+		 * memory node.
+		 */
+		panic("MMU: No memory bank found! Cannot continue\n");
+
+	if (get_cr() & CR_M) {
+		ttb = (uint64_t *)get_ttbr(current_el());
+		if (!request_sdram_region("ttb", (unsigned long)ttb, SZ_16K))
+			/*
+			* This can mean that:
+			* - the early MMU code has put the ttb into a place
+			*   which we don't have inside our available memory
+			* - Somebody else has occupied the ttb region which means
+			*   the ttb will get corrupted.
+			*/
+			pr_crit("Critical Error: Can't request SDRAM region for ttb at %p\n",
+				ttb);
+	} else {
+		ttb = memalign(GRANULE_SIZE, SZ_16K);
+		free_idx = 1;
+
+		memset(ttb, 0, GRANULE_SIZE);
+
+		set_ttbr_tcr_mair(current_el(), (uint64_t)ttb, TCR_FLAGS, UNCACHED_MEM);
+	}
+
+	pr_debug("ttb: 0x%p\n", ttb);
+
+	/* create a flat mapping using 1MiB sections */
+	create_sections(0, 0, GRANULE_SIZE, UNCACHED_MEM);
+
+	/* Map sdram cached. */
+	for_each_memory_bank(bank)
+		create_sections(bank->start, bank->start, bank->size, CACHED_MEM);
+
+	return 0;
+}
+mmu_initcall(mmu_init);
+
+void mmu_enable(void)
+{
+	if (!ttb)
+		arm_mmu_not_initialized_error();
+
+	if (!(get_cr() & CR_M)) {
+
+		isb();
+		set_cr(get_cr() | CR_M | CR_C | CR_I);
+	}
+}
+
+void mmu_disable(void)
+{
+	unsigned int cr;
+
+	if (!ttb)
+		arm_mmu_not_initialized_error();
+
+	cr = get_cr();
+	cr &= ~(CR_M | CR_C | CR_I);
+
+	tlb_invalidate();
+
+	dsb();
+	isb();
+
+	set_cr(cr);
+
+	dsb();
+	isb();
+}
+
+void mmu_early_enable(uint64_t membase, uint64_t memsize, uint64_t _ttb)
+{
+	ttb = (uint64_t *)_ttb;
+
+	memset(ttb, 0, GRANULE_SIZE);
+	free_idx = 1;
+
+	set_ttbr_tcr_mair(current_el(), (uint64_t)ttb, TCR_FLAGS, UNCACHED_MEM);
+
+	create_sections(0, 0, 4096, UNCACHED_MEM);
+
+	create_sections(membase, membase, memsize, CACHED_MEM);
+
+	isb();
+	set_cr(get_cr() | CR_M);
+}
+
+unsigned long virt_to_phys(volatile void *virt)
+{
+	return (unsigned long)virt;
+}
+
+void *phys_to_virt(unsigned long phys)
+{
+	return (void *)phys;
+}
diff --git a/arch/arm/cpu/setupc_64.S b/arch/arm/cpu/setupc_64.S
new file mode 100644
index 0000000000..3515854784
--- /dev/null
+++ b/arch/arm/cpu/setupc_64.S
@@ -0,0 +1,18 @@
+#include <linux/linkage.h>
+#include <asm/sections.h>
+
+.section .text.setupc
+
+/*
+ * setup_c: clear bss
+ */
+ENTRY(setup_c)
+	mov	x15, x30
+	ldr	x0, =__bss_start
+	mov	x1, #0
+	ldr	x2, =__bss_stop
+	sub	x2, x2, x0
+	bl	memset			/* clear bss */
+	mov	x30, x15
+	ret
+ENDPROC(setup_c)