[Qemu-devel] [PATCH RFC 1/2] atomic-test: Implement ARM and AARCH64 basic bare-metal infrastructure

[Alexander Spyridakis]

Minimal payload initialization for the virt machine model.
Setup a stack and jump to C code, where CPU0 will boot any
discovered secondary cores through PSCI. Immediately after
all cores are turned-off and the guest is powered down.

Added printf functionality is based on the multiboot test. In a
similar manner this test is standalone and not part of 'make check'.

Suggested-by: Jani Kokkonen <address@hidden>
Suggested-by: Claudio Fontana <address@hidden>
Signed-off-by: Alexander Spyridakis <address@hidden>
---
 tests/atomic-test/Makefile  |  63 ++++++++++++++++++
 tests/atomic-test/helpers.c |  84 ++++++++++++++++++++++++
 tests/atomic-test/helpers.h |  36 +++++++++++
 tests/atomic-test/link.ld.S |  19 ++++++
 tests/atomic-test/main.c    |  32 ++++++++++
 tests/atomic-test/printf.c  | 152 ++++++++++++++++++++++++++++++++++++++++++++
 tests/atomic-test/start.S   |  54 ++++++++++++++++
 7 files changed, 440 insertions(+)
 create mode 100644 tests/atomic-test/Makefile
 create mode 100644 tests/atomic-test/helpers.c
 create mode 100644 tests/atomic-test/helpers.h
 create mode 100644 tests/atomic-test/link.ld.S
 create mode 100644 tests/atomic-test/main.c
 create mode 100644 tests/atomic-test/printf.c
 create mode 100644 tests/atomic-test/start.S

diff --git a/tests/atomic-test/Makefile b/tests/atomic-test/Makefile
new file mode 100644
index 0000000..094e01a
--- /dev/null
+++ b/tests/atomic-test/Makefile
@@ -0,0 +1,63 @@
+#
+# Global variables, sources and tools
+#
+CC = $(CROSS_COMPILE)gcc
+LD = $(CROSS_COMPILE)ld
+
+S_OBJS    = start.o
+C_OBJS    = main.o printf.o helpers.o
+H_DEPS    = helpers.h
+LD_SCRIPT = link.ld.S
+
+LIBS      = $(shell $(CC) $(CCFLAGS) -print-libgcc-file-name)
+CPPFLAGS  += -gdwarf-2 -fno-stack-protector -nostdinc -fno-builtin
+
+#
+# Target specific variables
+#
+clean: export DIRS = build-virt build-virt64
+
+virt:   export CPPFLAGS += -march=armv7-a
+virt64: export CPPFLAGS += -march=armv8-a -mgeneral-regs-only -mstrict-align
+
+virt:   export CROSS_COMPILE ?= arm-none-eabi-
+virt64: export CROSS_COMPILE ?= aarch64-linux-gnu-
+
+virt:   export ARCH = ARCH_ARM
+virt64: export ARCH = ARCH_AARCH64
+
+virt virt64: export UART_PHYS = 0x09000000
+virt virt64: export ENTRY_POINT = 0x40000000
+
+virt virt64: export O_DIR = build-$@/
+virt virt64: export IMAGE = $(O_DIR)address@hidden
+
+#
+# Target build rules
+#
+all: virt virt64
+
+clean:
+       rm -rf $(DIRS)
+
+virt virt64:
+       mkdir -p $(O_DIR)
+       @$(MAKE) $(IMAGE) --no-print-directory
+
+$(IMAGE): $(addprefix $(O_DIR), $(S_OBJS)) \
+          $(addprefix $(O_DIR), $(C_OBJS)) $(H_DEPS) $(O_DIR)link.ld Makefile
+       $(LD) -o $@ $(addprefix $(O_DIR), $(S_OBJS)) \
+          $(addprefix $(O_DIR), $(C_OBJS)) $(LIBS) \
+          --script=$(O_DIR)link.ld -Map $(O_DIR)system.map
+
+$(O_DIR)link.ld: $(LD_SCRIPT)
+       $(CC) -DENTRY_POINT=$(ENTRY_POINT) -D$(ARCH) $(CPPFLAGS) -E -P -C -o $@ 
$<
+
+$(O_DIR)%.o: %.c $(H_DEPS)
+       $(CC) -DENTRY_POINT=$(ENTRY_POINT) \
+          -DUART_PHYS=$(UART_PHYS) -D$(ARCH) $(CPPFLAGS) -c -o $@ $<
+
+$(O_DIR)%.o: %.S $(H_DEPS)
+       $(CC) -D$(ARCH) $(CPPFLAGS) -c -o $@ $<
+
+.PHONY: all clean virt virt64
diff --git a/tests/atomic-test/helpers.c b/tests/atomic-test/helpers.c
new file mode 100644
index 0000000..8ac8c2c
--- /dev/null
+++ b/tests/atomic-test/helpers.c
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2015 Virtual Open Systems SAS
+ * Author: Alexander Spyridakis <address@hidden>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "helpers.h"
+
+#ifdef ARCH_ARM
+__asm__(".arch_extension virt");
+
+int get_cpuid(void)
+{
+    int cpu;
+    asm volatile ("mrc p15, 0, %0, c0, c0, 5; and %0, %0, #15\n" : "=r"(cpu));
+    return cpu;
+}
+
+int psci_call(int psci_function, int arg0, int arg1, int arg2)
+{
+    int ret;
+    asm volatile ("hvc #0; mov %0, r0\n" : "=r" (ret));
+    return ret;
+}
+#elif ARCH_AARCH64
+int get_cpuid(void)
+{
+    int cpu;
+    asm volatile ("mrs %0, MPIDR_EL1; and %0, %0, #15\n" : "=r"(cpu));
+    return cpu;
+}
+
+int psci_call(int psci_function, int arg0, int arg1, int arg2)
+{
+    int ret;
+    asm volatile ("hvc #0; mov %0, x0\n" : "=r" (ret));
+    return ret;
+}
+#endif
+
+void power_secondary(void)
+{
+    int ret, cpu = 1;
+
+    /* Sequentially power-up all secondary cores,
+     * error means trying to wake up non existing cores */
+    do { ret = psci_call(PSCI_CPU_ON, cpu++, ENTRY_POINT, 0); } while (!ret);
+}
+
+void power_off(void)
+{
+    int ret, i = 1;
+    int cpu = get_cpuid();
+
+    /* Only secondary cores should power off themselves */
+    if(cpu) {
+        psci_call(PSCI_CPU_OFF, 0, 0, 0);
+        return;
+    }
+
+    /* Primary core should wait for all secondaries to be powered off */
+    do {
+        ret = psci_call(PSCI_AFFINITY_INFO, i, 0, 0);
+
+        /* If a core was powered off, wait for the next one */
+        if (ret == 1) {
+            i++;
+        }
+    } while(ret >= 0);
+
+    /* Shut down system */
+    psci_call(PSCI_SYSTEM_OFF, 0, 0, 0);
+}
diff --git a/tests/atomic-test/helpers.h b/tests/atomic-test/helpers.h
new file mode 100644
index 0000000..66d440e
--- /dev/null
+++ b/tests/atomic-test/helpers.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2015 Virtual Open Systems SAS
+ * Author: Alexander Spyridakis <address@hidden>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HELPERS_H
+#define HELPERS_H
+
+#ifdef ARCH_ARM
+#define PSCI_CPU_ON        0x84000003
+#define PSCI_AFFINITY_INFO 0x84000004
+#elif ARCH_AARCH64
+#define PSCI_CPU_ON        0xC4000003
+#define PSCI_AFFINITY_INFO 0xC4000004
+#endif
+
+#define PSCI_CPU_OFF       0x84000002
+#define PSCI_SYSTEM_OFF    0x84000008
+
+int get_cpuid(void);
+void power_secondary(void);
+void power_off();
+
+#endif
diff --git a/tests/atomic-test/link.ld.S b/tests/atomic-test/link.ld.S
new file mode 100644
index 0000000..e1ab018
--- /dev/null
+++ b/tests/atomic-test/link.ld.S
@@ -0,0 +1,19 @@
+#ifdef ARCH_ARM
+OUTPUT_FORMAT("elf32-littlearm")
+OUTPUT_ARCH(arm)
+#elif ARCH_AARCH64
+OUTPUT_FORMAT("elf64-littleaarch64")
+OUTPUT_ARCH(aarch64)
+#endif
+
+SECTIONS
+{
+    . = ENTRY_POINT;
+
+    .text   ALIGN(4096) : { *(.text)   }
+    .data   ALIGN(4096) : { *(.data)   }
+    .bss    ALIGN(4096) : { *(.bss)    }
+    .rodata ALIGN(4096) : { *(.rodata) }
+
+    text_end = ALIGN(8);
+}
diff --git a/tests/atomic-test/main.c b/tests/atomic-test/main.c
new file mode 100644
index 0000000..72eaf59
--- /dev/null
+++ b/tests/atomic-test/main.c
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2015 Virtual Open Systems SAS
+ * Author: Alexander Spyridakis <address@hidden>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+void main(void)
+{
+    printf("CPU %d on\n", get_cpuid());
+    power_off();
+}
+
+void init(void)
+{
+    /* Only CPU 0 should be here */
+    if (get_cpuid()) {
+        return;
+    }
+
+    power_secondary();
+}
\ No newline at end of file
diff --git a/tests/atomic-test/printf.c b/tests/atomic-test/printf.c
new file mode 100644
index 0000000..7c40d37
--- /dev/null
+++ b/tests/atomic-test/printf.c
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2015 Virtual Open Systems SAS
+ * Author: Alexander Spyridakis <address@hidden>
+ *
+ * printf based on implementation by Kevin Wolf <address@hidden>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define UARTDR      UART_PHYS
+#define UARTFR      0x18
+#define UARTFR_TXFF (1 << 5)
+
+typedef __builtin_va_list   va_list;
+#define va_start(ap, X)     __builtin_va_start(ap, X)
+#define va_arg(ap, type)    __builtin_va_arg(ap, type)
+#define va_end(ap)          __builtin_va_end(ap)
+
+int *uart_phys = (int *)(UART_PHYS);
+int *uart_busy = (int *)(UART_PHYS + UARTFR);
+
+static void putc(char c)
+{
+    /* If the FIFO is full, wait before pushing data to the UART */
+    while (*uart_busy & UARTFR_TXFF);
+
+    *uart_phys = c;
+
+    /* Add the carriage return in case of a line feed character */
+    if (c == '\n') {
+        putc('\r');
+    }
+}
+
+static void print_str(char *s)
+{
+    while (*s) {
+        putc(*s++);
+    }
+}
+
+static void print_num(unsigned long long value, int base)
+{
+    char digits[] = "0123456789abcdef";
+    char buf[32] = { 0 };
+    int i = sizeof(buf) - 2;
+
+    do {
+        buf[i--] = digits[value % base];
+        value /= base;
+    } while (value);
+
+    print_str(&buf[i + 1]);
+}
+
+void printf(const char *fmt, ...)
+{
+    va_list ap;
+    char *str;
+    int base;
+    int has_long;
+    int alt_form;
+    unsigned long long val;
+
+    va_start(ap, fmt);
+
+    for (; *fmt; fmt++) {
+        if (*fmt != '%') {
+            putc(*fmt);
+            continue;
+        }
+        fmt++;
+
+        if (*fmt == '#') {
+            fmt++;
+            alt_form = 1;
+        } else {
+            alt_form = 0;
+        }
+
+        if (*fmt == 'l') {
+            fmt++;
+            if (*fmt == 'l') {
+                fmt++;
+                has_long = 2;
+            } else {
+                has_long = 1;
+            }
+        } else {
+            has_long = 0;
+        }
+
+        switch (*fmt) {
+        case 'x':
+        case 'p':
+            base = 16;
+            goto convert_number;
+        case 'd':
+        case 'i':
+        case 'u':
+            base = 10;
+            goto convert_number;
+        case 'o':
+            base = 8;
+            goto convert_number;
+
+        convert_number:
+            switch (has_long) {
+            case 0:
+                val = va_arg(ap, unsigned int);
+                break;
+            case 1:
+                val = va_arg(ap, unsigned long);
+                break;
+            case 2:
+                val = va_arg(ap, unsigned long long);
+                break;
+            }
+
+            if (alt_form && base == 16) {
+                print_str("0x");
+            }
+
+            print_num(val, base);
+            break;
+
+        case 's':
+            str = va_arg(ap, char*);
+            print_str(str);
+            break;
+        case '%':
+            putc(*fmt);
+            break;
+        default:
+            putc('%');
+            putc(*fmt);
+            break;
+        }
+    }
+
+    va_end(ap);
+}
diff --git a/tests/atomic-test/start.S b/tests/atomic-test/start.S
new file mode 100644
index 0000000..29e483f
--- /dev/null
+++ b/tests/atomic-test/start.S
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2015 Virtual Open Systems SAS
+ * Author: Alexander Spyridakis <address@hidden>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * Before jumping to C code we need to setup a stack.
+ * This macro gets the CPU ID from MPIDR and offsets the stack 
+ * for each CPU by 1 << 18 (256 KiB), starting after the text section.
+ */
+#ifdef ARCH_ARM
+.macro setup_stack
+    mrc p15, 0, r0, c0, c0, 5
+    and r0, r0, #15
+    add r0, r0, #1
+
+    lsl r0, r0, #20
+    ldr r1, =text_end
+    add r0, r0, r1
+    mov sp, r0
+.endm
+#elif ARCH_AARCH64
+.macro setup_stack
+    mrs x0, MPIDR_EL1
+    and x0, x0, #15
+    add x0, x0, #1
+
+    lsl x0, x0, #18
+    ldr x1, =text_end
+    add x0, x0, x1
+    mov sp, x0
+.endm
+#endif
+
+/* Entry point */
+.section .text
+.global _start
+_start:
+    setup_stack
+    bl init
+    bl main
+    b .
-- 
2.1.4