Rewrite platform initialisation in C

Add two assembly helpers for 32-bit MMIO accesses, and translate
platform initialisation to C. Since we can now add features without too
much pain, also output a string describing the boot-wrapper version.

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Signed-off-by: Mark Rutland <mark.rutland@arm.com>

4 files changed, 102 insertions, 48 deletions

diff --git a/Makefile.am b/Makefile.am
index 929df7a..ec7ed8b 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -90,8 +90,8 @@ CPPFLAGS	+= $(INITRD_FLAGS)
 CFLAGS		+= -Iinclude/ -I$(ARCH_SRC)/include/
 CFLAGS		+= -Wall -fomit-frame-pointer
 
-OFILES		+= boot_common.o
-OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o $(GIC) mmu.o ns.o $(BOOTMETHOD) utils.o)
+OFILES		+= boot_common.o ns.o
+OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o $(GIC) mmu.o $(BOOTMETHOD) utils.o)
 
 all: $(IMAGE)
 
diff --git a/arch/aarch64/include/asm/io.h b/arch/aarch64/include/asm/io.h
new file mode 100644
index 0000000..9aefe0f
--- /dev/null
+++ b/arch/aarch64/include/asm/io.h
@@ -0,0 +1,31 @@
+/*
+ * arch/aarch64/include/asm/io.h
+ *
+ * Copyright (C) 2015 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+#ifndef __ASM_AARCH64_IO_H
+#define __ASM_AARCH64_IO_H
+
+#include <stdint.h>
+
+#ifndef __ASSEMBLY__
+
+static inline void raw_writel(uint32_t val, void *addr)
+{
+	asm volatile ("str %w0, [%1]\n" : : "r" (val), "r" (addr));
+}
+
+static inline uint32_t raw_readl(void *addr)
+{
+	uint32_t val;
+
+	asm volatile ("ldr %w0, [%1]\n" : "=r" (val) : "r" (addr));
+	return val;
+}
+
+#endif /* !__ASSEMBLY__ */
+
+#endif
diff --git a/arch/aarch64/ns.S b/arch/aarch64/ns.S
deleted file mode 100644
index 054dab4..0000000
--- a/arch/aarch64/ns.S
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * arch/aarch64/ns.S - code to initialise everything required when first booting non-secure.
- *
- * Copyright (C) 2013 ARM Limited. All rights reserved.
- *
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE.txt file.
- */
-
-#define PL011_UARTIBRD	0x24
-#define PL011_UARTFBRD	0x28
-#define PL011_UART_LCR_H	0x2c
-#define PL011_UARTCR	0x30
-
-#define V2M_SYS_CFGDATA	0xa0
-#define V2M_SYS_CFGCTRL	0xa4
-
-	.text
-	.globl ns_init_system
-
-ns_init_system:
-	/*
-	 * UART initialisation (38400 8N1)
-	 */
-	ldr	x4, =UART_BASE
-	mov	w5, #0x10
-	str	w5, [x4, #PL011_UARTIBRD]
-	str	wzr, [x4, #PL011_UARTFBRD]
-	/* set parameters to 8N1 and enable the FIFOs */
-	mov	w5, #0x70
-	str	w5, [x4, #PL011_UART_LCR_H]
-	/* enable the UART, TXen and RXen */
-	mov	w5, #0x301
-	str	w5, [x4, #PL011_UARTCR]
-
-	/*
-	 * CLCD output site MB
-	 */
-	ldr	x4, =SYSREGS_BASE
-	ldr	w5, =(1 << 31) | (1 << 30) | (7 << 20) | (0 << 16)	// START|WRITE|MUXFPGA|SITE_MB
-	str	wzr, [x4, #V2M_SYS_CFGDATA]
-	str	w5, [x4, #V2M_SYS_CFGCTRL]
-
-	ret
-
-	.ltorg
diff --git a/ns.c b/ns.c
new file mode 100644
index 0000000..28f4376
--- /dev/null
+++ b/ns.c
@@ -0,0 +1,69 @@
+/*
+ * ns.c - code to initialise everything required when first booting.
+ *
+ * Copyright (C) 2015 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+
+#include <stdint.h>
+
+#include <asm/io.h>
+
+#define PL011_UARTDR		0x00
+#define PL011_UARTFR		0x18
+#define PL011_UARTIBRD		0x24
+#define PL011_UARTFBRD		0x28
+#define PL011_UART_LCR_H	0x2c
+#define PL011_UARTCR		0x30
+
+#define PL011_UARTFR_BUSY	(1 << 3)
+#define PL011_UARTFR_FIFO_FULL	(1 << 5)
+
+#define PL011(reg)	((void *)UART_BASE + PL011_##reg)
+
+#define V2M_SYS_CFGDATA		0xa0
+#define V2M_SYS_CFGCTRL		0xa4
+
+#define V2M_SYS(reg)	((void *)SYSREGS_BASE + V2M_SYS_##reg)
+
+static void print_string(const char *str)
+{
+	uint32_t flags;
+
+	while (*str) {
+		do
+			flags = raw_readl(PL011(UARTFR));
+		while (flags & PL011_UARTFR_FIFO_FULL);
+
+		raw_writel(*str++, PL011(UARTDR));
+
+		do
+			flags = raw_readl(PL011(UARTFR));
+		while (flags & PL011_UARTFR_BUSY);
+	}
+}
+
+void ns_init_system(void)
+{
+	/*
+	 * UART initialisation (38400 8N1)
+	 */
+	raw_writel(0x10,	PL011(UARTIBRD));
+	raw_writel(0x0,		PL011(UARTFBRD));
+	/* Set parameters to 8N1 and enable the FIFOs */
+	raw_writel(0x70,	PL011(UART_LCR_H));
+	/* Enable the UART, TXen and RXen */
+	raw_writel(0x301,	PL011(UARTCR));
+
+	print_string("Boot-wrapper v0.1\r\n\r\n");
+
+	/*
+	 * CLCD output site MB
+	 */
+	raw_writel(0x0,		V2M_SYS(CFGDATA));
+	/* START | WRITE | MUXFPGA | SITE_MB */
+	raw_writel((1 << 31) | (1 << 30) | (7 << 20) | (0 << 16),
+				V2M_SYS(CFGCTRL));
+}