arm64: Turn architectural register accessors into inline functions

Accesses to architectural registers should be really fast -- they're
just registers, after all. In fact, the arm64 architecture uses them for
some timing-senstive uses like the architectural timer. A read should be:
one instruction, no data dependencies, done.

However, our current coreboot framework wraps each of these accesses
into a separate function. Suddenly you have to spill registers on a
stack, make a function call, move your stack pointer, etc. When running
without MMU this adds a significant enough delay to cause timing
problems when bitbanging a UART on SDM845.

This patch replaces all those existing functions with static inline
definitions in the header so they will get reduced to a single
instruction as they should be. Also use some macros to condense the code
a little since they're all so regular, which should make it easier to
add more in the future. This patch also expands all the data types to
uint64_t since that's what the actual assembly instruction accesses,
even if the register itself only has 32 bits (the others will be ignored
by the processor and set to 0 on read). Arm regularly expands registers
as they add new bit fields to them with newer iterations of the
architecture anyway, so this just prepares us for the inevitable.

Change-Id: I2c41cc3ce49ee26bf12cd34e3d0509d8e61ffc63
Signed-off-by: Julius Werner <jwerner@chromium.org>
Reviewed-on: https://review.coreboot.org/27881
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Paul Menzel <paulepanter@users.sourceforge.net>
Reviewed-by: Patrick Rudolph <patrick.rudolph@9elements.com>
Reviewed-by: Aaron Durbin <adurbin@chromium.org>

10 files changed, 244 insertions, 1536 deletions

diff --git a/src/arch/arm64/armv8/Makefile.inc b/src/arch/arm64/armv8/Makefile.inc
index db7bd33793..44ebdef815 100644
--- a/src/arch/arm64/armv8/Makefile.inc
+++ b/src/arch/arm64/armv8/Makefile.inc
@@ -15,8 +15,6 @@
 ##
 ################################################################################
 
-subdirs-y += lib/
-
 ifeq ($(CONFIG_ARCH_ARMV8_EXTENSION),0)
 march = armv8-a
 else
diff --git a/src/arch/arm64/armv8/cache.c b/src/arch/arm64/armv8/cache.c
index 2e63db3bbb..46dc85958d 100644
--- a/src/arch/arm64/armv8/cache.c
+++ b/src/arch/arm64/armv8/cache.c
@@ -37,14 +37,6 @@
 #include <arch/lib_helpers.h>
 #include <program_loading.h>
 
-void tlb_invalidate_all(void)
-{
-	 /* TLBIALL includes dTLB and iTLB on systems that have them. */
-	tlbiall_el3();
-	dsb();
-	isb();
-}
-
 unsigned int dcache_line_bytes(void)
 {
 	uint32_t ctr_el0;
diff --git a/src/arch/arm64/armv8/exception.c b/src/arch/arm64/armv8/exception.c
index 924c344e13..e32c79a85a 100644
--- a/src/arch/arm64/armv8/exception.c
+++ b/src/arch/arm64/armv8/exception.c
@@ -78,9 +78,9 @@ static void print_regs(struct exc_state *exc_state)
 	struct elx_state *elx = &exc_state->elx;
 	struct regs *regs = &exc_state->regs;
 
-	printk(BIOS_DEBUG, "ELR = 0x%016llx         ESR = 0x%08x\n",
+	printk(BIOS_DEBUG, "ELR = 0x%016llx         ESR = 0x%08llx\n",
 	       elx->elr, raw_read_esr_el3());
-	printk(BIOS_DEBUG, "FAR = 0x%016llx        SPSR = 0x%08x\n",
+	printk(BIOS_DEBUG, "FAR = 0x%016llx        SPSR = 0x%08llx\n",
 	       raw_read_far_el3(), raw_read_spsr_el3());
 	for (i = 0; i < 30; i += 2) {
 		printk(BIOS_DEBUG,
diff --git a/src/arch/arm64/armv8/lib/Makefile.inc b/src/arch/arm64/armv8/lib/Makefile.inc
deleted file mode 100644
index 8fc44de5b6..0000000000
--- a/src/arch/arm64/armv8/lib/Makefile.inc
+++ /dev/null
@@ -1,36 +0,0 @@
-################################################################################
-## This file is part of the coreboot project.
-##
-## Copyright (C) 2014 Google Inc
-##
-## This program is free software; you can redistribute it and/or
-## modify it under the terms of the GNU General Public License as
-## published by the Free Software Foundation; version 2 of
-## the License.
-##
-## 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.
-##
-################################################################################
-
-lib_access = pstate.c sysctrl.c cache.c tlb.c
-
-ifeq ($(CONFIG_ARCH_BOOTBLOCK_ARMV8_64),y)
-decompressor-y += $(lib_access)
-bootblock-y += $(lib_access)
-endif
-
-ifeq ($(CONFIG_ARCH_VERSTAGE_ARMV8_64),y)
-verstage-y += $(lib_access)
-endif
-
-ifeq ($(CONFIG_ARCH_ROMSTAGE_ARMV8_64),y)
-romstage-y += $(lib_access)
-endif
-
-ifeq ($(CONFIG_ARCH_RAMSTAGE_ARMV8_64),y)
-ramstage-y += $(lib_access)
-
-endif
diff --git a/src/arch/arm64/armv8/lib/cache.c b/src/arch/arm64/armv8/lib/cache.c
deleted file mode 100644
index 0c621ef96d..0000000000
--- a/src/arch/arm64/armv8/lib/cache.c
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * This file is part of the coreboot project.
- *
- * Copyright (C) 2014 Google Inc
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; version 2 of
- * the License.
- *
- * 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.
- *
- * cache.c: Cache Maintenance Instructions
- * Reference: ARM Architecture Reference Manual, ARMv8-A edition
- */
-
-#include <stdint.h>
-
-#include <arch/lib_helpers.h>
-
-void dccisw(uint64_t cisw)
-{
-	__asm__ __volatile__("dc cisw, %0\n\t" : : "r" (cisw) : "memory");
-}
-
-void dccivac(uint64_t civac)
-{
-	__asm__ __volatile__("dc civac, %0\n\t" : : "r" (civac) : "memory");
-}
-
-void dccsw(uint64_t csw)
-{
-	__asm__ __volatile__("dc csw, %0\n\t" : : "r" (csw) : "memory");
-}
-
-void dccvac(uint64_t cvac)
-{
-	__asm__ __volatile__("dc cvac, %0\n\t" : : "r" (cvac) : "memory");
-}
-
-void dccvau(uint64_t cvau)
-{
-	__asm__ __volatile__("dc cvau, %0\n\t" : : "r" (cvau) : "memory");
-}
-
-void dcisw(uint64_t isw)
-{
-	__asm__ __volatile__("dc isw, %0\n\t" : : "r" (isw) : "memory");
-}
-
-void dcivac(uint64_t ivac)
-{
-	__asm__ __volatile__("dc ivac, %0\n\t" : : "r" (ivac) : "memory");
-}
-
-void dczva(uint64_t zva)
-{
-	__asm__ __volatile__("dc zva, %0\n\t" : : "r" (zva) : "memory");
-}
-
-void iciallu(void)
-{
-	__asm__ __volatile__("ic iallu\n\t" : : : "memory");
-}
-
-void icialluis(void)
-{
-	__asm__ __volatile__("ic ialluis\n\t" : : : "memory");
-}
-
-void icivau(uint64_t ivau)
-{
-	__asm__ __volatile__("ic ivau, %0\n\t" : : "r" (ivau) : "memory");
-}
diff --git a/src/arch/arm64/armv8/lib/pstate.c b/src/arch/arm64/armv8/lib/pstate.c
deleted file mode 100644
index 631974f6e0..0000000000
--- a/src/arch/arm64/armv8/lib/pstate.c
+++ /dev/null
@@ -1,392 +0,0 @@
-/*
- * This file is part of the coreboot project.
- *
- * Copyright (C) 2014 Google Inc
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; version 2 of
- * the License.
- *
- * 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.
- *
- * Reference: ARM Architecture Reference Manual, ARMv8-A edition
- * pstate.c: This file defines all the library functions for accessing
- * PSTATE and special purpose registers
- */
-
-#include <stdint.h>
-
-#include <arch/lib_helpers.h>
-
-/* CurrentEL */
-uint32_t raw_read_current_el(void)
-{
-	uint64_t current_el;
-
-	__asm__ __volatile__("mrs %0, CurrentEL\n\t" : "=r" (current_el) :  : "memory");
-
-	return current_el;
-}
-
-/* DAIF */
-uint32_t raw_read_daif(void)
-{
-	uint64_t daif;
-
-	__asm__ __volatile__("mrs %0, DAIF\n\t" : "=r" (daif) :  : "memory");
-
-	return daif;
-}
-
-void raw_write_daif(uint32_t daif)
-{
-	__asm__ __volatile__("msr DAIF, %0\n\t" : : "r" ((uint64_t)daif) : "memory");
-}
-
-void enable_debug_exceptions(void)
-{
-	__asm__ __volatile__("msr DAIFClr, %0\n\t" : : "i" (DAIF_DBG_BIT)  : "memory");
-}
-
-void enable_serror_exceptions(void)
-{
-	__asm__ __volatile__("msr DAIFClr, %0\n\t" : : "i" (DAIF_ABT_BIT)  : "memory");
-}
-
-void enable_irq(void)
-{
-	__asm__ __volatile__("msr DAIFClr, %0\n\t" : : "i" (DAIF_IRQ_BIT)  : "memory");
-}
-
-void enable_fiq(void)
-{
-	__asm__ __volatile__("msr DAIFClr, %0\n\t" : : "i" (DAIF_FIQ_BIT)  : "memory");
-}
-
-void disable_debug_exceptions(void)
-{
-	__asm__ __volatile__("msr DAIFSet, %0\n\t" : : "i" (DAIF_DBG_BIT)  : "memory");
-}
-
-void disable_serror_exceptions(void)
-{
-	__asm__ __volatile__("msr DAIFSet, %0\n\t" : : "i" (DAIF_ABT_BIT)  : "memory");
-}
-
-void disable_irq(void)
-{
-	__asm__ __volatile__("msr DAIFSet, %0\n\t" : : "i" (DAIF_IRQ_BIT)  : "memory");
-}
-
-void disable_fiq(void)
-{
-	__asm__ __volatile__("msr DAIFSet, %0\n\t" : : "i" (DAIF_FIQ_BIT)  : "memory");
-}
-
-/* DLR_EL0 */
-uint64_t raw_read_dlr_el0(void)
-{
-	uint64_t dlr_el0;
-
-	__asm__ __volatile__("mrs %0, DLR_EL0\n\t" : "=r" (dlr_el0) :  : "memory");
-
-	return dlr_el0;
-}
-void raw_write_dlr_el0(uint64_t dlr_el0)
-{
-	__asm__ __volatile__("msr DLR_EL0, %0\n\t" : : "r" (dlr_el0) : "memory");
-}
-
-/* DSPSR_EL0 */
-uint64_t raw_read_dspsr_el0(void)
-{
-	uint64_t dspsr_el0;
-
-	__asm__ __volatile__("mrs %0, DSPSR_EL0\n\t" : "=r" (dspsr_el0) :  : "memory");
-
-	return dspsr_el0;
-}
-void raw_write_dspsr_el0(uint64_t dspsr_el0)
-{
-	__asm__ __volatile__("msr DSPSR_EL0, %0\n\t" : : "r" (dspsr_el0) : "memory");
-}
-
-/* ELR */
-uint64_t raw_read_elr_el1(void)
-{
-	uint64_t elr_el1;
-
-	__asm__ __volatile__("mrs %0, ELR_EL1\n\t" : "=r" (elr_el1) :  : "memory");
-
-	return elr_el1;
-}
-
-void raw_write_elr_el1(uint64_t elr_el1)
-{
-	__asm__ __volatile__("msr ELR_EL1, %0\n\t" : : "r" (elr_el1) : "memory");
-}
-
-uint64_t raw_read_elr_el2(void)
-{
-	uint64_t elr_el2;
-
-	__asm__ __volatile__("mrs %0, ELR_EL2\n\t" : "=r" (elr_el2) :  : "memory");
-
-	return elr_el2;
-}
-
-void raw_write_elr_el2(uint64_t elr_el2)
-{
-	__asm__ __volatile__("msr ELR_EL2, %0\n\t" : : "r" (elr_el2) : "memory");
-}
-
-uint64_t raw_read_elr_el3(void)
-{
-	uint64_t elr_el3;
-
-	__asm__ __volatile__("mrs %0, ELR_EL3\n\t" : "=r" (elr_el3) :  : "memory");
-
-	return elr_el3;
-}
-
-void raw_write_elr_el3(uint64_t elr_el3)
-{
-	__asm__ __volatile__("msr ELR_EL3, %0\n\t" : : "r" (elr_el3) : "memory");
-}
-
-/* FPCR */
-uint32_t raw_read_fpcr(void)
-{
-	uint64_t fpcr;
-
-	__asm__ __volatile__("mrs %0, FPCR\n\t" : "=r" (fpcr) :  : "memory");
-
-	return fpcr;
-}
-
-void raw_write_fpcr(uint32_t fpcr)
-{
-	__asm__ __volatile__("msr FPCR, %0\n\t" : : "r" ((uint64_t)fpcr) : "memory");
-}
-
-/* FPSR */
-uint32_t raw_read_fpsr(void)
-{
-	uint64_t fpsr;
-
-	__asm__ __volatile__("mrs %0, FPSR\n\t" : "=r" (fpsr) :  : "memory");
-
-	return fpsr;
-}
-
-void raw_write_fpsr(uint32_t fpsr)
-{
-	__asm__ __volatile__("msr FPSR, %0\n\t" : : "r" ((uint64_t)fpsr) : "memory");
-}
-
-/* NZCV */
-uint32_t raw_read_nzcv(void)
-{
-	uint64_t nzcv;
-
-	__asm__ __volatile__("mrs %0, NZCV\n\t" : "=r" (nzcv) :  : "memory");
-
-	return nzcv;
-}
-
-void raw_write_nzcv(uint32_t nzcv)
-{
-	__asm__ __volatile__("msr NZCV, %0\n\t" : : "r" ((uint64_t)nzcv) : "memory");
-}
-
-/* SP */
-uint64_t raw_read_sp_el0(void)
-{
-	uint64_t sp_el0;
-
-	__asm__ __volatile__("mrs %0, SP_EL0\n\t" : "=r" (sp_el0) :  : "memory");
-
-	return sp_el0;
-}
-
-void raw_write_sp_el0(uint64_t sp_el0)
-{
-	__asm__ __volatile__("msr SP_EL0, %0\n\t" : : "r" (sp_el0) : "memory");
-}
-
-uint64_t raw_read_sp_el1(void)
-{
-	uint64_t sp_el1;
-
-	__asm__ __volatile__("mrs %0, SP_EL1\n\t" : "=r" (sp_el1) :  : "memory");
-
-	return sp_el1;
-}
-
-void raw_write_sp_el1(uint64_t sp_el1)
-{
-	__asm__ __volatile__("msr SP_EL1, %0\n\t" : : "r" (sp_el1) : "memory");
-}
-
-uint64_t raw_read_sp_el2(void)
-{
-	uint64_t sp_el2;
-
-	__asm__ __volatile__("mrs %0, SP_EL2\n\t" : "=r" (sp_el2) :  : "memory");
-
-	return sp_el2;
-}
-
-void raw_write_sp_el2(uint64_t sp_el2)
-{
-	__asm__ __volatile__("msr SP_EL2, %0\n\t" : : "r" (sp_el2) : "memory");
-}
-
-/* SPSel */
-uint32_t raw_read_spsel(void)
-{
-	uint64_t spsel;
-
-	__asm__ __volatile__("mrs %0, SPSel\n\t" : "=r" (spsel) :  : "memory");
-
-	return spsel;
-}
-
-void raw_write_spsel(uint32_t spsel)
-{
-	__asm__ __volatile__("msr SPSel, %0\n\t" : : "r" ((uint64_t)spsel) : "memory");
-}
-
-uint64_t raw_read_sp_el3(void)
-{
-	uint64_t sp_el3;
-	uint32_t spsel;
-
-	spsel = raw_read_spsel();
-	if (!spsel)
-		raw_write_spsel(1);
-
-	__asm__ __volatile__("mov %0, sp\n\t" : "=r" (sp_el3) :  : "memory");
-
-	if (!spsel)
-		raw_write_spsel(spsel);
-
-	return sp_el3;
-}
-
-void raw_write_sp_el3(uint64_t sp_el3)
-{
-	uint32_t spsel;
-
-	spsel = raw_read_spsel();
-	if (!spsel)
-		raw_write_spsel(1);
-
-	__asm__ __volatile__("mov sp, %0\n\t" : "=r" (sp_el3) :  : "memory");
-
-	if (!spsel)
-		raw_write_spsel(spsel);
-}
-
-/* SPSR */
-uint32_t raw_read_spsr_abt(void)
-{
-	uint64_t spsr_abt;
-
-	__asm__ __volatile__("mrs %0, SPSR_abt\n\t" : "=r" (spsr_abt) :  : "memory");
-
-	return spsr_abt;
-}
-
-void raw_write_spsr_abt(uint32_t spsr_abt)
-{
-	__asm__ __volatile__("msr SPSR_abt, %0\n\t" : : "r" ((uint64_t)spsr_abt) : "memory");
-}
-
-uint32_t raw_read_spsr_el1(void)
-{
-	uint64_t spsr_el1;
-
-	__asm__ __volatile__("mrs %0, SPSR_EL1\n\t" : "=r" (spsr_el1) :  : "memory");
-
-	return spsr_el1;
-}
-
-void raw_write_spsr_el1(uint32_t spsr_el1)
-{
-	__asm__ __volatile__("msr SPSR_EL1, %0\n\t" : : "r" ((uint64_t)spsr_el1) : "memory");
-}
-
-uint32_t raw_read_spsr_el2(void)
-{
-	uint64_t spsr_el2;
-
-	__asm__ __volatile__("mrs %0, SPSR_EL2\n\t" : "=r" (spsr_el2) :  : "memory");
-
-	return spsr_el2;
-}
-
-void raw_write_spsr_el2(uint32_t spsr_el2)
-{
-	__asm__ __volatile__("msr SPSR_EL2, %0\n\t" : : "r" ((uint64_t)spsr_el2) : "memory");
-}
-
-uint32_t raw_read_spsr_el3(void)
-{
-	uint64_t spsr_el3;
-
-	__asm__ __volatile__("mrs %0, SPSR_EL3\n\t" : "=r" (spsr_el3) :  : "memory");
-
-	return spsr_el3;
-}
-
-void raw_write_spsr_el3(uint32_t spsr_el3)
-{
-	__asm__ __volatile__("msr SPSR_EL3, %0\n\t" : : "r" ((uint64_t)spsr_el3) : "memory");
-}
-
-uint32_t raw_read_spsr_fiq(void)
-{
-	uint64_t spsr_fiq;
-
-	__asm__ __volatile__("mrs %0, SPSR_fiq\n\t" : "=r" (spsr_fiq) :  : "memory");
-
-	return spsr_fiq;
-}
-
-void raw_write_spsr_fiq(uint32_t spsr_fiq)
-{
-	__asm__ __volatile__("msr SPSR_fiq, %0\n\t" : : "r" ((uint64_t)spsr_fiq) : "memory");
-}
-
-uint32_t raw_read_spsr_irq(void)
-{
-	uint64_t spsr_irq;
-
-	__asm__ __volatile__("mrs %0, SPSR_irq\n\t" : "=r" (spsr_irq) :  : "memory");
-
-	return spsr_irq;
-}
-
-void raw_write_spsr_irq(uint32_t spsr_irq)
-{
-	__asm__ __volatile__("msr SPSR_irq, %0\n\t" : : "r" ((uint64_t)spsr_irq) : "memory");
-}
-
-uint32_t raw_read_spsr_und(void)
-{
-	uint64_t spsr_und;
-
-	__asm__ __volatile__("mrs %0, SPSR_und\n\t" : "=r" (spsr_und) :  : "memory");
-
-	return spsr_und;
-}
-
-void raw_write_spsr_und(uint32_t spsr_und)
-{
-	__asm__ __volatile__("msr SPSR_und, %0\n\t" : : "r" ((uint64_t)spsr_und) : "memory");
-}
diff --git a/src/arch/arm64/armv8/lib/sysctrl.c b/src/arch/arm64/armv8/lib/sysctrl.c
deleted file mode 100644
index 3e73c30173..0000000000
--- a/src/arch/arm64/armv8/lib/sysctrl.c
+++ /dev/null
@@ -1,777 +0,0 @@
-/*
- * This file is part of the coreboot project.
- *
- * Copyright (C) 2014 Google Inc
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; version 2 of
- * the License.
- *
- * 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.
- *
- * Reference: ARM Architecture Reference Manual, ARMv8-A edition
- * sysctrl.c: This file defines all the library functions for accessing system
- * control registers in Aarch64
- */
-
-#include <stdint.h>
-
-#include <arch/lib_helpers.h>
-
-/* ACTLR */
-uint32_t raw_read_actlr_el1(void)
-{
-	uint64_t actlr_el1;
-
-	__asm__ __volatile__("mrs %0, ACTLR_EL1\n\t" : "=r" (actlr_el1) :  : "memory");
-
-	return actlr_el1;
-}
-
-void raw_write_actlr_el1(uint32_t actlr_el1)
-{
-	__asm__ __volatile__("msr ACTLR_EL1, %0\n\t" : : "r" ((uint64_t)actlr_el1) : "memory");
-}
-
-uint32_t raw_read_actlr_el2(void)
-{
-	uint64_t actlr_el2;
-
-	__asm__ __volatile__("mrs %0, ACTLR_EL2\n\t" : "=r" (actlr_el2) :  : "memory");
-
-	return actlr_el2;
-}
-
-void raw_write_actlr_el2(uint32_t actlr_el2)
-{
-	__asm__ __volatile__("msr ACTLR_EL2, %0\n\t" : : "r" ((uint64_t)actlr_el2) : "memory");
-}
-
-uint32_t raw_read_actlr_el3(void)
-{
-	uint64_t actlr_el3;
-
-	__asm__ __volatile__("mrs %0, ACTLR_EL3\n\t" : "=r" (actlr_el3) :  : "memory");
-
-	return actlr_el3;
-}
-
-void raw_write_actlr_el3(uint32_t actlr_el3)
-{
-	__asm__ __volatile__("msr ACTLR_EL3, %0\n\t" : : "r" ((uint64_t)actlr_el3) : "memory");
-}
-
-/* AFSR0 */
-uint32_t raw_read_afsr0_el1(void)
-{
-	uint64_t afsr0_el1;
-
-	__asm__ __volatile__("mrs %0, AFSR0_EL1\n\t" : "=r" (afsr0_el1) :  : "memory");
-
-	return afsr0_el1;
-}
-
-void raw_write_afsr0_el1(uint32_t afsr0_el1)
-{
-	__asm__ __volatile__("msr AFSR0_EL1, %0\n\t" : : "r" ((uint64_t)afsr0_el1) : "memory");
-}
-
-uint32_t raw_read_afsr0_el2(void)
-{
-	uint64_t afsr0_el2;
-
-	__asm__ __volatile__("mrs %0, AFSR0_EL2\n\t" : "=r" (afsr0_el2) :  : "memory");
-
-	return afsr0_el2;
-}
-
-void raw_write_afsr0_el2(uint32_t afsr0_el2)
-{
-	__asm__ __volatile__("msr AFSR0_EL2, %0\n\t" : : "r" ((uint64_t)afsr0_el2) : "memory");
-}
-
-uint32_t raw_read_afsr0_el3(void)
-{
-	uint64_t afsr0_el3;
-
-	__asm__ __volatile__("mrs %0, AFSR0_EL3\n\t" : "=r" (afsr0_el3) :  : "memory");
-
-	return afsr0_el3;
-}
-
-void raw_write_afsr0_el3(uint32_t afsr0_el3)
-{
-	__asm__ __volatile__("msr AFSR0_EL3, %0\n\t" : : "r" ((uint64_t)afsr0_el3) : "memory");
-}
-
-/* AFSR1 */
-uint32_t raw_read_afsr1_el1(void)
-{
-	uint64_t afsr1_el1;
-
-	__asm__ __volatile__("mrs %0, AFSR1_EL1\n\t" : "=r" (afsr1_el1) :  : "memory");
-
-	return afsr1_el1;
-}
-
-void raw_write_afsr1_el1(uint32_t afsr1_el1)
-{
-	__asm__ __volatile__("msr AFSR1_EL1, %0\n\t" : : "r" ((uint64_t)afsr1_el1) : "memory");
-}
-
-uint32_t raw_read_afsr1_el2(void)
-{
-	uint64_t afsr1_el2;
-
-	__asm__ __volatile__("mrs %0, AFSR1_EL2\n\t" : "=r" (afsr1_el2) :  : "memory");
-
-	return afsr1_el2;
-}
-
-void raw_write_afsr1_el2(uint32_t afsr1_el2)
-{
-	__asm__ __volatile__("msr AFSR1_EL2, %0\n\t" : : "r" ((uint64_t)afsr1_el2) : "memory");
-}
-
-uint32_t raw_read_afsr1_el3(void)
-{
-	uint64_t afsr1_el3;
-
-	__asm__ __volatile__("mrs %0, AFSR1_EL3\n\t" : "=r" (afsr1_el3) :  : "memory");
-
-	return afsr1_el3;
-}
-
-void raw_write_afsr1_el3(uint32_t afsr1_el3)
-{
-	__asm__ __volatile__("msr AFSR1_EL3, %0\n\t" : : "r" ((uint64_t)afsr1_el3) : "memory");
-}
-
-/* AIDR */
-uint32_t raw_read_aidr_el1(void)
-{
-	uint64_t aidr_el1;
-
-	__asm__ __volatile__("mrs %0, AIDR_EL1\n\t" : "=r" (aidr_el1) :  : "memory");
-
-	return aidr_el1;
-}
-
-/* AMAIR */
-uint64_t raw_read_amair_el1(void)
-{
-	uint64_t amair_el1;
-
-	__asm__ __volatile__("mrs %0, AMAIR_EL1\n\t" : "=r" (amair_el1) :  : "memory");
-
-	return amair_el1;
-}
-
-void raw_write_amair_el1(uint64_t amair_el1)
-{
-	__asm__ __volatile__("msr AMAIR_EL1, %0\n\t" : : "r" (amair_el1) : "memory");
-}
-
-uint64_t raw_read_amair_el2(void)
-{
-	uint64_t amair_el2;
-
-	__asm__ __volatile__("mrs %0, AMAIR_EL2\n\t" : "=r" (amair_el2) :  : "memory");
-
-	return amair_el2;
-}
-
-void raw_write_amair_el2(uint64_t amair_el2)
-{
-	__asm__ __volatile__("msr AMAIR_EL2, %0\n\t" : : "r" (amair_el2) : "memory");
-}
-
-uint64_t raw_read_amair_el3(void)
-{
-	uint64_t amair_el3;
-
-	__asm__ __volatile__("mrs %0, AMAIR_EL3\n\t" : "=r" (amair_el3) :  : "memory");
-
-	return amair_el3;
-}
-
-void raw_write_amair_el3(uint64_t amair_el3)
-{
-	__asm__ __volatile__("msr AMAIR_EL3, %0\n\t" : : "r" (amair_el3) : "memory");
-}
-
-/* CCSIDR */
-uint32_t raw_read_ccsidr_el1(void)
-{
-	uint64_t ccsidr_el1;
-
-	__asm__ __volatile__("mrs %0, CCSIDR_EL1\n\t" : "=r" (ccsidr_el1) :  : "memory");
-
-	return ccsidr_el1;
-}
-
-/* CLIDR */
-uint32_t raw_read_clidr_el1(void)
-{
-	uint64_t clidr_el1;
-
-	__asm__ __volatile__("mrs %0, CLIDR_EL1\n\t" : "=r" (clidr_el1) :  : "memory");
-
-	return clidr_el1;
-}
-
-/* CPACR */
-uint32_t raw_read_cpacr_el1(void)
-{
-	uint64_t cpacr_el1;
-
-	__asm__ __volatile__("mrs %0, CPACR_EL1\n\t" : "=r" (cpacr_el1) :  : "memory");
-
-	return cpacr_el1;
-}
-
-void raw_write_cpacr_el1(uint32_t cpacr_el1)
-{
-	__asm__ __volatile__("msr CPACR_EL1, %0\n\t" : : "r" ((uint64_t)cpacr_el1) : "memory");
-}
-
-/* CPTR */
-uint32_t raw_read_cptr_el2(void)
-{
-	uint64_t cptr_el2;
-
-	__asm__ __volatile__("mrs %0, CPTR_EL2\n\t" : "=r" (cptr_el2) :  : "memory");
-
-	return cptr_el2;
-}
-
-void raw_write_cptr_el2(uint32_t cptr_el2)
-{
-	__asm__ __volatile__("msr CPTR_EL2, %0\n\t" : : "r" ((uint64_t)cptr_el2) : "memory");
-}
-
-uint32_t raw_read_cptr_el3(void)
-{
-	uint64_t cptr_el3;
-
-	__asm__ __volatile__("mrs %0, CPTR_EL3\n\t" : "=r" (cptr_el3) :  : "memory");
-
-	return cptr_el3;
-}
-
-void raw_write_cptr_el3(uint32_t cptr_el3)
-{
-	__asm__ __volatile__("msr CPTR_EL3, %0\n\t" : : "r" ((uint64_t)cptr_el3) : "memory");
-}
-
-/* CSSELR */
-uint32_t raw_read_csselr_el1(void)
-{
-	uint64_t csselr_el1;
-
-	__asm__ __volatile__("mrs %0, CSSELR_EL1\n\t" : "=r" (csselr_el1) :  : "memory");
-
-	return csselr_el1;
-}
-
-void raw_write_csselr_el1(uint32_t csselr_el1)
-{
-	__asm__ __volatile__("msr CSSELR_EL1, %0\n\t" : : "r" ((uint64_t)csselr_el1) : "memory");
-}
-
-/* CTR */
-uint32_t raw_read_ctr_el0(void)
-{
-	uint64_t ctr_el0;
-
-	__asm__ __volatile__("mrs %0, CTR_EL0\n\t" : "=r" (ctr_el0) :  : "memory");
-
-	return ctr_el0;
-}
-
-/* ESR */
-uint32_t raw_read_esr_el1(void)
-{
-	uint64_t esr_el1;
-
-	__asm__ __volatile__("mrs %0, ESR_EL1\n\t" : "=r" (esr_el1) :  : "memory");
-
-	return esr_el1;
-}
-
-void raw_write_esr_el1(uint32_t esr_el1)
-{
-	__asm__ __volatile__("msr ESR_EL1, %0\n\t" : : "r" ((uint64_t)esr_el1) : "memory");
-}
-
-uint32_t raw_read_esr_el2(void)
-{
-	uint64_t esr_el2;
-
-	__asm__ __volatile__("mrs %0, ESR_EL2\n\t" : "=r" (esr_el2) :  : "memory");
-
-	return esr_el2;
-}
-
-void raw_write_esr_el2(uint32_t esr_el2)
-{
-	__asm__ __volatile__("msr ESR_EL2, %0\n\t" : : "r" ((uint64_t)esr_el2) : "memory");
-}
-
-uint32_t raw_read_esr_el3(void)
-{
-	uint64_t esr_el3;
-
-	__asm__ __volatile__("mrs %0, ESR_EL3\n\t" : "=r" (esr_el3) :  : "memory");
-
-	return esr_el3;
-}
-
-void raw_write_esr_el3(uint32_t esr_el3)
-{
-	__asm__ __volatile__("msr ESR_EL3, %0\n\t" : : "r" ((uint64_t)esr_el3) : "memory");
-}
-
-/* FAR */
-uint64_t raw_read_far_el1(void)
-{
-	uint64_t far_el1;
-
-	__asm__ __volatile__("mrs %0, FAR_EL1\n\t" : "=r" (far_el1) :  : "memory");
-
-	return far_el1;
-}
-
-void raw_write_far_el1(uint64_t far_el1)
-{
-	__asm__ __volatile__("msr FAR_EL1, %0\n\t" : : "r" (far_el1) : "memory");
-}
-
-uint64_t raw_read_far_el2(void)
-{
-	uint64_t far_el2;
-
-	__asm__ __volatile__("mrs %0, FAR_EL2\n\t" : "=r" (far_el2) :  : "memory");
-
-	return far_el2;
-}
-
-void raw_write_far_el2(uint64_t far_el2)
-{
-	__asm__ __volatile__("msr FAR_EL2, %0\n\t" : : "r" (far_el2) : "memory");
-}
-
-uint64_t raw_read_far_el3(void)
-{
-	uint64_t far_el3;
-
-	__asm__ __volatile__("mrs %0, FAR_EL3\n\t" : "=r" (far_el3) :  : "memory");
-
-	return far_el3;
-}
-
-void raw_write_far_el3(uint64_t far_el3)
-{
-	__asm__ __volatile__("msr FAR_EL3, %0\n\t" : : "r" (far_el3) : "memory");
-}
-
-/* HCR */
-uint64_t raw_read_hcr_el2(void)
-{
-	uint64_t hcr_el2;
-
-	__asm__ __volatile__("mrs %0, HCR_EL2\n\t" : "=r" (hcr_el2) :  : "memory");
-
-	return hcr_el2;
-}
-
-void raw_write_hcr_el2(uint64_t hcr_el2)
-{
-	__asm__ __volatile__("msr HCR_EL2, %0\n\t" : : "r" (hcr_el2) : "memory");
-}
-
-/* AA64PFR0 */
-uint64_t raw_read_aa64pfr0_el1(void)
-{
-	uint64_t aa64pfr0_el1;
-
-	__asm__ __volatile__("mrs %0, ID_AA64PFR0_EL1\n\t" : "=r" (aa64pfr0_el1) :  : "memory");
-
-	return aa64pfr0_el1;
-}
-
-/* MAIR */
-uint64_t raw_read_mair_el1(void)
-{
-	uint64_t mair_el1;
-
-	__asm__ __volatile__("mrs %0, MAIR_EL1\n\t" : "=r" (mair_el1) :  : "memory");
-
-	return mair_el1;
-}
-
-void raw_write_mair_el1(uint64_t mair_el1)
-{
-	__asm__ __volatile__("msr MAIR_EL1, %0\n\t" : : "r" (mair_el1) : "memory");
-}
-
-uint64_t raw_read_mair_el2(void)
-{
-	uint64_t mair_el2;
-
-	__asm__ __volatile__("mrs %0, MAIR_EL2\n\t" : "=r" (mair_el2) :  : "memory");
-
-	return mair_el2;
-}
-
-void raw_write_mair_el2(uint64_t mair_el2)
-{
-	__asm__ __volatile__("msr MAIR_EL2, %0\n\t" : : "r" (mair_el2) : "memory");
-}
-
-uint64_t raw_read_mair_el3(void)
-{
-	uint64_t mair_el3;
-
-	__asm__ __volatile__("mrs %0, MAIR_EL3\n\t" : "=r" (mair_el3) :  : "memory");
-
-	return mair_el3;
-}
-
-void raw_write_mair_el3(uint64_t mair_el3)
-{
-	__asm__ __volatile__("msr MAIR_EL3, %0\n\t" : : "r" (mair_el3) : "memory");
-}
-
-/* MIDR */
-uint32_t raw_read_midr_el1(void)
-{
-	uint64_t midr_el1;
-
-	__asm__ __volatile__("mrs %0, MIDR_EL1\n\t" : "=r" (midr_el1) :  : "memory");
-
-	return midr_el1;
-}
-
-/* MPIDR */
-uint64_t raw_read_mpidr_el1(void)
-{
-	uint64_t mpidr_el1;
-
-	__asm__ __volatile__("mrs %0, MPIDR_EL1\n\t" : "=r" (mpidr_el1) :  : "memory");
-
-	return mpidr_el1;
-}
-
-/* RMR */
-uint32_t raw_read_rmr_el1(void)
-{
-	uint64_t rmr_el1;
-
-	__asm__ __volatile__("mrs %0, RMR_EL1\n\t" : "=r" (rmr_el1) :  : "memory");
-
-	return rmr_el1;
-}
-
-void raw_write_rmr_el1(uint32_t rmr_el1)
-{
-	__asm__ __volatile__("msr RMR_EL1, %0\n\t" : : "r" ((uint64_t)rmr_el1) : "memory");
-}
-
-uint32_t raw_read_rmr_el2(void)
-{
-	uint64_t rmr_el2;
-
-	__asm__ __volatile__("mrs %0, RMR_EL2\n\t" : "=r" (rmr_el2) :  : "memory");
-
-	return rmr_el2;
-}
-
-void raw_write_rmr_el2(uint32_t rmr_el2)
-{
-	__asm__ __volatile__("msr RMR_EL2, %0\n\t" : : "r" ((uint64_t)rmr_el2) : "memory");
-}
-
-uint32_t raw_read_rmr_el3(void)
-{
-	uint64_t rmr_el3;
-
-	__asm__ __volatile__("mrs %0, RMR_EL3\n\t" : "=r" (rmr_el3) :  : "memory");
-
-	return rmr_el3;
-}
-
-void raw_write_rmr_el3(uint32_t rmr_el3)
-{
-	__asm__ __volatile__("msr RMR_EL3, %0\n\t" : : "r" ((uint64_t)rmr_el3) : "memory");
-}
-
-/* RVBAR */
-uint64_t raw_read_rvbar_el1(void)
-{
-	uint64_t rvbar_el1;
-
-	__asm__ __volatile__("mrs %0, RVBAR_EL1\n\t" : "=r" (rvbar_el1) :  : "memory");
-
-	return rvbar_el1;
-}
-
-void raw_write_rvbar_el1(uint64_t rvbar_el1)
-{
-	__asm__ __volatile__("msr RVBAR_EL1, %0\n\t" : : "r" (rvbar_el1) : "memory");
-}
-
-uint64_t raw_read_rvbar_el2(void)
-{
-	uint64_t rvbar_el2;
-
-	__asm__ __volatile__("mrs %0, RVBAR_EL2\n\t" : "=r" (rvbar_el2) :  : "memory");
-
-	return rvbar_el2;
-}
-
-void raw_write_rvbar_el2(uint64_t rvbar_el2)
-{
-	__asm__ __volatile__("msr RVBAR_EL2, %0\n\t" : : "r" (rvbar_el2) : "memory");
-}
-
-uint64_t raw_read_rvbar_el3(void)
-{
-	uint64_t rvbar_el3;
-
-	__asm__ __volatile__("mrs %0, RVBAR_EL3\n\t" : "=r" (rvbar_el3) :  : "memory");
-
-	return rvbar_el3;
-}
-
-void raw_write_rvbar_el3(uint64_t rvbar_el3)
-{
-	__asm__ __volatile__("msr RVBAR_EL3, %0\n\t" : : "r" (rvbar_el3) : "memory");
-}
-
-/* Scr */
-uint32_t raw_read_scr_el3(void)
-{
-	uint64_t scr_el3;
-
-	__asm__ __volatile__("mrs %0, SCR_EL3\n\t" : "=r" (scr_el3) :  : "memory");
-
-	return scr_el3;
-}
-
-void raw_write_scr_el3(uint32_t scr_el3)
-{
-	__asm__ __volatile__("msr SCR_EL3, %0\n\t" : : "r" ((uint64_t)scr_el3) : "memory");
-}
-
-/* SCTLR */
-uint32_t raw_read_sctlr_el1(void)
-{
-	uint64_t sctlr_el1;
-
-	__asm__ __volatile__("mrs %0, SCTLR_EL1\n\t" : "=r" (sctlr_el1) :  : "memory");
-
-	return sctlr_el1;
-}
-
-void raw_write_sctlr_el1(uint32_t sctlr_el1)
-{
-	__asm__ __volatile__("msr SCTLR_EL1, %0\n\t" : : "r" ((uint64_t)sctlr_el1) : "memory");
-}
-
-uint32_t raw_read_sctlr_el2(void)
-{
-	uint64_t sctlr_el2;
-
-	__asm__ __volatile__("mrs %0, SCTLR_EL2\n\t" : "=r" (sctlr_el2) :  : "memory");
-
-	return sctlr_el2;
-}
-
-void raw_write_sctlr_el2(uint32_t sctlr_el2)
-{
-	__asm__ __volatile__("msr SCTLR_EL2, %0\n\t" : : "r" ((uint64_t)sctlr_el2) : "memory");
-}
-
-uint32_t raw_read_sctlr_el3(void)
-{
-	uint64_t sctlr_el3;
-
-	__asm__ __volatile__("mrs %0, SCTLR_EL3\n\t" : "=r" (sctlr_el3) :  : "memory");
-
-	return sctlr_el3;
-}
-
-void raw_write_sctlr_el3(uint32_t sctlr_el3)
-{
-	__asm__ __volatile__("msr SCTLR_EL3, %0\n\t" : : "r" ((uint64_t)sctlr_el3) : "memory");
-}
-
-/* TCR */
-uint64_t raw_read_tcr_el1(void)
-{
-	uint64_t tcr_el1;
-
-	__asm__ __volatile__("mrs %0, TCR_EL1\n\t" : "=r" (tcr_el1) :  : "memory");
-
-	return tcr_el1;
-}
-
-void raw_write_tcr_el1(uint64_t tcr_el1)
-{
-	__asm__ __volatile__("msr TCR_EL1, %0\n\t" : : "r" (tcr_el1) : "memory");
-}
-
-uint32_t raw_read_tcr_el2(void)
-{
-	uint64_t tcr_el2;
-
-	__asm__ __volatile__("mrs %0, TCR_EL2\n\t" : "=r" (tcr_el2) :  : "memory");
-
-	return tcr_el2;
-}
-
-void raw_write_tcr_el2(uint32_t tcr_el2)
-{
-	__asm__ __volatile__("msr TCR_EL2, %0\n\t" : : "r" ((uint64_t)tcr_el2) : "memory");
-}
-
-uint32_t raw_read_tcr_el3(void)
-{
-	uint64_t tcr_el3;
-
-	__asm__ __volatile__("mrs %0, TCR_EL3\n\t" : "=r" (tcr_el3) :  : "memory");
-
-	return tcr_el3;
-}
-
-void raw_write_tcr_el3(uint32_t tcr_el3)
-{
-	__asm__ __volatile__("msr TCR_EL3, %0\n\t" : : "r" ((uint64_t)tcr_el3) : "memory");
-}
-
-/* TTBR0 */
-uint64_t raw_read_ttbr0_el1(void)
-{
-	uint64_t ttbr0_el1;
-
-	__asm__ __volatile__("mrs %0, TTBR0_EL1\n\t" : "=r" (ttbr0_el1) :  : "memory");
-
-	return ttbr0_el1;
-}
-
-void raw_write_ttbr0_el1(uint64_t ttbr0_el1)
-{
-	__asm__ __volatile__("msr TTBR0_EL1, %0\n\t" : : "r" (ttbr0_el1) : "memory");
-}
-
-uint64_t raw_read_ttbr0_el2(void)
-{
-	uint64_t ttbr0_el2;
-
-	__asm__ __volatile__("mrs %0, TTBR0_EL2\n\t" : "=r" (ttbr0_el2) :  : "memory");
-
-	return ttbr0_el2;
-}
-
-void raw_write_ttbr0_el2(uint64_t ttbr0_el2)
-{
-	__asm__ __volatile__("msr TTBR0_EL2, %0\n\t" : : "r" (ttbr0_el2) : "memory");
-}
-
-uint64_t raw_read_ttbr0_el3(void)
-{
-	uint64_t ttbr0_el3;
-
-	__asm__ __volatile__("mrs %0, TTBR0_EL3\n\t" : "=r" (ttbr0_el3) :  : "memory");
-
-	return ttbr0_el3;
-}
-
-void raw_write_ttbr0_el3(uint64_t ttbr0_el3)
-{
-	__asm__ __volatile__("msr TTBR0_EL3, %0\n\t" : : "r" (ttbr0_el3) : "memory");
-}
-
-/* TTBR1 */
-uint64_t raw_read_ttbr1_el1(void)
-{
-	uint64_t ttbr1_el1;
-
-	__asm__ __volatile__("mrs %0, TTBR1_EL1\n\t" : "=r" (ttbr1_el1) :  : "memory");
-
-	return ttbr1_el1;
-}
-
-void raw_write_ttbr1_el1(uint64_t ttbr1_el1)
-{
-	__asm__ __volatile__("msr TTBR1_EL1, %0\n\t" : : "r" (ttbr1_el1) : "memory");
-}
-
-/* VBAR */
-uint64_t raw_read_vbar_el1(void)
-{
-	uint64_t vbar_el1;
-
-	__asm__ __volatile__("mrs %0, VBAR_EL1\n\t" : "=r" (vbar_el1) :  : "memory");
-
-	return vbar_el1;
-}
-
-void raw_write_vbar_el1(uint64_t vbar_el1)
-{
-	__asm__ __volatile__("msr VBAR_EL1, %0\n\t" : : "r" (vbar_el1) : "memory");
-}
-
-uint64_t raw_read_vbar_el2(void)
-{
-	uint64_t vbar_el2;
-
-	__asm__ __volatile__("mrs %0, VBAR_EL2\n\t" : "=r" (vbar_el2) :  : "memory");
-
-	return vbar_el2;
-}
-
-void raw_write_vbar_el2(uint64_t vbar_el2)
-{
-	__asm__ __volatile__("msr VBAR_EL2, %0\n\t" : : "r" (vbar_el2) : "memory");
-}
-
-uint64_t raw_read_vbar_el3(void)
-{
-	uint64_t vbar_el3;
-
-	__asm__ __volatile__("mrs %0, VBAR_EL3\n\t" : "=r" (vbar_el3) :  : "memory");
-
-	return vbar_el3;
-}
-
-void raw_write_vbar_el3(uint64_t vbar_el3)
-{
-	__asm__ __volatile__("msr VBAR_EL3, %0\n\t" : : "r" (vbar_el3) : "memory");
-}
-
-uint32_t raw_read_cntfrq_el0(void)
-{
-	uint64_t cntfrq_el0;
-
-	__asm__ __volatile__("mrs %0, CNTFRQ_EL0\n\t" : "=r" (cntfrq_el0) : : "memory");
-	return cntfrq_el0;
-}
-
-void raw_write_cntfrq_el0(uint32_t cntfrq_el0)
-{
-	__asm__ __volatile__("msr CNTFRQ_EL0, %0\n\t" : : "r" ((uint64_t)cntfrq_el0) : "memory");
-}
-
-uint64_t raw_read_cntpct_el0(void)
-{
-	uint64_t cntpct_el0;
-
-	__asm__ __volatile__("mrs %0, CNTPCT_EL0\n\t" : "=r" (cntpct_el0) : : "memory");
-	return cntpct_el0;
-}
diff --git a/src/arch/arm64/armv8/lib/tlb.c b/src/arch/arm64/armv8/lib/tlb.c
deleted file mode 100644
index b40fb8f135..0000000000
--- a/src/arch/arm64/armv8/lib/tlb.c
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * This file is part of the coreboot project.
- *
- * Copyright (C) 2014 Google Inc
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; version 2 of
- * the License.
- *
- * 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.
- *
- * tlb.c: System intructions for TLB maintenance.
- * Reference: ARM Architecture Reference Manual, ARMv8-A edition
- */
-
-#include <stdint.h>
-
-#include <arch/lib_helpers.h>
-
-/* TLBIALL */
-void tlbiall_el1(void)
-{
-	__asm__ __volatile__("tlbi alle1\n\t" : : : "memory");
-}
-
-void tlbiall_el2(void)
-{
-	__asm__ __volatile__("tlbi alle2\n\t" : : : "memory");
-}
-
-void tlbiall_el3(void)
-{
-	__asm__ __volatile__("tlbi alle3\n\t" : : : "memory");
-}
-
-/* TLBIALLIS */
-void tlbiallis_el1(void)
-{
-	__asm__ __volatile__("tlbi alle1is\n\t" : : : "memory");
-}
-
-void tlbiallis_el2(void)
-{
-	__asm__ __volatile__("tlbi alle2is\n\t" : : : "memory");
-}
-
-void tlbiallis_el3(void)
-{
-	__asm__ __volatile__("tlbi alle3is\n\t" : : : "memory");
-}
-
-/* TLBIVAA */
-void tlbivaa_el1(uint64_t va)
-{
-	__asm__ __volatile__("tlbi vaae1, %0\n\t" : : "r" (va) : "memory");
-}
diff --git a/src/arch/arm64/include/armv8/arch/cache.h b/src/arch/arm64/include/armv8/arch/cache.h
index 7bf81cf258..3de2e80877 100644
--- a/src/arch/arm64/include/armv8/arch/cache.h
+++ b/src/arch/arm64/include/armv8/arch/cache.h
@@ -58,6 +58,7 @@
 #include <stddef.h>
 #include <stdint.h>
 #include <arch/barrier.h>
+#include <arch/lib_helpers.h>
 
 /* dcache clean by virtual address to PoC */
 void dcache_clean_by_mva(void const *addr, size_t len);
@@ -76,18 +77,22 @@ void dcache_clean_invalidate_all(void);
 /* returns number of bytes per cache line */
 unsigned int dcache_line_bytes(void);
 
-/* tlb invalidate all */
-void tlb_invalidate_all(void);
+/* Invalidate all TLB entries. */
+static inline void tlb_invalidate_all(void)
+{
+	/* TLBIALL includes dTLB and iTLB on systems that have them. */
+	tlbiall_el3();
+	dsb();
+	isb();
+}
 
 /* Invalidate all of the instruction cache for PE to PoU. */
 static inline void icache_invalidate_all(void)
 {
-	__asm__ __volatile__(
-		"dsb	sy\n\t"
-		"ic	iallu\n\t"
-		"dsb	sy\n\t"
-		"isb\n\t"
-	: : : "memory");
+	dsb();
+	iciallu();
+	dsb();
+	isb();
 }
 
 #endif /* __ASSEMBLER__ */
diff --git a/src/arch/arm64/include/armv8/arch/lib_helpers.h b/src/arch/arm64/include/armv8/arch/lib_helpers.h
index e810098490..e270e0096c 100644
--- a/src/arch/arm64/include/armv8/arch/lib_helpers.h
+++ b/src/arch/arm64/include/armv8/arch/lib_helpers.h
@@ -145,184 +145,239 @@
 
 #include <stdint.h>
 
-/* PSTATE and special purpose register access functions */
-uint32_t raw_read_current_el(void);
-uint32_t raw_read_daif(void);
-void raw_write_daif(uint32_t daif);
-void enable_debug_exceptions(void);
-void enable_serror_exceptions(void);
-void enable_irq(void);
-void enable_fiq(void);
-void disable_debug_exceptions(void);
-void disable_serror_exceptions(void);
-void disable_irq(void);
-void disable_fiq(void);
-uint64_t raw_read_dlr_el0(void);
-void raw_write_dlr_el0(uint64_t dlr_el0);
-uint64_t raw_read_dspsr_el0(void);
-void raw_write_dspsr_el0(uint64_t dspsr_el0);
-uint64_t raw_read_elr_el1(void);
-void raw_write_elr_el1(uint64_t elr_el1);
-uint64_t raw_read_elr_el2(void);
-void raw_write_elr_el2(uint64_t elr_el2);
-uint64_t raw_read_elr_el3(void);
-void raw_write_elr_el3(uint64_t elr_el3);
-uint32_t raw_read_fpcr(void);
-void raw_write_fpcr(uint32_t fpcr);
-uint32_t raw_read_fpsr(void);
-void raw_write_fpsr(uint32_t fpsr);
-uint32_t raw_read_nzcv(void);
-void raw_write_nzcv(uint32_t nzcv);
-uint64_t raw_read_sp_el0(void);
-void raw_write_sp_el0(uint64_t sp_el0);
-uint64_t raw_read_sp_el1(void);
-void raw_write_sp_el1(uint64_t sp_el1);
-uint64_t raw_read_sp_el2(void);
-void raw_write_sp_el2(uint64_t sp_el2);
-uint32_t raw_read_spsel(void);
-void raw_write_spsel(uint32_t spsel);
-uint64_t raw_read_sp_el3(void);
-void raw_write_sp_el3(uint64_t sp_el3);
-uint32_t raw_read_spsr_abt(void);
-void raw_write_spsr_abt(uint32_t spsr_abt);
-uint32_t raw_read_spsr_el1(void);
-void raw_write_spsr_el1(uint32_t spsr_el1);
-uint32_t raw_read_spsr_el2(void);
-void raw_write_spsr_el2(uint32_t spsr_el2);
-uint32_t raw_read_spsr_el3(void);
-void raw_write_spsr_el3(uint32_t spsr_el3);
-uint32_t raw_read_spsr_fiq(void);
-void raw_write_spsr_fiq(uint32_t spsr_fiq);
-uint32_t raw_read_spsr_irq(void);
-void raw_write_spsr_irq(uint32_t spsr_irq);
-uint32_t raw_read_spsr_und(void);
-void raw_write_spsr_und(uint32_t spsr_und);
-
-/* System control register access */
-uint32_t raw_read_actlr_el1(void);
-void raw_write_actlr_el1(uint32_t actlr_el1);
-uint32_t raw_read_actlr_el2(void);
-void raw_write_actlr_el2(uint32_t actlr_el2);
-uint32_t raw_read_actlr_el3(void);
-void raw_write_actlr_el3(uint32_t actlr_el3);
-uint32_t raw_read_afsr0_el1(void);
-void raw_write_afsr0_el1(uint32_t afsr0_el1);
-uint32_t raw_read_afsr0_el2(void);
-void raw_write_afsr0_el2(uint32_t afsr0_el2);
-uint32_t raw_read_afsr0_el3(void);
-void raw_write_afsr0_el3(uint32_t afsr0_el3);
-uint32_t raw_read_afsr1_el1(void);
-void raw_write_afsr1_el1(uint32_t afsr1_el1);
-uint32_t raw_read_afsr1_el2(void);
-void raw_write_afsr1_el2(uint32_t afsr1_el2);
-uint32_t raw_read_afsr1_el3(void);
-void raw_write_afsr1_el3(uint32_t afsr1_el3);
-uint32_t raw_read_aidr_el1(void);
-uint64_t raw_read_amair_el1(void);
-void raw_write_amair_el1(uint64_t amair_el1);
-uint64_t raw_read_amair_el2(void);
-void raw_write_amair_el2(uint64_t amair_el2);
-uint64_t raw_read_amair_el3(void);
-void raw_write_amair_el3(uint64_t amair_el3);
-uint32_t raw_read_ccsidr_el1(void);
-uint32_t raw_read_clidr_el1(void);
-uint32_t raw_read_cpacr_el1(void);
-void raw_write_cpacr_el1(uint32_t cpacr_el1);
-uint32_t raw_read_cptr_el2(void);
-void raw_write_cptr_el2(uint32_t cptr_el2);
-uint32_t raw_read_cptr_el3(void);
-void raw_write_cptr_el3(uint32_t cptr_el3);
-uint32_t raw_read_csselr_el1(void);
-void raw_write_csselr_el1(uint32_t csselr_el1);
-uint32_t raw_read_ctr_el0(void);
-uint32_t raw_read_esr_el1(void);
-void raw_write_esr_el1(uint32_t esr_el1);
-uint32_t raw_read_esr_el2(void);
-void raw_write_esr_el2(uint32_t esr_el2);
-uint32_t raw_read_esr_el3(void);
-void raw_write_esr_el3(uint32_t esr_el3);
-uint64_t raw_read_far_el1(void);
-void raw_write_far_el1(uint64_t far_el1);
-uint64_t raw_read_far_el2(void);
-void raw_write_far_el2(uint64_t far_el2);
-uint64_t raw_read_far_el3(void);
-void raw_write_far_el3(uint64_t far_el3);
-uint64_t raw_read_hcr_el2(void);
-void raw_write_hcr_el2(uint64_t hcr_el2);
-uint64_t raw_read_aa64pfr0_el1(void);
-uint64_t raw_read_mair_el1(void);
-void raw_write_mair_el1(uint64_t mair_el1);
-uint64_t raw_read_mair_el2(void);
-void raw_write_mair_el2(uint64_t mair_el2);
-uint64_t raw_read_mair_el3(void);
-void raw_write_mair_el3(uint64_t mair_el3);
-uint32_t raw_read_midr_el1(void);
-uint64_t raw_read_mpidr_el1(void);
-uint32_t raw_read_rmr_el1(void);
-void raw_write_rmr_el1(uint32_t rmr_el1);
-uint32_t raw_read_rmr_el2(void);
-void raw_write_rmr_el2(uint32_t rmr_el2);
-uint32_t raw_read_rmr_el3(void);
-void raw_write_rmr_el3(uint32_t rmr_el3);
-uint64_t raw_read_rvbar_el1(void);
-void raw_write_rvbar_el1(uint64_t rvbar_el1);
-uint64_t raw_read_rvbar_el2(void);
-void raw_write_rvbar_el2(uint64_t rvbar_el2);
-uint64_t raw_read_rvbar_el3(void);
-void raw_write_rvbar_el3(uint64_t rvbar_el3);
-uint32_t raw_read_scr_el3(void);
-void raw_write_scr_el3(uint32_t scr_el3);
-uint32_t raw_read_sctlr_el1(void);
-void raw_write_sctlr_el1(uint32_t sctlr_el1);
-uint32_t raw_read_sctlr_el2(void);
-void raw_write_sctlr_el2(uint32_t sctlr_el2);
-uint32_t raw_read_sctlr_el3(void);
-void raw_write_sctlr_el3(uint32_t sctlr_el3);
-uint64_t raw_read_tcr_el1(void);
-void raw_write_tcr_el1(uint64_t tcr_el1);
-uint32_t raw_read_tcr_el2(void);
-void raw_write_tcr_el2(uint32_t tcr_el2);
-uint32_t raw_read_tcr_el3(void);
-void raw_write_tcr_el3(uint32_t tcr_el3);
-uint64_t raw_read_ttbr0_el1(void);
-void raw_write_ttbr0_el1(uint64_t ttbr0_el1);
-uint64_t raw_read_ttbr0_el2(void);
-void raw_write_ttbr0_el2(uint64_t ttbr0_el2);
-uint64_t raw_read_ttbr0_el3(void);
-void raw_write_ttbr0_el3(uint64_t ttbr0_el3);
-uint64_t raw_read_ttbr1_el1(void);
-void raw_write_ttbr1_el1(uint64_t ttbr1_el1);
-uint64_t raw_read_vbar_el1(void);
-void raw_write_vbar_el1(uint64_t vbar_el1);
-uint64_t raw_read_vbar_el2(void);
-void raw_write_vbar_el2(uint64_t vbar_el2);
-uint64_t raw_read_vbar_el3(void);
-void raw_write_vbar_el3(uint64_t vbar_el3);
-uint32_t raw_read_cntfrq_el0(void);
-void raw_write_cntfrq_el0(uint32_t cntfrq_el0);
-uint64_t raw_read_cntpct_el0(void);
+#define MAKE_REGISTER_ACCESSORS(reg) \
+	static inline uint64_t raw_read_##reg(void) \
+	{ \
+		uint64_t value; \
+		__asm__ __volatile__("mrs %0, " #reg "\n\t" \
+				     : "=r" (value) : : "memory"); \
+		return value; \
+	} \
+	static inline void raw_write_##reg(uint64_t value) \
+	{ \
+		__asm__ __volatile__("msr " #reg ", %0\n\t" \
+				     : : "r" (value) : "memory"); \
+	}
+
+#define MAKE_REGISTER_ACCESSORS_EL123(reg) \
+	MAKE_REGISTER_ACCESSORS(reg##_el1) \
+	MAKE_REGISTER_ACCESSORS(reg##_el2) \
+	MAKE_REGISTER_ACCESSORS(reg##_el3)
+
+/* Architectural register accessors */
+MAKE_REGISTER_ACCESSORS_EL123(actlr)
+MAKE_REGISTER_ACCESSORS_EL123(afsr0)
+MAKE_REGISTER_ACCESSORS_EL123(afsr1)
+MAKE_REGISTER_ACCESSORS(aidr_el1)
+MAKE_REGISTER_ACCESSORS_EL123(amair)
+MAKE_REGISTER_ACCESSORS(ccsidr_el1)
+MAKE_REGISTER_ACCESSORS(clidr_el1)
+MAKE_REGISTER_ACCESSORS(cntfrq_el0)
+MAKE_REGISTER_ACCESSORS(cnthctl_el2)
+MAKE_REGISTER_ACCESSORS(cnthp_ctl_el2)
+MAKE_REGISTER_ACCESSORS(cnthp_cval_el2)
+MAKE_REGISTER_ACCESSORS(cnthp_tval_el2)
+MAKE_REGISTER_ACCESSORS(cntkctl_el1)
+MAKE_REGISTER_ACCESSORS(cntp_ctl_el0)
+MAKE_REGISTER_ACCESSORS(cntp_cval_el0)
+MAKE_REGISTER_ACCESSORS(cntp_tval_el0)
+MAKE_REGISTER_ACCESSORS(cntpct_el0)
+MAKE_REGISTER_ACCESSORS(cntps_ctl_el1)
+MAKE_REGISTER_ACCESSORS(cntps_cval_el1)
+MAKE_REGISTER_ACCESSORS(cntps_tval_el1)
+MAKE_REGISTER_ACCESSORS(cntv_ctl_el0)
+MAKE_REGISTER_ACCESSORS(cntv_cval_el0)
+MAKE_REGISTER_ACCESSORS(cntv_tval_el0)
+MAKE_REGISTER_ACCESSORS(cntvct_el0)
+MAKE_REGISTER_ACCESSORS(cntvoff_el2)
+MAKE_REGISTER_ACCESSORS(contextidr_el1)
+MAKE_REGISTER_ACCESSORS(cpacr_el1)
+MAKE_REGISTER_ACCESSORS(cptr_el2)
+MAKE_REGISTER_ACCESSORS(cptr_el3)
+MAKE_REGISTER_ACCESSORS(csselr_el1)
+MAKE_REGISTER_ACCESSORS(ctr_el0)
+MAKE_REGISTER_ACCESSORS(currentel)
+MAKE_REGISTER_ACCESSORS(daif)
+MAKE_REGISTER_ACCESSORS(dczid_el0)
+MAKE_REGISTER_ACCESSORS_EL123(elr)
+MAKE_REGISTER_ACCESSORS_EL123(esr)
+MAKE_REGISTER_ACCESSORS_EL123(far)
+MAKE_REGISTER_ACCESSORS(fpcr)
+MAKE_REGISTER_ACCESSORS(fpsr)
+MAKE_REGISTER_ACCESSORS(hacr_el2)
+MAKE_REGISTER_ACCESSORS(hcr_el2)
+MAKE_REGISTER_ACCESSORS(hpfar_el2)
+MAKE_REGISTER_ACCESSORS(hstr_el2)
+MAKE_REGISTER_ACCESSORS(isr_el1)
+MAKE_REGISTER_ACCESSORS_EL123(mair)
+MAKE_REGISTER_ACCESSORS(midr_el1)
+MAKE_REGISTER_ACCESSORS(mpidr_el1)
+MAKE_REGISTER_ACCESSORS(nzcv)
+MAKE_REGISTER_ACCESSORS(par_el1)
+MAKE_REGISTER_ACCESSORS(revdir_el1)
+MAKE_REGISTER_ACCESSORS_EL123(rmr)
+MAKE_REGISTER_ACCESSORS_EL123(rvbar)
+MAKE_REGISTER_ACCESSORS(scr_el3)
+MAKE_REGISTER_ACCESSORS_EL123(sctlr)
+MAKE_REGISTER_ACCESSORS(sp_el0)
+MAKE_REGISTER_ACCESSORS(sp_el1)
+MAKE_REGISTER_ACCESSORS(sp_el2)
+MAKE_REGISTER_ACCESSORS(spsel)
+MAKE_REGISTER_ACCESSORS_EL123(spsr)
+MAKE_REGISTER_ACCESSORS(spsr_abt)
+MAKE_REGISTER_ACCESSORS(spsr_fiq)
+MAKE_REGISTER_ACCESSORS(spsr_irq)
+MAKE_REGISTER_ACCESSORS(spsr_und)
+MAKE_REGISTER_ACCESSORS_EL123(tcr)
+MAKE_REGISTER_ACCESSORS_EL123(tpidr)
+MAKE_REGISTER_ACCESSORS_EL123(ttbr0)
+MAKE_REGISTER_ACCESSORS(ttbr1_el1)
+MAKE_REGISTER_ACCESSORS_EL123(vbar)
+MAKE_REGISTER_ACCESSORS(vmpidr_el2)
+MAKE_REGISTER_ACCESSORS(vpidr_el2)
+MAKE_REGISTER_ACCESSORS(vtcr_el2)
+MAKE_REGISTER_ACCESSORS(vttbr_el2)
+
+/* Special DAIF accessor functions */
+static inline void enable_debug_exceptions(void)
+{
+	__asm__ __volatile__("msr DAIFClr, %0\n\t"
+			     : : "i" (DAIF_DBG_BIT)  : "memory");
+}
+
+static inline void enable_serror_exceptions(void)
+{
+	__asm__ __volatile__("msr DAIFClr, %0\n\t"
+			     : : "i" (DAIF_ABT_BIT)  : "memory");
+}
+
+static inline void enable_irq(void)
+{
+	__asm__ __volatile__("msr DAIFClr, %0\n\t"
+			     : : "i" (DAIF_IRQ_BIT)  : "memory");
+}
+
+static inline void enable_fiq(void)
+{
+	__asm__ __volatile__("msr DAIFClr, %0\n\t"
+			     : : "i" (DAIF_FIQ_BIT)  : "memory");
+}
+
+static inline void disable_debug_exceptions(void)
+{
+	__asm__ __volatile__("msr DAIFSet, %0\n\t"
+			     : : "i" (DAIF_DBG_BIT)  : "memory");
+}
+
+static inline void disable_serror_exceptions(void)
+{
+	__asm__ __volatile__("msr DAIFSet, %0\n\t"
+			     : : "i" (DAIF_ABT_BIT)  : "memory");
+}
+
+static inline void disable_irq(void)
+{
+	__asm__ __volatile__("msr DAIFSet, %0\n\t"
+			     : : "i" (DAIF_IRQ_BIT)  : "memory");
+}
+
+static inline void disable_fiq(void)
+{
+	__asm__ __volatile__("msr DAIFSet, %0\n\t"
+			     : : "i" (DAIF_FIQ_BIT)  : "memory");
+}
 
 /* Cache maintenance system instructions */
-void dccisw(uint64_t cisw);
-void dccivac(uint64_t civac);
-void dccsw(uint64_t csw);
-void dccvac(uint64_t cvac);
-void dccvau(uint64_t cvau);
-void dcisw(uint64_t isw);
-void dcivac(uint64_t ivac);
-void dczva(uint64_t zva);
-void iciallu(void);
-void icialluis(void);
-void icivau(uint64_t ivau);
+static inline void dccisw(uint64_t cisw)
+{
+	__asm__ __volatile__("dc cisw, %0\n\t" : : "r" (cisw) : "memory");
+}
+
+static inline void dccivac(uint64_t civac)
+{
+	__asm__ __volatile__("dc civac, %0\n\t" : : "r" (civac) : "memory");
+}
+
+static inline void dccsw(uint64_t csw)
+{
+	__asm__ __volatile__("dc csw, %0\n\t" : : "r" (csw) : "memory");
+}
+
+static inline void dccvac(uint64_t cvac)
+{
+	__asm__ __volatile__("dc cvac, %0\n\t" : : "r" (cvac) : "memory");
+}
+
+static inline void dccvau(uint64_t cvau)
+{
+	__asm__ __volatile__("dc cvau, %0\n\t" : : "r" (cvau) : "memory");
+}
+
+static inline void dcisw(uint64_t isw)
+{
+	__asm__ __volatile__("dc isw, %0\n\t" : : "r" (isw) : "memory");
+}
+
+static inline void dcivac(uint64_t ivac)
+{
+	__asm__ __volatile__("dc ivac, %0\n\t" : : "r" (ivac) : "memory");
+}
+
+static inline void dczva(uint64_t zva)
+{
+	__asm__ __volatile__("dc zva, %0\n\t" : : "r" (zva) : "memory");
+}
+
+static inline void iciallu(void)
+{
+	__asm__ __volatile__("ic iallu\n\t" : : : "memory");
+}
+
+static inline void icialluis(void)
+{
+	__asm__ __volatile__("ic ialluis\n\t" : : : "memory");
+}
+
+static inline void icivau(uint64_t ivau)
+{
+	__asm__ __volatile__("ic ivau, %0\n\t" : : "r" (ivau) : "memory");
+}
 
 /* TLB maintenance instructions */
-void tlbiall_el1(void);
-void tlbiall_el2(void);
-void tlbiall_el3(void);
-void tlbiallis_el1(void);
-void tlbiallis_el2(void);
-void tlbiallis_el3(void);
-void tlbivaa_el1(uint64_t va);
+static inline void tlbiall_el1(void)
+{
+	__asm__ __volatile__("tlbi alle1\n\t" : : : "memory");
+}
+
+static inline void tlbiall_el2(void)
+{
+	__asm__ __volatile__("tlbi alle2\n\t" : : : "memory");
+}
+
+static inline void tlbiall_el3(void)
+{
+	__asm__ __volatile__("tlbi alle3\n\t" : : : "memory");
+}
+
+static inline void tlbiallis_el1(void)
+{
+	__asm__ __volatile__("tlbi alle1is\n\t" : : : "memory");
+}
+
+static inline void tlbiallis_el2(void)
+{
+	__asm__ __volatile__("tlbi alle2is\n\t" : : : "memory");
+}
+
+static inline void tlbiallis_el3(void)
+{
+	__asm__ __volatile__("tlbi alle3is\n\t" : : : "memory");
+}
+
+static inline void tlbivaa_el1(uint64_t va)
+{
+	__asm__ __volatile__("tlbi vaae1, %0\n\t" : : "r" (va) : "memory");
+}
 
 #endif /* __ASSEMBLER__ */