25 files changed, 4055 insertions, 0 deletions

diff --git a/lib/aarch64/cache_helpers.S b/lib/aarch64/cache_helpers.S
new file mode 100644
index 0000000..04caac4
--- /dev/null
+++ b/lib/aarch64/cache_helpers.S
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+	.globl	dcisw
+	.globl	dccisw
+	.globl	dccsw
+	.globl	dccvac
+	.globl	dcivac
+	.globl	dccivac
+	.globl	dccvau
+	.globl	dczva
+	.globl	flush_dcache_range
+	.globl	inv_dcache_range
+	.globl	dcsw_op_louis
+	.globl	dcsw_op_all
+
+	.global _clean_dcache_addr
+	.global _clean_invd_dcache_addr
+	.globl	platform_stack_set_bl2
+
+	.type   platform_stack_set_bl2, @function
+platform_stack_set_bl2:
+	mov x9, x30 //lr
+	mov sp, x0
+	ret x9
+
+func dcisw
+	dc	isw, x0
+	ret
+
+
+func dccisw
+	dc	cisw, x0
+	ret
+
+
+func dccsw
+	dc	csw, x0
+	ret
+
+
+func dccvac
+	dc	cvac, x0
+	ret
+
+
+func dcivac
+	dc	ivac, x0
+	ret
+
+
+func dccivac
+	dc	civac, x0
+	ret
+
+
+func dccvau
+	dc	cvau, x0
+	ret
+
+
+func dczva
+	dc	zva, x0
+	ret
+
+
+	/* ------------------------------------------
+	 * Clean+Invalidate from base address till
+	 * size. 'x0' = addr, 'x1' = size
+	 * ------------------------------------------
+	 */
+func flush_dcache_range
+	dcache_line_size x2, x3
+	add	x1, x0, x1
+	sub	x3, x2, #1
+	bic	x0, x0, x3
+flush_loop:
+	dc	civac, x0
+	add	x0, x0, x2
+	cmp	x0, x1
+	b.lo    flush_loop
+	dsb	sy
+	ret
+
+
+	/* ------------------------------------------
+	 * Invalidate from base address till
+	 * size. 'x0' = addr, 'x1' = size
+	 * ------------------------------------------
+	 */
+func inv_dcache_range
+	dcache_line_size x2, x3
+	add	x1, x0, x1
+	sub	x3, x2, #1
+	bic	x0, x0, x3
+inv_loop:
+	dc	ivac, x0
+	add	x0, x0, x2
+	cmp	x0, x1
+	b.lo    inv_loop
+	dsb	sy
+	ret
+
+
+	/* ---------------------------------------------------------------
+	 * Data cache operations by set/way to the level specified
+	 *
+	 * The main function, do_dcsw_op requires:
+	 * x0: The operation type (0-2), as defined in arch.h
+	 * x3: The last cache level to operate on
+	 * x9: clidr_el1
+	 * and will carry out the operation on each data cache from level 0
+	 * to the level in x3 in sequence
+	 *
+	 * The dcsw_op macro sets up the x3 and x9 parameters based on
+	 * clidr_el1 cache information before invoking the main function
+	 * ---------------------------------------------------------------
+	 */
+
+	.macro	dcsw_op shift, fw, ls
+	mrs	x9, clidr_el1
+	ubfx	x3, x9, \shift, \fw
+	lsl	x3, x3, \ls
+	b	do_dcsw_op
+	.endm
+
+func do_dcsw_op
+	cbz	x3, exit
+	mov	x10, xzr
+	adr	x14, dcsw_loop_table	// compute inner loop address
+	add	x14, x14, x0, lsl #5	// inner loop is 8x32-bit instructions
+	mov	x0, x9
+	mov	w8, #1
+loop1:
+	add	x2, x10, x10, lsr #1	// work out 3x current cache level
+	lsr	x1, x0, x2		// extract cache type bits from clidr
+	and	x1, x1, #7		// mask the bits for current cache only
+	cmp	x1, #2			// see what cache we have at this level
+	b.lt	level_done		// nothing to do if no cache or icache
+
+	msr	csselr_el1, x10		// select current cache level in csselr
+	isb				// isb to sych the new cssr&csidr
+	mrs	x1, ccsidr_el1		// read the new ccsidr
+	and	x2, x1, #7		// extract the length of the cache lines
+	add	x2, x2, #4		// add 4 (line length offset)
+	ubfx	x4, x1, #3, #10		// maximum way number
+	clz	w5, w4			// bit position of way size increment
+	lsl	w9, w4, w5		// w9 = aligned max way number
+	lsl	w16, w8, w5		// w16 = way number loop decrement
+	orr	w9, w10, w9		// w9 = combine way and cache number
+	ubfx	w6, w1, #13, #15	// w6 = max set number
+	lsl	w17, w8, w2		// w17 = set number loop decrement
+	dsb	sy			// barrier before we start this level
+	br	x14			// jump to DC operation specific loop
+
+	.macro	dcsw_loop _op
+loop2_\_op:
+	lsl	w7, w6, w2		// w7 = aligned max set number
+
+loop3_\_op:
+	orr	w11, w9, w7		// combine cache, way and set number
+	dc	\_op, x11
+	subs	w7, w7, w17		// decrement set number
+	b.ge	loop3_\_op
+
+	subs	x9, x9, x16		// decrement way number
+	b.ge	loop2_\_op
+
+	b	level_done
+	.endm
+
+level_done:
+	add	x10, x10, #2		// increment cache number
+	cmp	x3, x10
+	b.gt    loop1
+	msr	csselr_el1, xzr		// select cache level 0 in csselr
+	dsb	sy			// barrier to complete final cache operation
+	isb
+exit:
+	ret
+
+dcsw_loop_table:
+	dcsw_loop isw
+	dcsw_loop cisw
+	dcsw_loop csw
+
+
+func dcsw_op_louis
+	dcsw_op #LOUIS_SHIFT, #CLIDR_FIELD_WIDTH, #LEVEL_SHIFT
+
+
+func dcsw_op_all
+	dcsw_op #LOC_SHIFT, #CLIDR_FIELD_WIDTH, #LEVEL_SHIFT
+
+
+#if 0
+//==============================================
+// void _clean_dcache_addr(unsigned long addr)
+// clean dcache by VA
+//==============================================
+	.type _clean_dcache_addr, @function
+_clean_dcache_addr:
+	dc cvac, x0
+	dsb sy
+	ret
+//==============================================
+// void _clean_invd_dcache_addr(unsigned long addr)
+// clean&invalid dcache by VA
+//==============================================
+	.type _clean_invd_dcache_addr, @function
+_clean_invd_dcache_addr:
+	dc civac, x0
+	dsb sy
+	ret
+#endif
diff --git a/lib/aarch64/cpu_helpers.S b/lib/aarch64/cpu_helpers.S
new file mode 100644
index 0000000..008d39d
--- /dev/null
+++ b/lib/aarch64/cpu_helpers.S
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+	.weak	cpu_reset_handler
+
+
+func cpu_reset_handler
+	/* ---------------------------------------------
+	 * As a bare minimal enable the SMP bit.
+	 * ---------------------------------------------
+	 */
+	mrs	x0, midr_el1
+	lsr     x1, x0, #MIDR_PN_SHIFT
+	and     x1, x1, #MIDR_PN_MASK
+	cmp	x1, #MIDR_PN_A57
+	b.eq	a57_setup_begin
+	cmp	x1, #MIDR_PN_A53
+	b.eq	smp_setup_begin
+	b	smp_setup_end
+
+a57_setup_begin:
+	ubfx	x1, x0, #MIDR_VAR_SHIFT, #4
+	cmp	x1, #0		// Major Revision 0
+	b.ne	smp_setup_begin
+	ubfx	x1, x0, #MIDR_REV_SHIFT, #4
+	cmp	x1, #0		// Minor Revision 0
+	b.ne	smp_setup_begin
+	mov	x1, #CPUACTLR_NO_ALLOC_WBWA
+	orr	x1, x1, #CPUACTLR_DIS_DMB_NULL
+	orr	x1, x1, #CPUACTLR_DCC_AS_DCCI
+	mrs	x0, CPUACTLR_EL1
+	orr	x0, x0, x1
+	msr	CPUACTLR_EL1, x0
+	mov	x0, #0x082
+	msr	s3_1_c11_c0_2, x0
+
+smp_setup_begin:
+	mrs	x0, CPUECTLR_EL1
+	orr	x0, x0, #CPUECTLR_SMP_BIT
+	msr	CPUECTLR_EL1, x0
+	isb
+
+smp_setup_end:
+	ret
diff --git a/lib/aarch64/misc_helpers.S b/lib/aarch64/misc_helpers.S
new file mode 100644
index 0000000..e15c243
--- /dev/null
+++ b/lib/aarch64/misc_helpers.S
@@ -0,0 +1,324 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+	.globl	enable_irq
+	.globl	disable_irq
+
+	.globl	enable_fiq
+	.globl	disable_fiq
+
+	.globl	enable_serror
+	.globl	disable_serror
+
+	.globl	enable_debug_exceptions
+	.globl	disable_debug_exceptions
+
+	.globl	read_daif
+	.globl	write_daif
+
+	.globl	read_spsr_el1
+	.globl	read_spsr_el2
+	.globl	read_spsr_el3
+
+	.globl	write_spsr_el1
+	.globl	write_spsr_el2
+	.globl	write_spsr_el3
+
+	.globl	read_elr_el1
+	.globl	read_elr_el2
+	.globl	read_elr_el3
+
+	.globl	write_elr_el1
+	.globl	write_elr_el2
+	.globl	write_elr_el3
+
+	.globl	get_afflvl_shift
+	.globl	mpidr_mask_lower_afflvls
+	.globl	dsb
+	.globl	isb
+	.globl	sev
+	.globl	wfe
+	.globl	wfi
+	.globl	eret
+	.globl	smc
+
+	.globl	zeromem16
+	.globl	memcpy16
+
+	.globl	disable_mmu_el3
+	.globl	disable_mmu_icache_el3
+	.globl  remap_zero_address
+
+
+func get_afflvl_shift
+	cmp	x0, #3
+	cinc	x0, x0, eq
+	mov	x1, #MPIDR_AFFLVL_SHIFT
+	lsl	x0, x0, x1
+	ret
+
+func mpidr_mask_lower_afflvls
+	cmp	x1, #3
+	cinc	x1, x1, eq
+	mov	x2, #MPIDR_AFFLVL_SHIFT
+	lsl	x2, x1, x2
+	lsr	x0, x0, x2
+	lsl	x0, x0, x2
+	ret
+
+	/* -----------------------------------------------------
+	 * Asynchronous exception manipulation accessors
+	 * -----------------------------------------------------
+	 */
+func enable_irq
+	msr	daifclr, #DAIF_IRQ_BIT
+	ret
+
+
+func enable_fiq
+	msr	daifclr, #DAIF_FIQ_BIT
+	ret
+
+
+func enable_serror
+	msr	daifclr, #DAIF_ABT_BIT
+	ret
+
+
+func enable_debug_exceptions
+	msr	daifclr, #DAIF_DBG_BIT
+	ret
+
+
+func disable_irq
+	msr	daifset, #DAIF_IRQ_BIT
+	ret
+
+
+func disable_fiq
+	msr	daifset, #DAIF_FIQ_BIT
+	ret
+
+
+func disable_serror
+	msr	daifset, #DAIF_ABT_BIT
+	ret
+
+
+func disable_debug_exceptions
+	msr	daifset, #DAIF_DBG_BIT
+	ret
+
+
+func read_daif
+	mrs	x0, daif
+	ret
+
+
+func write_daif
+	msr	daif, x0
+	ret
+
+
+func read_spsr_el1
+	mrs	x0, spsr_el1
+	ret
+
+
+func read_spsr_el2
+	mrs	x0, spsr_el2
+	ret
+
+
+func read_spsr_el3
+	mrs	x0, spsr_el3
+	ret
+
+
+func write_spsr_el1
+	msr	spsr_el1, x0
+	ret
+
+
+func write_spsr_el2
+	msr	spsr_el2, x0
+	ret
+
+
+func write_spsr_el3
+	msr	spsr_el3, x0
+	ret
+
+
+func read_elr_el1
+	mrs	x0, elr_el1
+	ret
+
+
+func read_elr_el2
+	mrs	x0, elr_el2
+	ret
+
+
+func read_elr_el3
+	mrs	x0, elr_el3
+	ret
+
+
+func write_elr_el1
+	msr	elr_el1, x0
+	ret
+
+
+func write_elr_el2
+	msr	elr_el2, x0
+	ret
+
+
+func write_elr_el3
+	msr	elr_el3, x0
+	ret
+
+
+func dsb
+	dsb	sy
+	ret
+
+
+func isb
+	isb
+	ret
+
+
+func sev
+	sev
+	ret
+
+
+func wfe
+	wfe
+	ret
+
+
+func wfi
+	wfi
+	ret
+
+
+func eret
+	eret
+
+
+func smc
+	smc	#0
+
+/* -----------------------------------------------------------------------
+ * void zeromem16(void *mem, unsigned int length);
+ *
+ * Initialise a memory region to 0.
+ * The memory address must be 16-byte aligned.
+ * -----------------------------------------------------------------------
+ */
+func zeromem16
+	add	x2, x0, x1
+/* zero 16 bytes at a time */
+z_loop16:
+	sub	x3, x2, x0
+	cmp	x3, #16
+	b.lt	z_loop1
+	stp	xzr, xzr, [x0], #16
+	b	z_loop16
+/* zero byte per byte */
+z_loop1:
+	cmp	x0, x2
+	b.eq	z_end
+	strb	wzr, [x0], #1
+	b	z_loop1
+z_end:	ret
+
+
+/* --------------------------------------------------------------------------
+ * void memcpy16(void *dest, const void *src, unsigned int length)
+ *
+ * Copy length bytes from memory area src to memory area dest.
+ * The memory areas should not overlap.
+ * Destination and source addresses must be 16-byte aligned.
+ * --------------------------------------------------------------------------
+ */
+func memcpy16
+/* copy 16 bytes at a time */
+m_loop16:
+	cmp	x2, #16
+	b.lt	m_loop1
+	ldp	x3, x4, [x1], #16
+	stp	x3, x4, [x0], #16
+	sub	x2, x2, #16
+	b	m_loop16
+/* copy byte per byte */
+m_loop1:
+	cbz	x2, m_end
+	ldrb	w3, [x1], #1
+	strb	w3, [x0], #1
+	subs	x2, x2, #1
+	b.ne	m_loop1
+m_end:	ret
+
+/* ---------------------------------------------------------------------------
+ * Disable the MMU at EL3
+ * This is implemented in assembler to ensure that the data cache is cleaned
+ * and invalidated after the MMU is disabled without any intervening cacheable
+ * data accesses
+ * ---------------------------------------------------------------------------
+ */
+
+func disable_mmu_el3
+	mov	x1, #(SCTLR_M_BIT | SCTLR_C_BIT)
+do_disable_mmu:
+	mrs	x0, sctlr_el3
+	bic	x0, x0, x1
+	msr	sctlr_el3, x0
+	isb				// ensure MMU is off
+	mov	x0, #DCCISW		// DCache clean and invalidate
+	b	dcsw_op_all
+
+
+func disable_mmu_icache_el3
+	mov	x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
+	b	do_disable_mmu
+
+func remap_zero_address
+	ldr x1, =0xc1300000
+	ldr w0, =0x00000001
+	str w0, [x1]
+	isb
+	dmb sy
diff --git a/lib/aarch64/sysreg_helpers.S b/lib/aarch64/sysreg_helpers.S
new file mode 100644
index 0000000..925e93e
--- /dev/null
+++ b/lib/aarch64/sysreg_helpers.S
@@ -0,0 +1,782 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+	.globl	read_vbar_el1
+	.globl	read_vbar_el2
+	.globl	read_vbar_el3
+	.globl	write_vbar_el1
+	.globl	write_vbar_el2
+	.globl	write_vbar_el3
+
+	.globl	read_sctlr_el1
+	.globl	read_sctlr_el2
+	.globl	read_sctlr_el3
+	.globl	write_sctlr_el1
+	.globl	write_sctlr_el2
+	.globl	write_sctlr_el3
+
+	.globl	read_actlr_el1
+	.globl	read_actlr_el2
+	.globl	read_actlr_el3
+	.globl	write_actlr_el1
+	.globl	write_actlr_el2
+	.globl	write_actlr_el3
+
+	.globl	read_esr_el1
+	.globl	read_esr_el2
+	.globl	read_esr_el3
+	.globl	write_esr_el1
+	.globl	write_esr_el2
+	.globl	write_esr_el3
+
+	.globl	read_afsr0_el1
+	.globl	read_afsr0_el2
+	.globl	read_afsr0_el3
+	.globl	write_afsr0_el1
+	.globl	write_afsr0_el2
+	.globl	write_afsr0_el3
+
+	.globl	read_afsr1_el1
+	.globl	read_afsr1_el2
+	.globl	read_afsr1_el3
+	.globl	write_afsr1_el1
+	.globl	write_afsr1_el2
+	.globl	write_afsr1_el3
+
+	.globl	read_far_el1
+	.globl	read_far_el2
+	.globl	read_far_el3
+	.globl	write_far_el1
+	.globl	write_far_el2
+	.globl	write_far_el3
+
+	.globl	read_mair_el1
+	.globl	read_mair_el2
+	.globl	read_mair_el3
+	.globl	write_mair_el1
+	.globl	write_mair_el2
+	.globl	write_mair_el3
+
+	.globl	read_amair_el1
+	.globl	read_amair_el2
+	.globl	read_amair_el3
+	.globl	write_amair_el1
+	.globl	write_amair_el2
+	.globl	write_amair_el3
+
+	.globl	read_rvbar_el1
+	.globl	read_rvbar_el2
+	.globl	read_rvbar_el3
+
+	.globl	read_rmr_el1
+	.globl	read_rmr_el2
+	.globl	read_rmr_el3
+	.globl	write_rmr_el1
+	.globl	write_rmr_el2
+	.globl	write_rmr_el3
+
+	.globl	read_tcr_el1
+	.globl	read_tcr_el2
+	.globl	read_tcr_el3
+	.globl	write_tcr_el1
+	.globl	write_tcr_el2
+	.globl	write_tcr_el3
+
+	.globl	read_cptr_el2
+	.globl	read_cptr_el3
+	.globl	write_cptr_el2
+	.globl	write_cptr_el3
+
+	.globl	read_ttbr0_el1
+	.globl	read_ttbr0_el2
+	.globl	read_ttbr0_el3
+	.globl	write_ttbr0_el1
+	.globl	write_ttbr0_el2
+	.globl	write_ttbr0_el3
+
+	.globl	read_ttbr1_el1
+	.globl	write_ttbr1_el1
+
+	.globl	read_cpacr
+	.globl	write_cpacr
+
+	.globl	read_cntfrq
+	.globl	write_cntfrq
+
+	.globl	read_cpuectlr
+	.globl	write_cpuectlr
+
+	.globl	read_cnthctl_el2
+	.globl	write_cnthctl_el2
+
+	.globl	read_cntfrq_el0
+	.globl	write_cntfrq_el0
+
+	.globl	read_cntps_ctl_el1
+	.globl	write_cntps_ctl_el1
+
+	.globl	read_cntps_cval_el1
+	.globl	write_cntps_cval_el1
+
+	.globl	read_cntps_tval_el1
+	.globl	write_cntps_tval_el1
+
+	.globl	read_scr
+	.globl	write_scr
+
+	.globl	read_hcr
+	.globl	write_hcr
+
+	.globl	read_midr
+	.globl	read_mpidr
+
+	.globl	read_cntpct_el0
+	.globl	read_current_el
+	.globl	read_id_pfr1_el1
+	.globl	read_id_aa64pfr0_el1
+
+	.globl	write_tpidr_el3
+	.globl	read_tpidr_el3
+
+#if SUPPORT_VFP
+	.globl	enable_vfp
+#endif
+
+
+func read_current_el
+	mrs	x0, CurrentEl
+	ret
+
+
+func read_id_pfr1_el1
+	mrs	x0, id_pfr1_el1
+	ret
+
+
+func read_id_aa64pfr0_el1
+	mrs	x0, id_aa64pfr0_el1
+	ret
+
+
+	/* -----------------------------------------------------
+	 * VBAR accessors
+	 * -----------------------------------------------------
+	 */
+func read_vbar_el1
+	mrs	x0, vbar_el1
+	ret
+
+
+func read_vbar_el2
+	mrs	x0, vbar_el2
+	ret
+
+
+func read_vbar_el3
+	mrs	x0, vbar_el3
+	ret
+
+
+func write_vbar_el1
+	msr	vbar_el1, x0
+	ret
+
+
+func write_vbar_el2
+	msr	vbar_el2, x0
+	ret
+
+
+func write_vbar_el3
+	msr	vbar_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * AFSR0 accessors
+	 * -----------------------------------------------------
+	 */
+func read_afsr0_el1
+	mrs	x0, afsr0_el1
+	ret
+
+
+func read_afsr0_el2
+	mrs	x0, afsr0_el2
+	ret
+
+
+func read_afsr0_el3
+	mrs	x0, afsr0_el3
+	ret
+
+
+func write_afsr0_el1
+	msr	afsr0_el1, x0
+	ret
+
+
+func write_afsr0_el2
+	msr	afsr0_el2, x0
+	ret
+
+
+func write_afsr0_el3
+	msr	afsr0_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * FAR accessors
+	 * -----------------------------------------------------
+	 */
+func read_far_el1
+	mrs	x0, far_el1
+	ret
+
+
+func read_far_el2
+	mrs	x0, far_el2
+	ret
+
+
+func read_far_el3
+	mrs	x0, far_el3
+	ret
+
+
+func write_far_el1
+	msr	far_el1, x0
+	ret
+
+
+func write_far_el2
+	msr	far_el2, x0
+	ret
+
+
+func write_far_el3
+	msr	far_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * MAIR accessors
+	 * -----------------------------------------------------
+	 */
+func read_mair_el1
+	mrs	x0, mair_el1
+	ret
+
+
+func read_mair_el2
+	mrs	x0, mair_el2
+	ret
+
+
+func read_mair_el3
+	mrs	x0, mair_el3
+	ret
+
+
+func write_mair_el1
+	msr	mair_el1, x0
+	ret
+
+
+func write_mair_el2
+	msr	mair_el2, x0
+	ret
+
+
+func write_mair_el3
+	msr	mair_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * AMAIR accessors
+	 * -----------------------------------------------------
+	 */
+func read_amair_el1
+	mrs	x0, amair_el1
+	ret
+
+
+func read_amair_el2
+	mrs	x0, amair_el2
+	ret
+
+
+func read_amair_el3
+	mrs	x0, amair_el3
+	ret
+
+
+func write_amair_el1
+	msr	amair_el1, x0
+	ret
+
+
+func write_amair_el2
+	msr	amair_el2, x0
+	ret
+
+
+func write_amair_el3
+	msr	amair_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * RVBAR accessors
+	 * -----------------------------------------------------
+	 */
+func read_rvbar_el1
+	mrs	x0, rvbar_el1
+	ret
+
+
+func read_rvbar_el2
+	mrs	x0, rvbar_el2
+	ret
+
+
+func read_rvbar_el3
+	mrs	x0, rvbar_el3
+	ret
+
+
+	/* -----------------------------------------------------
+	 * RMR accessors
+	 * -----------------------------------------------------
+	 */
+func read_rmr_el1
+	mrs	x0, rmr_el1
+	ret
+
+
+func read_rmr_el2
+	mrs	x0, rmr_el2
+	ret
+
+
+func read_rmr_el3
+	mrs	x0, rmr_el3
+	ret
+
+
+func write_rmr_el1
+	msr	rmr_el1, x0
+	ret
+
+
+func write_rmr_el2
+	msr	rmr_el2, x0
+	ret
+
+
+func write_rmr_el3
+	msr	rmr_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * AFSR1 accessors
+	 * -----------------------------------------------------
+	 */
+func read_afsr1_el1
+	mrs	x0, afsr1_el1
+	ret
+
+
+func read_afsr1_el2
+	mrs	x0, afsr1_el2
+	ret
+
+
+func read_afsr1_el3
+	mrs	x0, afsr1_el3
+	ret
+
+
+func write_afsr1_el1
+	msr	afsr1_el1, x0
+	ret
+
+
+func write_afsr1_el2
+	msr	afsr1_el2, x0
+	ret
+
+
+func write_afsr1_el3
+	msr	afsr1_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * SCTLR accessors
+	 * -----------------------------------------------------
+	 */
+func read_sctlr_el1
+	mrs	x0, sctlr_el1
+	ret
+
+
+func read_sctlr_el2
+	mrs	x0, sctlr_el2
+	ret
+
+
+func read_sctlr_el3
+	mrs	x0, sctlr_el3
+	ret
+
+
+func write_sctlr_el1
+	msr	sctlr_el1, x0
+	ret
+
+
+func write_sctlr_el2
+	msr	sctlr_el2, x0
+	ret
+
+
+func write_sctlr_el3
+	msr	sctlr_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * ACTLR accessors
+	 * -----------------------------------------------------
+	 */
+func read_actlr_el1
+	mrs	x0, actlr_el1
+	ret
+
+
+func read_actlr_el2
+	mrs	x0, actlr_el2
+	ret
+
+
+func read_actlr_el3
+	mrs	x0, actlr_el3
+	ret
+
+
+func write_actlr_el1
+	msr	actlr_el1, x0
+	ret
+
+
+func write_actlr_el2
+	msr	actlr_el2, x0
+	ret
+
+
+func write_actlr_el3
+	msr	actlr_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * ESR accessors
+	 * -----------------------------------------------------
+	 */
+func read_esr_el1
+	mrs	x0, esr_el1
+	ret
+
+
+func read_esr_el2
+	mrs	x0, esr_el2
+	ret
+
+
+func read_esr_el3
+	mrs	x0, esr_el3
+	ret
+
+
+func write_esr_el1
+	msr	esr_el1, x0
+	ret
+
+
+func write_esr_el2
+	msr	esr_el2, x0
+	ret
+
+
+func write_esr_el3
+	msr	esr_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * TCR accessors
+	 * -----------------------------------------------------
+	 */
+func read_tcr_el1
+	mrs	x0, tcr_el1
+	ret
+
+
+func read_tcr_el2
+	mrs	x0, tcr_el2
+	ret
+
+
+func read_tcr_el3
+	mrs	x0, tcr_el3
+	ret
+
+
+func write_tcr_el1
+	msr	tcr_el1, x0
+	ret
+
+
+func write_tcr_el2
+	msr	tcr_el2, x0
+	ret
+
+
+func write_tcr_el3
+	msr	tcr_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * CPTR accessors
+	 * -----------------------------------------------------
+	 */
+func read_cptr_el2
+	mrs	x0, cptr_el2
+	ret
+
+
+func read_cptr_el3
+	mrs	x0, cptr_el3
+	ret
+
+
+func write_cptr_el2
+	msr	cptr_el2, x0
+	ret
+
+
+func write_cptr_el3
+	msr	cptr_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * TTBR0 accessors
+	 * -----------------------------------------------------
+	 */
+func read_ttbr0_el1
+	mrs	x0, ttbr0_el1
+	ret
+
+
+func read_ttbr0_el2
+	mrs	x0, ttbr0_el2
+	ret
+
+
+func read_ttbr0_el3
+	mrs	x0, ttbr0_el3
+	ret
+
+
+func write_ttbr0_el1
+	msr	ttbr0_el1, x0
+	ret
+
+
+func write_ttbr0_el2
+	msr	ttbr0_el2, x0
+	ret
+
+
+func write_ttbr0_el3
+	msr	ttbr0_el3, x0
+	ret
+
+
+	/* -----------------------------------------------------
+	 * TTBR1 accessors
+	 * -----------------------------------------------------
+	 */
+func read_ttbr1_el1
+	mrs	x0, ttbr1_el1
+	ret
+
+
+func write_ttbr1_el1
+	msr	ttbr1_el1, x0
+	ret
+
+
+func read_hcr
+	mrs	x0, hcr_el2
+	ret
+
+
+func write_hcr
+	msr	hcr_el2, x0
+	ret
+
+
+func read_cpacr
+	mrs	x0, cpacr_el1
+	ret
+
+
+func write_cpacr
+	msr	cpacr_el1, x0
+	ret
+
+
+func read_cntfrq_el0
+	mrs	x0, cntfrq_el0
+	ret
+
+
+func write_cntfrq_el0
+	msr	cntfrq_el0, x0
+	ret
+
+func read_cntps_ctl_el1
+	mrs	x0, cntps_ctl_el1
+	ret
+
+func write_cntps_ctl_el1
+	msr	cntps_ctl_el1, x0
+	ret
+
+func read_cntps_cval_el1
+	mrs	x0, cntps_cval_el1
+	ret
+
+func write_cntps_cval_el1
+	msr	cntps_cval_el1, x0
+	ret
+
+func read_cntps_tval_el1
+	mrs	x0, cntps_tval_el1
+	ret
+
+func write_cntps_tval_el1
+	msr	cntps_tval_el1, x0
+	ret
+
+func read_cntpct_el0
+	mrs	x0, cntpct_el0
+	ret
+
+func read_cpuectlr
+	mrs	x0, CPUECTLR_EL1
+	ret
+
+
+func write_cpuectlr
+	msr	CPUECTLR_EL1, x0
+	ret
+
+
+func read_cnthctl_el2
+	mrs	x0, cnthctl_el2
+	ret
+
+
+func write_cnthctl_el2
+	msr	cnthctl_el2, x0
+	ret
+
+
+func read_cntfrq
+	mrs	x0, cntfrq_el0
+	ret
+
+
+func write_cntfrq
+	msr	cntfrq_el0, x0
+	ret
+
+
+func write_scr
+	msr	scr_el3, x0
+	ret
+
+
+func read_scr
+	mrs	x0, scr_el3
+	ret
+
+
+func read_midr
+	mrs	x0, midr_el1
+	ret
+
+
+func read_mpidr
+	mrs	x0, mpidr_el1
+	ret
+
+func write_tpidr_el3
+	msr	tpidr_el3, x0
+	ret
+
+func read_tpidr_el3
+	mrs	x0, tpidr_el3
+	ret
+
+#if SUPPORT_VFP
+func enable_vfp
+	mrs	x0, cpacr_el1
+	orr	x0, x0, #CPACR_VFP_BITS
+	msr	cpacr_el1, x0
+	mrs	x0, cptr_el3
+	mov	x1, #AARCH64_CPTR_TFP
+	bic	x0, x0, x1
+	msr	cptr_el3, x0
+	isb
+	ret
+
+#endif
diff --git a/lib/aarch64/tlb_helpers.S b/lib/aarch64/tlb_helpers.S
new file mode 100644
index 0000000..8dfae12
--- /dev/null
+++ b/lib/aarch64/tlb_helpers.S
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm_macros.S>
+
+	.globl	tlbialle1
+	.globl	tlbialle1is
+	.globl	tlbialle2
+	.globl	tlbialle2is
+	.globl	tlbialle3
+	.globl	tlbialle3is
+	.globl	tlbivmalle1
+
+
+func tlbialle1
+	tlbi	alle1
+	ret
+
+
+func tlbialle1is
+	tlbi	alle1is
+	ret
+
+
+func tlbialle2
+	tlbi	alle2
+	ret
+
+
+func tlbialle2is
+	tlbi	alle2is
+	ret
+
+
+func tlbialle3
+	tlbi	alle3
+	ret
+
+
+func tlbialle3is
+	tlbi	alle3is
+	ret
+
+func tlbivmalle1
+	tlbi	vmalle1
+	ret
diff --git a/lib/aarch64/xlat_helpers.c b/lib/aarch64/xlat_helpers.c
new file mode 100644
index 0000000..d401ffc
--- /dev/null
+++ b/lib/aarch64/xlat_helpers.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <assert.h>
+
+/*******************************************************************************
+ * Helper to create a level 1/2 table descriptor which points to a level 2/3
+ * table.
+ ******************************************************************************/
+unsigned long create_table_desc(unsigned long *next_table_ptr)
+{
+	unsigned long desc = (unsigned long) next_table_ptr;
+
+	/* Clear the last 12 bits */
+	desc >>= FOUR_KB_SHIFT;
+	desc <<= FOUR_KB_SHIFT;
+
+	desc |= TABLE_DESC;
+
+	return desc;
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to addr
+ ******************************************************************************/
+unsigned long create_block_desc(unsigned long desc,
+				unsigned long addr,
+				unsigned int level)
+{
+	switch (level) {
+	case LEVEL1:
+		desc |= (addr << FIRST_LEVEL_DESC_N) | BLOCK_DESC;
+		break;
+	case LEVEL2:
+		desc |= (addr << SECOND_LEVEL_DESC_N) | BLOCK_DESC;
+		break;
+	case LEVEL3:
+		desc |= (addr << THIRD_LEVEL_DESC_N) | TABLE_DESC;
+		break;
+	default:
+		assert(0);
+	}
+
+	return desc;
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to output_
+ * addr with Device nGnRE attributes.
+ ******************************************************************************/
+unsigned long create_device_block(unsigned long output_addr,
+				  unsigned int level,
+				  unsigned int ns)
+{
+	unsigned long upper_attrs, lower_attrs, desc;
+
+	lower_attrs = LOWER_ATTRS(ACCESS_FLAG | OSH | AP_RW);
+	lower_attrs |= LOWER_ATTRS(ns | ATTR_DEVICE_INDEX);
+	upper_attrs = UPPER_ATTRS(XN);
+	desc = upper_attrs | lower_attrs;
+
+	return create_block_desc(desc, output_addr, level);
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to output_
+ * addr with inner-shareable normal wbwa read-only memory attributes.
+ ******************************************************************************/
+unsigned long create_romem_block(unsigned long output_addr,
+				 unsigned int level,
+				 unsigned int ns)
+{
+	unsigned long upper_attrs, lower_attrs, desc;
+
+	lower_attrs = LOWER_ATTRS(ACCESS_FLAG | ISH | AP_RO);
+	lower_attrs |= LOWER_ATTRS(ns | ATTR_IWBWA_OWBWA_NTR_INDEX);
+	upper_attrs = UPPER_ATTRS(0ull);
+	desc = upper_attrs | lower_attrs;
+
+	return create_block_desc(desc, output_addr, level);
+}
+
+/*******************************************************************************
+ * Helper to create a level 1/2/3 block descriptor which maps the va to output_
+ * addr with inner-shareable normal wbwa read-write memory attributes.
+ ******************************************************************************/
+unsigned long create_rwmem_block(unsigned long output_addr,
+				 unsigned int level,
+				 unsigned int ns)
+{
+	unsigned long upper_attrs, lower_attrs, desc;
+
+	lower_attrs = LOWER_ATTRS(ACCESS_FLAG | ISH | AP_RW);
+	lower_attrs |= LOWER_ATTRS(ns | ATTR_IWBWA_OWBWA_NTR_INDEX);
+	upper_attrs = UPPER_ATTRS(XN);
+	desc = upper_attrs | lower_attrs;
+
+	return create_block_desc(desc, output_addr, level);
+}
diff --git a/lib/aarch64/xlat_tables.c b/lib/aarch64/xlat_tables.c
new file mode 100644
index 0000000..6e8df59
--- /dev/null
+++ b/lib/aarch64/xlat_tables.c
@@ -0,0 +1,298 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <assert.h>
+#include <platform_def.h>
+#include <string.h>
+#include <xlat_tables.h>
+#include <stdio.h>
+#include <fip.h>
+
+#ifndef DEBUG_XLAT_TABLE
+#define DEBUG_XLAT_TABLE 0
+#endif
+
+#if DEBUG_XLAT_TABLE
+#define debug_print(...) printf(__VA_ARGS__)
+#else
+#define debug_print(...) ((void)0)
+#endif
+
+
+#define UNSET_DESC	~0ul
+
+#define NUM_L1_ENTRIES (ADDR_SPACE_SIZE >> L1_XLAT_ADDRESS_SHIFT)
+
+static uint64_t l1_xlation_table[NUM_L1_ENTRIES]
+__aligned(NUM_L1_ENTRIES * sizeof(uint64_t));
+
+static uint64_t **xlat_tables;
+//__aligned(XLAT_TABLE_SIZE) __attribute__((section("xlat_table")));
+
+static unsigned next_xlat;
+
+/*
+ * Array of all memory regions stored in order of ascending base address.
+ * The list is terminated by the first entry with size == 0.
+ */
+static mmap_region_t mmap[MAX_MMAP_REGIONS + 1];
+
+static void print_mmap(void)
+{
+#if DEBUG_XLAT_TABLE
+	debug_print("mmap:\n");
+	mmap_region_t *mm = mmap;
+	while (mm->size) {
+		debug_print(" %010lx %10lx %x\n", mm->base, mm->size, mm->attr);
+		++mm;
+	};
+	debug_print("\n");
+#endif
+}
+
+void mmap_add_region(unsigned long base, unsigned long size, unsigned attr)
+{
+	mmap_region_t *mm = mmap;
+	mmap_region_t *mm_last = mm + sizeof(mmap) / sizeof(mmap[0]) - 1;
+
+	assert(IS_PAGE_ALIGNED(base));
+	assert(IS_PAGE_ALIGNED(size));
+
+	if (!size)
+		return;
+
+	/* Find correct place in mmap to insert new region */
+	while (mm->base < base && mm->size)
+		++mm;
+
+	/* Make room for new region by moving other regions up by one place */
+	memmove(mm + 1, mm, (uintptr_t)mm_last - (uintptr_t)mm);
+
+	/* Check we haven't lost the empty sentinal from the end of the array */
+	assert(mm_last->size == 0);
+
+	mm->base = base;
+	mm->size = size;
+	mm->attr = attr;
+}
+
+void mmap_add(const mmap_region_t *mm)
+{
+	while (mm->size) {
+		mmap_add_region(mm->base, mm->size, mm->attr);
+		++mm;
+	}
+}
+
+static unsigned long mmap_desc(unsigned attr, unsigned long addr,
+					unsigned level)
+{
+	unsigned long desc = addr;
+
+	desc |= level == 3 ? TABLE_DESC : BLOCK_DESC;
+
+	desc |= attr & MT_NS ? LOWER_ATTRS(NS) : 0;
+
+	desc |= attr & MT_RW ? LOWER_ATTRS(AP_RW) : LOWER_ATTRS(AP_RO);
+
+	desc |= LOWER_ATTRS(ACCESS_FLAG);
+
+	if (attr & MT_MEMORY) {
+		desc |= LOWER_ATTRS(ATTR_IWBWA_OWBWA_NTR_INDEX | ISH);
+		if (attr & MT_RW)
+			desc |= UPPER_ATTRS(XN);
+	} else {
+		desc |= LOWER_ATTRS(ATTR_DEVICE_INDEX | OSH);
+		desc |= UPPER_ATTRS(XN);
+	}
+
+	debug_print(attr & MT_MEMORY ? "MEM" : "DEV");
+	debug_print(attr & MT_RW ? "-RW" : "-RO");
+	debug_print(attr & MT_NS ? "-NS" : "-S");
+
+	return desc;
+}
+
+static int mmap_region_attr(mmap_region_t *mm, unsigned long base,
+					unsigned long size)
+{
+	int attr = mm->attr;
+
+	for (;;) {
+		++mm;
+
+		if (!mm->size)
+			return attr; /* Reached end of list */
+
+		if (mm->base >= base + size)
+			return attr; /* Next region is after area so end */
+
+		if (mm->base + mm->size <= base)
+			continue; /* Next region has already been overtaken */
+
+		if ((mm->attr & attr) == attr)
+			continue; /* Region doesn't override attribs so skip */
+
+		attr &= mm->attr;
+
+		if (mm->base > base || mm->base + mm->size < base + size)
+			return -1; /* Region doesn't fully cover our area */
+	}
+}
+
+static mmap_region_t *init_xlation_table(mmap_region_t *mm, unsigned long base,
+					unsigned long *table, unsigned level)
+{
+	unsigned level_size_shift = L1_XLAT_ADDRESS_SHIFT - (level - 1) *
+						XLAT_TABLE_ENTRIES_SHIFT;
+	unsigned level_size = 1 << level_size_shift;
+	unsigned long level_index_mask = XLAT_TABLE_ENTRIES_MASK << level_size_shift;
+
+	assert(level <= 3);
+
+	debug_print("New xlat table:\n");
+
+	do  {
+		unsigned long desc = UNSET_DESC;
+
+		if (mm->base + mm->size <= base) {
+			/* Area now after the region so skip it */
+			++mm;
+			continue;
+		}
+
+		debug_print("      %010lx %8lx " + 6 - 2 * level, base, level_size);
+
+		if (mm->base >= base + level_size) {
+			/* Next region is after area so nothing to map yet */
+			desc = INVALID_DESC;
+		} else if (mm->base <= base &&
+				mm->base + mm->size >= base + level_size) {
+			/* Next region covers all of area */
+			int attr = mmap_region_attr(mm, base, level_size);
+			if (attr >= 0)
+				desc = mmap_desc(attr, base, level);
+		}
+		/* else Next region only partially covers area, so need */
+
+		if (desc == UNSET_DESC) {
+			/* Area not covered by a region so need finer table */
+			unsigned long *new_table = xlat_tables[next_xlat++];
+			assert(next_xlat <= MAX_XLAT_TABLES);
+			desc = TABLE_DESC | (unsigned long)new_table;
+
+			/* Recurse to fill in new table */
+			mm = init_xlation_table(mm, base, new_table, level+1);
+		}
+
+		debug_print("\n");
+
+		*table++ = desc;
+		base += level_size;
+	} while (mm->size && (base & level_index_mask));
+
+	return mm;
+}
+
+void init_xlat_tables(void)
+{
+	/* move mmu table to ddr*/
+	memset((void *)MMU_TABLE_BASE, 0, MMU_TABLE_SIZE);
+	uint64_t * xlat_tables_x[MAX_XLAT_TABLES] = {0};
+	uint32_t loop = 0;
+	for (loop=0; loop<MAX_XLAT_TABLES; loop++) {
+		xlat_tables_x[loop] = (uint64_t *)(uint64_t)(MMU_TABLE_BASE + loop * MMU_TABLE_SIZE);
+	}
+	xlat_tables = xlat_tables_x;
+
+	print_mmap();
+	init_xlation_table(mmap, 0, l1_xlation_table, 1);
+}
+
+/*******************************************************************************
+ * Macro generating the code for the function enabling the MMU in the given
+ * exception level, assuming that the pagetables have already been created.
+ *
+ *   _el:		Exception level at which the function will run
+ *   _tcr_extra:	Extra bits to set in the TCR register. This mask will
+ *			be OR'ed with the default TCR value.
+ *   _tlbi_fct:		Function to invalidate the TLBs at the current
+ *			exception level
+ ******************************************************************************/
+#define DEFINE_ENABLE_MMU_EL(_el, _tcr_extra, _tlbi_fct)		\
+	void enable_mmu_el##_el(void)					\
+	{								\
+		uint64_t mair, tcr, ttbr;				\
+		uint32_t sctlr;						\
+									\
+		assert(IS_IN_EL(_el));					\
+		assert((read_sctlr_el##_el() & SCTLR_M_BIT) == 0);	\
+									\
+		/* Set attributes in the right indices of the MAIR */	\
+		mair = MAIR_ATTR_SET(ATTR_DEVICE, ATTR_DEVICE_INDEX);	\
+		mair |= MAIR_ATTR_SET(ATTR_IWBWA_OWBWA_NTR,		\
+				ATTR_IWBWA_OWBWA_NTR_INDEX);		\
+		write_mair_el##_el(mair);				\
+									\
+		/* Invalidate TLBs at the current exception level */	\
+		_tlbi_fct();						\
+									\
+		/* Set TCR bits as well. */				\
+		/* Inner & outer WBWA & shareable + T0SZ = 32 */	\
+		tcr = TCR_SH_INNER_SHAREABLE | TCR_RGN_OUTER_WBA |	\
+			TCR_RGN_INNER_WBA | TCR_T0SZ_4GB;		\
+		tcr |= _tcr_extra;					\
+		write_tcr_el##_el(tcr);					\
+									\
+		/* Set TTBR bits as well */				\
+		ttbr = (uint64_t) l1_xlation_table;			\
+		write_ttbr0_el##_el(ttbr);				\
+									\
+		/* Ensure all translation table writes have drained */	\
+		/* into memory, the TLB invalidation is complete, */	\
+		/* and translation register writes are committed */	\
+		/* before enabling the MMU */				\
+		dsb();							\
+		isb();							\
+									\
+		sctlr = read_sctlr_el##_el();				\
+		sctlr |= SCTLR_WXN_BIT | SCTLR_I_BIT;	\
+		sctlr |= SCTLR_A_BIT | SCTLR_C_BIT;			\
+		write_sctlr_el##_el(sctlr);				\
+									\
+		/* Ensure the MMU enable takes effect immediately */	\
+		isb();							\
+	}
+
+/* Define EL1 and EL3 variants of the function enabling the MMU */
+DEFINE_ENABLE_MMU_EL(1, 0, tlbivmalle1)
+DEFINE_ENABLE_MMU_EL(3, TCR_EL3_RES1, tlbialle3)
diff --git a/lib/io_storage.c b/lib/io_storage.c
new file mode 100644
index 0000000..204310a
--- /dev/null
+++ b/lib/io_storage.c
@@ -0,0 +1,386 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <io_driver.h>
+#include <io_storage.h>
+#include <stddef.h>
+
+
+#define MAX_DEVICES(plat_data)						\
+	(sizeof((plat_data)->devices)/sizeof((plat_data)->devices[0]))
+
+
+/* Storage for a fixed maximum number of IO entities, definable by platform */
+static io_entity_t entity_pool[MAX_IO_HANDLES];
+
+/* Simple way of tracking used storage - each entry is NULL or a pointer to an
+ * entity */
+static io_entity_t *entity_map[MAX_IO_HANDLES];
+
+/* Track number of allocated entities */
+static unsigned int entity_count;
+
+
+/* Used to keep a reference to platform-specific data */
+static io_plat_data_t *platform_data;
+
+
+#if DEBUG	/* Extra validation functions only used in debug builds */
+
+/* Return a boolean value indicating whether a device connector is valid */
+static int is_valid_dev_connector(const io_dev_connector_t *dev_con)
+{
+	int result = (dev_con != NULL) && (dev_con->dev_open != NULL);
+	return result;
+}
+
+
+/* Return a boolean value indicating whether a device handle is valid */
+static int is_valid_dev(const uintptr_t dev_handle)
+{
+	const io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+	int result = (dev != NULL) && (dev->funcs != NULL) &&
+			(dev->funcs->type != NULL) &&
+			(dev->funcs->type() < IO_TYPE_MAX);
+	return result;
+}
+
+
+/* Return a boolean value indicating whether an IO entity is valid */
+static int is_valid_entity(const uintptr_t handle)
+{
+	const io_entity_t *entity = (io_entity_t *)handle;
+	int result = (entity != NULL) &&
+			(is_valid_dev((uintptr_t)entity->dev_handle));
+	return result;
+}
+
+
+/* Return a boolean value indicating whether a seek mode is valid */
+static int is_valid_seek_mode(io_seek_mode_t mode)
+{
+	return ((mode != IO_SEEK_INVALID) && (mode < IO_SEEK_MAX));
+}
+
+#endif	/* End of debug-only validation functions */
+
+
+/* Open a connection to a specific device */
+static int dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
+		io_dev_info_t **dev_info)
+{
+	int result = IO_FAIL;
+	assert(dev_info != NULL);
+	assert(is_valid_dev_connector(dev_con));
+
+	result = dev_con->dev_open(dev_spec, dev_info);
+	return result;
+}
+
+
+/* Set a handle to track an entity */
+static void set_handle(uintptr_t *handle, io_entity_t *entity)
+{
+	assert(handle != NULL);
+	*handle = (uintptr_t)entity;
+}
+
+
+/* Locate an entity in the pool, specified by address */
+static int find_first_entity(const io_entity_t *entity, unsigned int *index_out)
+{
+	int result = IO_FAIL;
+	for (int index = 0; index < MAX_IO_HANDLES; ++index) {
+		if (entity_map[index] == entity) {
+			result = IO_SUCCESS;
+			*index_out = index;
+			break;
+		}
+	}
+	return result;
+}
+
+
+/* Allocate an entity from the pool and return a pointer to it */
+static int allocate_entity(io_entity_t **entity)
+{
+	int result = IO_FAIL;
+	assert(entity != NULL);
+
+	if (entity_count < MAX_IO_HANDLES) {
+		unsigned int index = 0;
+		result = find_first_entity(NULL, &index);
+		assert(result == IO_SUCCESS);
+		*entity = entity_map[index] = &entity_pool[index];
+		++entity_count;
+	} else
+		result = IO_RESOURCES_EXHAUSTED;
+
+	return result;
+}
+
+
+/* Release an entity back to the pool */
+static int free_entity(const io_entity_t *entity)
+{
+	int result = IO_FAIL;
+	unsigned int index = 0;
+	assert(entity != NULL);
+
+	result = find_first_entity(entity, &index);
+	if (result ==  IO_SUCCESS) {
+		entity_map[index] = NULL;
+		--entity_count;
+	}
+
+	return result;
+}
+
+
+/* Exported API */
+
+
+/* Initialise the IO layer */
+void io_init(io_plat_data_t *data)
+{
+	assert(data != NULL);
+	platform_data = data;
+}
+
+
+/* Register a device driver */
+int io_register_device(const io_dev_info_t *dev_info)
+{
+	int result = IO_FAIL;
+	assert(dev_info != NULL);
+	assert(platform_data != NULL);
+
+	unsigned int dev_count = platform_data->dev_count;
+
+	if (dev_count < MAX_DEVICES(platform_data)) {
+		platform_data->devices[dev_count] = dev_info;
+		platform_data->dev_count++;
+		result = IO_SUCCESS;
+	} else {
+		result = IO_RESOURCES_EXHAUSTED;
+	}
+
+	return result;
+}
+
+
+/* Open a connection to an IO device */
+int io_dev_open(const io_dev_connector_t *dev_con, const uintptr_t dev_spec,
+		uintptr_t *handle)
+{
+	int result = IO_FAIL;
+	assert(handle != NULL);
+
+	result = dev_open(dev_con, dev_spec, (io_dev_info_t **)handle);
+	return result;
+}
+
+
+/* Initialise an IO device explicitly - to permit lazy initialisation or
+ * re-initialisation */
+int io_dev_init(uintptr_t dev_handle, const uintptr_t init_params)
+{
+	int result = IO_FAIL;
+	assert(dev_handle != (uintptr_t)NULL);
+	assert(is_valid_dev(dev_handle));
+
+	io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+	if (dev->funcs->dev_init != NULL) {
+		result = dev->funcs->dev_init(dev, init_params);
+	} else {
+		/* Absence of registered function implies NOP here */
+		result = IO_SUCCESS;
+	}
+	return result;
+}
+
+
+/* TODO: Consider whether an explicit "shutdown" API should be included */
+
+/* Close a connection to a device */
+int io_dev_close(uintptr_t dev_handle)
+{
+	int result = IO_FAIL;
+	assert(dev_handle != (uintptr_t)NULL);
+	assert(is_valid_dev(dev_handle));
+
+	io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+
+	if (dev->funcs->dev_close != NULL) {
+		result = dev->funcs->dev_close(dev);
+	} else {
+		/* Absence of registered function implies NOP here */
+		result = IO_SUCCESS;
+	}
+
+	return result;
+}
+
+
+/* Synchronous operations */
+
+
+/* Open an IO entity */
+int io_open(uintptr_t dev_handle, const uintptr_t spec, uintptr_t *handle)
+{
+	int result = IO_FAIL;
+	assert((spec != (uintptr_t)NULL) && (handle != NULL));
+	assert(is_valid_dev(dev_handle));
+
+	io_dev_info_t *dev = (io_dev_info_t *)dev_handle;
+	io_entity_t *entity;
+
+	result = allocate_entity(&entity);
+
+	if (result == IO_SUCCESS) {
+		assert(dev->funcs->open != NULL);
+		result = dev->funcs->open(dev, spec, entity);
+
+		if (result == IO_SUCCESS) {
+			entity->dev_handle = dev;
+			set_handle(handle, entity);
+		} else
+			free_entity(entity);
+	}
+	return result;
+}
+
+
+/* Seek to a specific position in an IO entity */
+int io_seek(uintptr_t handle, io_seek_mode_t mode, ssize_t offset)
+{
+	int result = IO_FAIL;
+	assert(is_valid_entity(handle) && is_valid_seek_mode(mode));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->seek != NULL)
+		result = dev->funcs->seek(entity, mode, offset);
+	else
+		result = IO_NOT_SUPPORTED;
+
+	return result;
+}
+
+
+/* Determine the length of an IO entity */
+int io_size(uintptr_t handle, size_t *length)
+{
+	int result = IO_FAIL;
+	assert(is_valid_entity(handle) && (length != NULL));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->size != NULL)
+		result = dev->funcs->size(entity, length);
+	else
+		result = IO_NOT_SUPPORTED;
+
+	return result;
+}
+
+
+/* Read data from an IO entity */
+int io_read(uintptr_t handle,
+		uintptr_t buffer,
+		size_t length,
+		size_t *length_read)
+{
+	int result = IO_FAIL;
+	assert(is_valid_entity(handle) && (buffer != (uintptr_t)NULL));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->read != NULL)
+		result = dev->funcs->read(entity, buffer, length, length_read);
+	else
+		result = IO_NOT_SUPPORTED;
+
+	return result;
+}
+
+
+/* Write data to an IO entity */
+int io_write(uintptr_t handle,
+		const uintptr_t buffer,
+		size_t length,
+		size_t *length_written)
+{
+	int result = IO_FAIL;
+	assert(is_valid_entity(handle) && (buffer != (uintptr_t)NULL));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->write != NULL) {
+		result = dev->funcs->write(entity, buffer, length,
+				length_written);
+	} else
+		result = IO_NOT_SUPPORTED;
+
+	return result;
+}
+
+
+/* Close an IO entity */
+int io_close(uintptr_t handle)
+{
+	int result = IO_FAIL;
+	assert(is_valid_entity(handle));
+
+	io_entity_t *entity = (io_entity_t *)handle;
+
+	io_dev_info_t *dev = entity->dev_handle;
+
+	if (dev->funcs->close != NULL)
+		result = dev->funcs->close(entity);
+	else {
+		/* Absence of registered function implies NOP here */
+		result = IO_SUCCESS;
+	}
+	/* Ignore improbable free_entity failure */
+	(void)free_entity(entity);
+
+	return result;
+}
diff --git a/lib/locks/bakery/bakery_lock.c b/lib/locks/bakery/bakery_lock.c
new file mode 100644
index 0000000..4e148b5
--- /dev/null
+++ b/lib/locks/bakery/bakery_lock.c
@@ -0,0 +1,193 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <bakery_lock.h>
+#include <platform.h>
+#include <string.h>
+
+/*
+ * Functions in this file implement Bakery Algorithm for mutual exclusion.
+ *
+ * ARM architecture offers a family of exclusive access instructions to
+ * efficiently implement mutual exclusion with hardware support. However, as
+ * well as depending on external hardware, the these instructions have defined
+ * behavior only on certain memory types (cacheable and Normal memory in
+ * particular; see ARMv8 Architecture Reference Manual section B2.10). Use cases
+ * in trusted firmware are such that mutual exclusion implementation cannot
+ * expect that accesses to the lock have the specific type required by the
+ * architecture for these primitives to function (for example, not all
+ * contenders may have address translation enabled).
+ *
+ * This implementation does not use mutual exclusion primitives. It expects
+ * memory regions where the locks reside to be fully ordered and coherent
+ * (either by disabling address translation, or by assigning proper attributes
+ * when translation is enabled).
+ *
+ * Note that the ARM architecture guarantees single-copy atomicity for aligned
+ * accesses regardless of status of address translation.
+ */
+
+#define assert_bakery_entry_valid(entry, bakery) do {	\
+	assert(bakery);					\
+	assert(entry < BAKERY_LOCK_MAX_CPUS);		\
+} while (0)
+
+/* Convert a ticket to priority */
+#define PRIORITY(t, pos)	(((t) << 8) | (pos))
+
+
+/* Initialize Bakery Lock to reset ownership and all ticket values */
+void bakery_lock_init(bakery_lock_t *bakery)
+{
+	assert(bakery);
+
+	/* All ticket values need to be 0 */
+	memset(bakery, 0, sizeof(*bakery));
+	bakery->owner = NO_OWNER;
+}
+
+
+/* Obtain a ticket for a given CPU */
+static unsigned int bakery_get_ticket(bakery_lock_t *bakery, unsigned int me)
+{
+	unsigned int my_ticket, their_ticket;
+	unsigned int they;
+
+	/*
+	 * Flag that we're busy getting our ticket. All CPUs are iterated in the
+	 * order of their ordinal position to decide the maximum ticket value
+	 * observed so far. Our priority is set to be greater than the maximum
+	 * observed priority
+	 *
+	 * Note that it's possible that more than one contender gets the same
+	 * ticket value. That's OK as the lock is acquired based on the priority
+	 * value, not the ticket value alone.
+	 */
+	my_ticket = 0;
+	bakery->entering[me] = 1;
+	for (they = 0; they < BAKERY_LOCK_MAX_CPUS; they++) {
+		their_ticket = bakery->number[they];
+		if (their_ticket > my_ticket)
+			my_ticket = their_ticket;
+	}
+
+	/*
+	 * Compute ticket; then signal to other contenders waiting for us to
+	 * finish calculating our ticket value that we're done
+	 */
+	++my_ticket;
+	bakery->number[me] = my_ticket;
+	bakery->entering[me] = 0;
+	sev();
+
+	return my_ticket;
+}
+
+
+/*
+ * Acquire bakery lock
+ *
+ * Contending CPUs need first obtain a non-zero ticket and then calculate
+ * priority value. A contending CPU iterate over all other CPUs in the platform,
+ * which may be contending for the same lock, in the order of their ordinal
+ * position (CPU0, CPU1 and so on). A non-contending CPU will have its ticket
+ * (and priority) value as 0. The contending CPU compares its priority with that
+ * of others'. The CPU with the highest priority (lowest numerical value)
+ * acquires the lock
+ */
+void bakery_lock_get(unsigned long mpidr, bakery_lock_t *bakery)
+{
+	unsigned int they, me;
+	unsigned int my_ticket, my_prio, their_ticket;
+
+	me = platform_get_core_pos(mpidr);
+
+	assert_bakery_entry_valid(me, bakery);
+
+	/* Prevent recursive acquisition */
+	assert(bakery->owner != me);
+
+	/* Get a ticket */
+	my_ticket = bakery_get_ticket(bakery, me);
+
+	/*
+	 * Now that we got our ticket, compute our priority value, then compare
+	 * with that of others, and proceed to acquire the lock
+	 */
+	my_prio = PRIORITY(my_ticket, me);
+	for (they = 0; they < BAKERY_LOCK_MAX_CPUS; they++) {
+		if (me == they)
+			continue;
+
+		/* Wait for the contender to get their ticket */
+		while (bakery->entering[they])
+			wfe();
+
+		/*
+		 * If the other party is a contender, they'll have non-zero
+		 * (valid) ticket value. If they do, compare priorities
+		 */
+		their_ticket = bakery->number[they];
+		if (their_ticket && (PRIORITY(their_ticket, they) < my_prio)) {
+			/*
+			 * They have higher priority (lower value). Wait for
+			 * their ticket value to change (either release the lock
+			 * to have it dropped to 0; or drop and probably content
+			 * again for the same lock to have an even higher value)
+			 */
+			do {
+				wfe();
+			} while (their_ticket == bakery->number[they]);
+		}
+	}
+
+	/* Lock acquired */
+	bakery->owner = me;
+}
+
+
+/* Release the lock and signal contenders */
+void bakery_lock_release(unsigned long mpidr, bakery_lock_t *bakery)
+{
+	unsigned int me = platform_get_core_pos(mpidr);
+
+	assert_bakery_entry_valid(me, bakery);
+	assert(bakery->owner == me);
+
+	/*
+	 * Release lock by resetting ownership and ticket. Then signal other
+	 * waiting contenders
+	 */
+	bakery->owner = NO_OWNER;
+	bakery->number[me] = 0;
+	sev();
+}
diff --git a/lib/locks/exclusive/spinlock.S b/lib/locks/exclusive/spinlock.S
new file mode 100644
index 0000000..5eae2b0
--- /dev/null
+++ b/lib/locks/exclusive/spinlock.S
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm_macros.S>
+
+	.globl	spin_lock
+	.globl	spin_unlock
+
+
+func spin_lock
+	mov	w2, #1
+	sevl
+l1:	wfe
+l2:	ldaxr	w1, [x0]
+	cbnz	w1, l1
+	stxr	w1, w2, [x0]
+	cbnz	w1, l2
+	ret
+
+
+func spin_unlock
+	stlr	wzr, [x0]
+	ret
diff --git a/lib/mmio.c b/lib/mmio.c
new file mode 100644
index 0000000..1350609
--- /dev/null
+++ b/lib/mmio.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdint.h>
+
+void mmio_write_8(uintptr_t addr, uint8_t value)
+{
+	*(volatile uint8_t*)addr = value;
+}
+
+uint8_t mmio_read_8(uintptr_t addr)
+{
+	return *(volatile uint8_t*)addr;
+}
+
+void mmio_write_32(uintptr_t addr, uint32_t value)
+{
+	*(volatile uint32_t*)addr = value;
+}
+
+uint32_t mmio_read_32(uintptr_t addr)
+{
+	return *(volatile uint32_t*)addr;
+}
+
+void mmio_write_64(uintptr_t addr, uint64_t value)
+{
+	*(volatile uint64_t*)addr = value;
+}
+
+uint64_t mmio_read_64(uintptr_t addr)
+{
+	return *(volatile uint64_t*)addr;
+}
diff --git a/lib/semihosting/aarch64/semihosting_call.S b/lib/semihosting/aarch64/semihosting_call.S
new file mode 100644
index 0000000..e6a9675
--- /dev/null
+++ b/lib/semihosting/aarch64/semihosting_call.S
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm_macros.S>
+
+	.globl	semihosting_call
+
+func semihosting_call
+	hlt	#0xf000
+	ret
diff --git a/lib/semihosting/semihosting.c b/lib/semihosting/semihosting.c
new file mode 100644
index 0000000..849ec12
--- /dev/null
+++ b/lib/semihosting/semihosting.c
@@ -0,0 +1,238 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <semihosting.h>
+#include <string.h>
+
+#ifndef SEMIHOSTING_SUPPORTED
+#define SEMIHOSTING_SUPPORTED  1
+#endif
+
+long semihosting_call(unsigned long operation,
+			void *system_block_address);
+
+typedef struct {
+	const char *file_name;
+	unsigned long mode;
+	size_t name_length;
+} smh_file_open_block_t;
+
+typedef struct {
+	long handle;
+	uintptr_t buffer;
+	size_t length;
+} smh_file_read_write_block_t;
+
+typedef struct {
+	long handle;
+	ssize_t location;
+} smh_file_seek_block_t;
+
+typedef struct {
+	char *command_line;
+	size_t command_length;
+} smh_system_block_t;
+
+long semihosting_connection_supported(void)
+{
+	return SEMIHOSTING_SUPPORTED;
+}
+
+long semihosting_file_open(const char *file_name, size_t mode)
+{
+	smh_file_open_block_t open_block;
+
+	open_block.file_name = file_name;
+	open_block.mode = mode;
+	open_block.name_length = strlen(file_name);
+
+	return semihosting_call(SEMIHOSTING_SYS_OPEN,
+				(void *) &open_block);
+}
+
+long semihosting_file_seek(long file_handle, ssize_t offset)
+{
+	smh_file_seek_block_t seek_block;
+	long result;
+
+	seek_block.handle = file_handle;
+	seek_block.location = offset;
+
+	result = semihosting_call(SEMIHOSTING_SYS_SEEK,
+				  (void *) &seek_block);
+
+	if (result)
+		result = semihosting_call(SEMIHOSTING_SYS_ERRNO, 0);
+
+	return result;
+}
+
+long semihosting_file_read(long file_handle, size_t *length, uintptr_t buffer)
+{
+	smh_file_read_write_block_t read_block;
+	long result = -EINVAL;
+
+	if ((length == NULL) || (buffer == (uintptr_t)NULL))
+		return result;
+
+	read_block.handle = file_handle;
+	read_block.buffer = buffer;
+	read_block.length = *length;
+
+	result = semihosting_call(SEMIHOSTING_SYS_READ,
+				  (void *) &read_block);
+
+	if (result == *length) {
+		return -EINVAL;
+	} else if (result < *length) {
+		*length -= result;
+		return 0;
+	} else
+		return result;
+}
+
+long semihosting_file_write(long file_handle,
+			    size_t *length,
+			    const uintptr_t buffer)
+{
+	smh_file_read_write_block_t write_block;
+
+	if ((length == NULL) || (buffer == (uintptr_t)NULL))
+		return -EINVAL;
+
+	write_block.handle = file_handle;
+	write_block.buffer = (uintptr_t)buffer; /* cast away const */
+	write_block.length = *length;
+
+	*length = semihosting_call(SEMIHOSTING_SYS_WRITE,
+				   (void *) &write_block);
+
+	return *length;
+}
+
+long semihosting_file_close(long file_handle)
+{
+	return semihosting_call(SEMIHOSTING_SYS_CLOSE,
+				(void *) &file_handle);
+}
+
+long semihosting_file_length(long file_handle)
+{
+	return semihosting_call(SEMIHOSTING_SYS_FLEN,
+				(void *) &file_handle);
+}
+
+char semihosting_read_char(void)
+{
+	return semihosting_call(SEMIHOSTING_SYS_READC, NULL);
+}
+
+void semihosting_write_char(char character)
+{
+	semihosting_call(SEMIHOSTING_SYS_WRITEC, (void *) &character);
+}
+
+void semihosting_write_string(char *string)
+{
+	semihosting_call(SEMIHOSTING_SYS_WRITE0, (void *) string);
+}
+
+long semihosting_system(char *command_line)
+{
+	smh_system_block_t system_block;
+
+	system_block.command_line = command_line;
+	system_block.command_length = strlen(command_line);
+
+	return semihosting_call(SEMIHOSTING_SYS_SYSTEM,
+				(void *) &system_block);
+}
+
+long semihosting_get_flen(const char *file_name)
+{
+	long file_handle;
+	size_t length;
+
+	assert(semihosting_connection_supported());
+
+	file_handle = semihosting_file_open(file_name, FOPEN_MODE_RB);
+	if (file_handle == -1)
+		return file_handle;
+
+	/* Find the length of the file */
+	length = semihosting_file_length(file_handle);
+
+	return semihosting_file_close(file_handle) ? -1 : length;
+}
+
+long semihosting_download_file(const char *file_name,
+			      size_t buf_size,
+			      uintptr_t buf)
+{
+	long ret = -EINVAL;
+	size_t length;
+	long file_handle;
+
+	/* Null pointer check */
+	if (!buf)
+		return ret;
+
+	assert(semihosting_connection_supported());
+
+	file_handle = semihosting_file_open(file_name, FOPEN_MODE_RB);
+	if (file_handle == -1)
+		return ret;
+
+	/* Find the actual length of the file */
+	length = semihosting_file_length(file_handle);
+	if (length == -1)
+		goto semihosting_fail;
+
+	/* Signal error if we do not have enough space for the file */
+	if (length > buf_size)
+		goto semihosting_fail;
+
+	/*
+	 * A successful read will return 0 in which case we pass back
+	 * the actual number of bytes read. Else we pass a negative
+	 * value indicating an error.
+	 */
+	ret = semihosting_file_read(file_handle, &length, buf);
+	if (ret)
+		goto semihosting_fail;
+	else
+		ret = length;
+
+semihosting_fail:
+	semihosting_file_close(file_handle);
+	return ret;
+}
diff --git a/lib/stdlib/abort.c b/lib/stdlib/abort.c
new file mode 100644
index 0000000..1b06132
--- /dev/null
+++ b/lib/stdlib/abort.c
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+
+/*
+ * This is a basic implementation. This could be improved.
+ */
+void abort (void)
+{
+	serial_puts("ABORT\n\r");
+	while (1) ;
+}
diff --git a/lib/stdlib/assert.c b/lib/stdlib/assert.c
new file mode 100644
index 0000000..0a5ae43
--- /dev/null
+++ b/lib/stdlib/assert.c
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+
+/*
+ * This is a basic implementation. This could be improved.
+ */
+void __assert (const char *function, const char *file, unsigned int line,
+		const char *assertion)
+{
+	serial_puts("ASSERT: ");
+	serial_puts(function);
+	serial_puts(" <");
+	serial_put_dec(line);
+	serial_puts("> : ");
+	serial_puts(assertion);
+	serial_puts("\n\r");
+	while (1) ;
+}
diff --git a/lib/stdlib/mem.c b/lib/stdlib/mem.c
new file mode 100644
index 0000000..f1f335a
--- /dev/null
+++ b/lib/stdlib/mem.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stddef.h> /* size_t */
+
+/*
+ * Fill @count bytes of memory pointed to by @dst with @val
+ */
+void *memset(void *dst, int val, size_t count)
+{
+	char *ptr = dst;
+
+	while (count--)
+		*ptr++ = val;
+
+	return dst;
+}
+
+/*
+ * Compare @len bytes of @s1 and @s2
+ */
+int memcmp(const void *s1, const void *s2, size_t len)
+{
+	const char *s = s1;
+	const char *d = s2;
+	char dc;
+	char sc;
+
+	while (len--) {
+		sc = *s++;
+		dc = *d++;
+		if (sc - dc)
+			return (sc - dc);
+	}
+
+	return 0;
+}
+
+/*
+ * Copy @len bytes from @src to @dst
+ */
+void *memcpy(void *dst, const void *src, size_t len)
+{
+	const char *s = src;
+	char *d = dst;
+
+	while (len--)
+		*d++ = *s++;
+
+	return dst;
+}
+
+/*
+ * Move @len bytes from @src to @dst
+ */
+void *memmove(void *dst, const void *src, size_t len)
+{
+	/*
+	 * The following test makes use of unsigned arithmetic overflow to
+	 * more efficiently test the condition !(src <= dst && dst < str+len).
+	 * It also avoids the situation where the more explicit test would give
+	 * incorrect results were the calculation str+len to overflow (though
+	 * that issue is probably moot as such usage is probably undefined
+	 * behaviour and a bug anyway.
+	 */
+	if ((size_t)dst - (size_t)src >= len) {
+		/* destination not in source data, so can safely use memcpy */
+		return memcpy(dst, src, len);
+	} else {
+		/* copy backwards... */
+		const char *end = dst;
+		const char *s = (const char *)src + len;
+		char *d = (char *)dst + len;
+		while (d != end)
+			*--d = *--s;
+	}
+	return dst;
+}
+
+/*
+ * Scan @len bytes of @src for value @c
+ */
+void *memchr(const void *src, int c, size_t len)
+{
+	const char *s = src;
+
+	while (len--) {
+		if (*s == c)
+			return (void *) s;
+		s++;
+	}
+
+	return NULL;
+}
diff --git a/lib/stdlib/printf.c b/lib/stdlib/printf.c
new file mode 100644
index 0000000..d339668
--- /dev/null
+++ b/lib/stdlib/printf.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#ifdef ENABLE_PRINTF
+/* Choose max of 128 chars for now. */
+#define PRINT_BUFFER_SIZE 128
+int printf(const char *fmt, ...)
+{
+	va_list args;
+	char buf[PRINT_BUFFER_SIZE];
+	int count;
+
+	va_start(args, fmt);
+	vsnprintf(buf, sizeof(buf) - 1, fmt, args);
+	va_end(args);
+
+	/* Use putchar directly as 'puts()' adds a newline. */
+	buf[PRINT_BUFFER_SIZE - 1] = '\0';
+	count = 0;
+	while (buf[count])
+	{
+		if (putchar(buf[count]) != EOF) {
+			count++;
+		} else {
+			count = EOF;
+			break;
+		}
+	}
+
+	return count;
+}
+#endif
\ No newline at end of file
diff --git a/lib/stdlib/putchar.c b/lib/stdlib/putchar.c
new file mode 100644
index 0000000..81375bb
--- /dev/null
+++ b/lib/stdlib/putchar.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <serial.h>
+
+/* Putchar() should either return the character printed or EOF in case of error.
+ * Our current console_putc() function assumes success and returns the
+ * character. Write all other printing functions in terms of putchar(), if
+ * possible, so they all benefit when this is improved.
+ */
+int putchar(int c)
+{
+	int res;
+	if (serial_putc((unsigned char)c) >= 0)
+		res = c;
+	else
+		res = EOF;
+
+	return res;
+}
diff --git a/lib/stdlib/puts.c b/lib/stdlib/puts.c
new file mode 100644
index 0000000..542902e
--- /dev/null
+++ b/lib/stdlib/puts.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+
+int puts(const char *s)
+{
+	int count = 0;
+	while (*s)
+	{
+		if (putchar(*s++) != EOF) {
+			count++;
+		} else {
+			count = EOF;
+			break;
+		}
+	}
+
+	/* According to the puts(3) manpage, the function should write a
+	 * trailing newline.
+	 */
+	if ((count != EOF) && (putchar('\n') != EOF))
+		count++;
+	else
+		count = EOF;
+
+	return count;
+}
diff --git a/lib/stdlib/std.c b/lib/stdlib/std.c
new file mode 100644
index 0000000..4608754
--- /dev/null
+++ b/lib/stdlib/std.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/* Include the various implemented functions */
+#include "abort.c"
+#include "assert.c"
+#include "mem.c"
+#include "printf.c"
+#include "putchar.c"
+#include "puts.c"
+#include "strchr.c"
+#include "strcmp.c"
+#include "strlen.c"
+#include "strncmp.c"
+#include "subr_prf.c"
diff --git a/lib/stdlib/strchr.c b/lib/stdlib/strchr.c
new file mode 100644
index 0000000..4247dcd
--- /dev/null
+++ b/lib/stdlib/strchr.c
@@ -0,0 +1,52 @@
+/*-
+ * Copyright (c) 1990, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * Portions copyright (c) 2013-2014, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
+#include <sys/cdefs.h>
+#include <stddef.h>
+#include <string.h>
+
+char *
+strchr(const char *p, int ch)
+{
+	char c;
+
+	c = ch;
+	for (;; ++p) {
+		if (*p == c)
+			return ((char *)p);
+		if (*p == '\0')
+			return (NULL);
+	}
+	/* NOTREACHED */
+}
diff --git a/lib/stdlib/strcmp.c b/lib/stdlib/strcmp.c
new file mode 100644
index 0000000..1d26f2b
--- /dev/null
+++ b/lib/stdlib/strcmp.c
@@ -0,0 +1,51 @@
+/*-
+ * Copyright (c) 1990, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * This code is derived from software contributed to Berkeley by
+ * Chris Torek.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * Portions copyright (c) 2014, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
+#include <sys/cdefs.h>
+#include <string.h>
+
+/*
+ * Compare strings.
+ */
+int
+strcmp(const char *s1, const char *s2)
+{
+	while (*s1 == *s2++)
+		if (*s1++ == '\0')
+			return 0;
+	return *(const unsigned char *)s1 - *(const unsigned char *)(s2 - 1);
+}
diff --git a/lib/stdlib/strlen.c b/lib/stdlib/strlen.c
new file mode 100644
index 0000000..23c3d39
--- /dev/null
+++ b/lib/stdlib/strlen.c
@@ -0,0 +1,44 @@
+/*-
+ * Copyright (c) 1990, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * Portions copyright (c) 2009-2014, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <stddef.h>
+
+size_t
+strlen(str)
+	const char *str;
+{
+	register const char *s;
+
+	for (s = str; *s; ++s);
+	return(s - str);
+}
diff --git a/lib/stdlib/strncmp.c b/lib/stdlib/strncmp.c
new file mode 100644
index 0000000..f45f4a2
--- /dev/null
+++ b/lib/stdlib/strncmp.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 1989, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/*
+ * Portions copyright (c) 2014, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
+#include <sys/cdefs.h>
+#include <string.h>
+
+int
+strncmp(const char *s1, const char *s2, size_t n)
+{
+
+	if (n == 0)
+		return 0;
+	do {
+		if (*s1 != *s2++)
+			return (*(const unsigned char *)s1 -
+				*(const unsigned char *)(s2 - 1));
+		if (*s1++ == '\0')
+			break;
+	} while (--n != 0);
+	return 0;
+}
diff --git a/lib/stdlib/subr_prf.c b/lib/stdlib/subr_prf.c
new file mode 100644
index 0000000..97d6fad
--- /dev/null
+++ b/lib/stdlib/subr_prf.c
@@ -0,0 +1,552 @@
+/*-
+ * Copyright (c) 1986, 1988, 1991, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ * (c) UNIX System Laboratories, Inc.
+ * All or some portions of this file are derived from material licensed
+ * to the University of California by American Telephone and Telegraph
+ * Co. or Unix System Laboratories, Inc. and are reproduced herein with
+ * the permission of UNIX System Laboratories, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ *	@(#)subr_prf.c	8.3 (Berkeley) 1/21/94
+ */
+
+/*
+ * Portions copyright (c) 2009-2014, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdarg.h>
+#include <stddef.h>
+#include <string.h>
+#include <ctype.h>
+
+typedef unsigned char u_char;
+typedef unsigned int u_int;
+typedef int64_t quad_t;
+typedef uint64_t u_quad_t;
+typedef unsigned long u_long;
+typedef unsigned short u_short;
+
+static inline int imax(int a, int b) { return (a > b ? a : b); }
+
+/*
+ * Note that stdarg.h and the ANSI style va_start macro is used for both
+ * ANSI and traditional C compilers.
+ */
+
+#define TOCONS	0x01
+#define TOTTY	0x02
+#define TOLOG	0x04
+
+/* Max number conversion buffer length: a u_quad_t in base 2, plus NUL byte. */
+#define MAXNBUF	(sizeof(intmax_t) * 8 + 1)
+
+struct putchar_arg {
+	int	flags;
+	int	pri;
+	struct	tty *tty;
+	char	*p_bufr;
+	size_t	n_bufr;
+	char	*p_next;
+	size_t	remain;
+};
+
+struct snprintf_arg {
+	char	*str;
+	size_t	remain;
+};
+
+extern	int log_open;
+
+#ifdef ENABLE_PRINTF
+
+static char *ksprintn(char *nbuf, uintmax_t num, int base, int *len, int upper);
+static void  snprintf_func(int ch, void *arg);
+static int kvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap);
+
+int vsnprintf(char *str, size_t size, const char *format, va_list ap);
+
+static char const hex2ascii_data[] = "0123456789abcdefghijklmnopqrstuvwxyz";
+#define hex2ascii(hex) (hex2ascii_data[hex])
+
+/*
+ * Scaled down version of sprintf(3).
+ */
+int
+sprintf(char *buf, const char *cfmt, ...)
+{
+	int retval;
+	va_list ap;
+
+	va_start(ap, cfmt);
+	retval = kvprintf(cfmt, NULL, (void *)buf, 10, ap);
+	buf[retval] = '\0';
+	va_end(ap);
+	return (retval);
+}
+
+/*
+ * Scaled down version of vsprintf(3).
+ */
+int
+vsprintf(char *buf, const char *cfmt, va_list ap)
+{
+	int retval;
+
+	retval = kvprintf(cfmt, NULL, (void *)buf, 10, ap);
+	buf[retval] = '\0';
+	return (retval);
+}
+
+/*
+ * Scaled down version of snprintf(3).
+ */
+int
+snprintf(char *str, size_t size, const char *format, ...)
+{
+	int retval;
+	va_list ap;
+
+	va_start(ap, format);
+	retval = vsnprintf(str, size, format, ap);
+	va_end(ap);
+	return(retval);
+}
+
+/*
+ * Scaled down version of vsnprintf(3).
+ */
+int
+vsnprintf(char *str, size_t size, const char *format, va_list ap)
+{
+	struct snprintf_arg info;
+	int retval;
+
+	info.str = str;
+	info.remain = size;
+	retval = kvprintf(format, snprintf_func, &info, 10, ap);
+	if (info.remain >= 1)
+		*info.str++ = '\0';
+	return (retval);
+}
+
+static void
+snprintf_func(int ch, void *arg)
+{
+	struct snprintf_arg *const info = arg;
+
+	if (info->remain >= 2) {
+		*info->str++ = ch;
+		info->remain--;
+	}
+}
+
+
+/*
+ * Kernel version which takes radix argument vsnprintf(3).
+ */
+int
+vsnrprintf(char *str, size_t size, int radix, const char *format, va_list ap)
+{
+	struct snprintf_arg info;
+	int retval;
+
+	info.str = str;
+	info.remain = size;
+	retval = kvprintf(format, snprintf_func, &info, radix, ap);
+	if (info.remain >= 1)
+		*info.str++ = '\0';
+	return (retval);
+}
+
+
+/*
+ * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
+ * order; return an optional length and a pointer to the last character
+ * written in the buffer (i.e., the first character of the string).
+ * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
+ */
+static char *
+ksprintn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
+{
+	char *p, c;
+
+	p = nbuf;
+	*p = '\0';
+	do {
+		c = hex2ascii(num % base);
+		*++p = upper ? toupper(c) : c;
+	} while (num /= base);
+	if (lenp)
+		*lenp = p - nbuf;
+	return (p);
+}
+
+/*
+ * Scaled down version of printf(3).
+ *
+ * Two additional formats:
+ *
+ * The format %b is supported to decode error registers.
+ * Its usage is:
+ *
+ *	printf("reg=%b\n", regval, "<base><arg>*");
+ *
+ * where <base> is the output base expressed as a control character, e.g.
+ * \10 gives octal; \20 gives hex.  Each arg is a sequence of characters,
+ * the first of which gives the bit number to be inspected (origin 1), and
+ * the next characters (up to a control character, i.e. a character <= 32),
+ * give the name of the register.  Thus:
+ *
+ *	kvprintf("reg=%b\n", 3, "\10\2BITTWO\1BITONE\n");
+ *
+ * would produce output:
+ *
+ *	reg=3<BITTWO,BITONE>
+ *
+ * XXX:  %D  -- Hexdump, takes pointer and separator string:
+ *		("%6D", ptr, ":")   -> XX:XX:XX:XX:XX:XX
+ *		("%*D", len, ptr, " " -> XX XX XX XX ...
+ */
+int
+kvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap)
+{
+#define PCHAR(c) {int cc=(c); if (func) (*func)(cc,arg); else *d++ = cc; retval++; }
+	char nbuf[MAXNBUF];
+	char *d;
+	const char *p, *percent, *q;
+	u_char *up;
+	int ch, n;
+	uintmax_t num;
+	int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
+	int cflag, hflag, jflag, tflag, zflag;
+	int dwidth, upper;
+	char padc;
+	int stop = 0, retval = 0;
+
+	num = 0;
+	if (!func)
+		d = (char *) arg;
+	else
+		d = NULL;
+
+	if (fmt == NULL)
+		fmt = "(fmt null)\n";
+
+	if (radix < 2 || radix > 36)
+		radix = 10;
+
+	for (;;) {
+		padc = ' ';
+		width = 0;
+		while ((ch = (u_char)*fmt++) != '%' || stop) {
+			if (ch == '\0')
+				return (retval);
+			PCHAR(ch);
+		}
+		percent = fmt - 1;
+		qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
+		sign = 0; dot = 0; dwidth = 0; upper = 0;
+		cflag = 0; hflag = 0; jflag = 0; tflag = 0; zflag = 0;
+reswitch:	switch (ch = (u_char)*fmt++) {
+		case '.':
+			dot = 1;
+			goto reswitch;
+		case '#':
+			sharpflag = 1;
+			goto reswitch;
+		case '+':
+			sign = 1;
+			goto reswitch;
+		case '-':
+			ladjust = 1;
+			goto reswitch;
+		case '%':
+			PCHAR(ch);
+			break;
+		case '*':
+			if (!dot) {
+				width = va_arg(ap, int);
+				if (width < 0) {
+					ladjust = !ladjust;
+					width = -width;
+				}
+			} else {
+				dwidth = va_arg(ap, int);
+			}
+			goto reswitch;
+		case '0':
+			if (!dot) {
+				padc = '0';
+				goto reswitch;
+			}
+		case '1': case '2': case '3': case '4':
+		case '5': case '6': case '7': case '8': case '9':
+				for (n = 0;; ++fmt) {
+					n = n * 10 + ch - '0';
+					ch = *fmt;
+					if (ch < '0' || ch > '9')
+						break;
+				}
+			if (dot)
+				dwidth = n;
+			else
+				width = n;
+			goto reswitch;
+		case 'b':
+			num = (u_int)va_arg(ap, int);
+			p = va_arg(ap, char *);
+			for (q = ksprintn(nbuf, num, *p++, NULL, 0); *q;)
+				PCHAR(*q--);
+
+			if (num == 0)
+				break;
+
+			for (tmp = 0; *p;) {
+				n = *p++;
+				if (num & (1 << (n - 1))) {
+					PCHAR(tmp ? ',' : '<');
+					for (; (n = *p) > ' '; ++p)
+						PCHAR(n);
+					tmp = 1;
+				} else
+					for (; *p > ' '; ++p)
+						continue;
+			}
+			if (tmp)
+				PCHAR('>');
+			break;
+		case 'c':
+			PCHAR(va_arg(ap, int));
+			break;
+		case 'D':
+			up = va_arg(ap, u_char *);
+			p = va_arg(ap, char *);
+			if (!width)
+				width = 16;
+			while (width--) {
+				PCHAR(hex2ascii(*up >> 4));
+				PCHAR(hex2ascii(*up & 0x0f));
+				up++;
+				if (width)
+					for (q=p;*q;q++)
+						PCHAR(*q);
+			}
+			break;
+		case 'd':
+		case 'i':
+			base = 10;
+			sign = 1;
+			goto handle_sign;
+		case 'h':
+			if (hflag) {
+				hflag = 0;
+				cflag = 1;
+			} else
+				hflag = 1;
+			goto reswitch;
+		case 'j':
+			jflag = 1;
+			goto reswitch;
+		case 'l':
+			if (lflag) {
+				lflag = 0;
+				qflag = 1;
+			} else
+				lflag = 1;
+			goto reswitch;
+		case 'n':
+			if (jflag)
+				*(va_arg(ap, intmax_t *)) = retval;
+			else if (qflag)
+				*(va_arg(ap, quad_t *)) = retval;
+			else if (lflag)
+				*(va_arg(ap, long *)) = retval;
+			else if (zflag)
+				*(va_arg(ap, size_t *)) = retval;
+			else if (hflag)
+				*(va_arg(ap, short *)) = retval;
+			else if (cflag)
+				*(va_arg(ap, char *)) = retval;
+			else
+				*(va_arg(ap, int *)) = retval;
+			break;
+		case 'o':
+			base = 8;
+			goto handle_nosign;
+		case 'p':
+			base = 16;
+			sharpflag = (width == 0);
+			sign = 0;
+			num = (uintptr_t)va_arg(ap, void *);
+			goto number;
+		case 'q':
+			qflag = 1;
+			goto reswitch;
+		case 'r':
+			base = radix;
+			if (sign)
+				goto handle_sign;
+			goto handle_nosign;
+		case 's':
+			p = va_arg(ap, char *);
+			if (p == NULL)
+				p = "(null)";
+			if (!dot)
+				n = strlen (p);
+			else
+				for (n = 0; n < dwidth && p[n]; n++)
+					continue;
+
+			width -= n;
+
+			if (!ladjust && width > 0)
+				while (width--)
+					PCHAR(padc);
+			while (n--)
+				PCHAR(*p++);
+			if (ladjust && width > 0)
+				while (width--)
+					PCHAR(padc);
+			break;
+		case 't':
+			tflag = 1;
+			goto reswitch;
+		case 'u':
+			base = 10;
+			goto handle_nosign;
+		case 'X':
+			upper = 1;
+		case 'x':
+			base = 16;
+			goto handle_nosign;
+		case 'y':
+			base = 16;
+			sign = 1;
+			goto handle_sign;
+		case 'z':
+			zflag = 1;
+			goto reswitch;
+handle_nosign:
+			sign = 0;
+			if (jflag)
+				num = va_arg(ap, uintmax_t);
+			else if (qflag)
+				num = va_arg(ap, u_quad_t);
+			else if (tflag)
+				num = va_arg(ap, ptrdiff_t);
+			else if (lflag)
+				num = va_arg(ap, u_long);
+			else if (zflag)
+				num = va_arg(ap, size_t);
+			else if (hflag)
+				num = (u_short)va_arg(ap, int);
+			else if (cflag)
+				num = (u_char)va_arg(ap, int);
+			else
+				num = va_arg(ap, u_int);
+			goto number;
+handle_sign:
+			if (jflag)
+				num = va_arg(ap, intmax_t);
+			else if (qflag)
+				num = va_arg(ap, quad_t);
+			else if (tflag)
+				num = va_arg(ap, ptrdiff_t);
+			else if (lflag)
+				num = va_arg(ap, long);
+			else if (zflag)
+				num = va_arg(ap, ssize_t);
+			else if (hflag)
+				num = (short)va_arg(ap, int);
+			else if (cflag)
+				num = (char)va_arg(ap, int);
+			else
+				num = va_arg(ap, int);
+number:
+			if (sign && (intmax_t)num < 0) {
+				neg = 1;
+				num = -(intmax_t)num;
+			}
+			p = ksprintn(nbuf, num, base, &n, upper);
+			tmp = 0;
+			if (sharpflag && num != 0) {
+				if (base == 8)
+					tmp++;
+				else if (base == 16)
+					tmp += 2;
+			}
+			if (neg)
+				tmp++;
+
+			if (!ladjust && padc == '0')
+				dwidth = width - tmp;
+			width -= tmp + imax(dwidth, n);
+			dwidth -= n;
+			if (!ladjust)
+				while (width-- > 0)
+					PCHAR(' ');
+			if (neg)
+				PCHAR('-');
+			if (sharpflag && num != 0) {
+				if (base == 8) {
+					PCHAR('0');
+				} else if (base == 16) {
+					PCHAR('0');
+					PCHAR('x');
+				}
+			}
+			while (dwidth-- > 0)
+				PCHAR('0');
+
+			while (*p)
+				PCHAR(*p--);
+
+			if (ladjust)
+				while (width-- > 0)
+					PCHAR(' ');
+
+			break;
+		default:
+			while (percent < fmt)
+				PCHAR(*percent++);
+			/*
+			 * Since we ignore an formatting argument it is no
+			 * longer safe to obey the remaining formatting
+			 * arguments as the arguments will no longer match
+			 * the format specs.
+			 */
+			stop = 1;
+			break;
+		}
+	}
+#undef PCHAR
+}
+
+#endif
\ No newline at end of file