demo: initial commit

8 files changed, 389 insertions, 0 deletions

diff --git a/demo/console.c b/demo/console.c
new file mode 100644
index 0000000..e531b2e
--- /dev/null
+++ b/demo/console.c
@@ -0,0 +1,145 @@
+#include <stdarg.h>
+
+#include "demo.h"
+
+struct lock console_lock;
+
+#define JTAG_UART_BASE           0x30000000
+#define JTAG_UART_DATA           (*(volatile unsigned *)(JTAG_UART_BASE + 0))
+#define JTAG_UART_CONTROL        (*(volatile unsigned *)(JTAG_UART_BASE + 4))
+#define JTAG_UART_CONTROL_RE     (1 << 0)
+#define JTAG_UART_CONTROL_AC     (1 << 10)
+#define JTAG_UART_CONTROL_WSPACE 0xffff0000
+
+#define INTC_BASE       0x30070000
+#define INTC_MASK       (*(volatile unsigned *)(INTC_BASE + 4))
+#define INTC_MASK_TIMER (1 << 0)
+#define INTC_MASK_UART  (1 << 1)
+
+static void print_char(char c)
+{
+	while (!(JTAG_UART_CONTROL & JTAG_UART_CONTROL_WSPACE));
+	JTAG_UART_DATA = (unsigned char)c;
+}
+
+static void print_str(const char *s)
+{
+	while (*s)
+		print_char(*s++);
+}
+
+static void print_hex(unsigned val, unsigned bits)
+{
+	static const char hex_digits[16] = "0123456789abcdef";
+
+	print_str("0x");
+
+	if (bits < 4)
+		bits = 4;
+
+	if (bits > 32)
+		bits = 32;
+
+	bits = bits & ~3;
+	do
+		print_char(hex_digits[(val >> (bits -= 4)) & 0xf]);
+	while (bits);
+}
+
+static void print_dec(unsigned val)
+{
+	char dec[11] = {'\0'};
+	char *next = dec;
+
+	do {
+		*next++ = '0' + val % 10;
+		val /= 10;
+	} while (val && next < dec + sizeof dec); 
+
+	const char *c = next;
+	while (c > dec)
+		print_char(*--c);
+}
+
+void console_init(void)
+{
+	spin_init(&console_lock);
+
+	// Habilitar irqs de entrada uart
+	JTAG_UART_CONTROL = JTAG_UART_CONTROL_RE;
+
+	// Habilitar irqs en controlador de interrupciones
+	INTC_MASK = INTC_MASK_UART;
+
+	// Habilitar irqs en core
+	asm volatile("msr cpsr_c, #0x53");
+}
+
+void print(const char *fmt, ...)
+{
+	va_list args;
+	va_start(args, fmt);
+
+	unsigned irq_save;
+	spin_lock(&console_lock, &irq_save);
+
+	while (!(JTAG_UART_CONTROL & JTAG_UART_CONTROL_AC));
+
+	print_str("cpu");
+	print_dec(this_cpu->num);
+	print_str(": ");
+
+	char c;
+	while ((c = *fmt++)) {
+		if (c != '%') {
+			print_char(c);
+			continue;
+		}
+
+		unsigned val;
+		switch ((c = *fmt++)) {
+			case 'c':
+				print_char((char)va_arg(args, int));
+				break;
+
+			case 'd':
+			case 'i':
+				print_dec((unsigned)va_arg(args, int));
+				break;
+
+			case 'p':
+				print_hex((unsigned)va_arg(args, const void *), 32);
+				break;
+
+			case 'r':
+				val = va_arg(args, unsigned);
+				print_hex(val, va_arg(args, unsigned));
+				break;
+
+			case 'u':
+				print_dec(va_arg(args, unsigned));
+				break;
+
+			case 's':
+				print_str(va_arg(args, const char *));
+				break;
+
+			case 'x':
+				print_hex(va_arg(args, unsigned), 32);
+				break;
+
+			default:
+				print_char('%');
+				if (c)
+					print_char(c);
+
+				break;
+		}
+	}
+
+	print_str("\r\n");
+	JTAG_UART_CONTROL = JTAG_UART_CONTROL_RE | JTAG_UART_CONTROL_AC;
+
+	spin_unlock(&console_lock, irq_save);
+	va_end(args);
+}
diff --git a/demo/demo.h b/demo/demo.h
new file mode 100644
index 0000000..1bff8d6
--- /dev/null
+++ b/demo/demo.h
@@ -0,0 +1,34 @@
+#ifndef DEMO_H
+#define DEMO_H
+
+#define NUM_CPUS 4
+
+struct lock
+{
+	volatile unsigned val;
+};
+
+struct cpu
+{
+	unsigned num;
+};
+
+/* R12 está reservado por la ABI
+ * https://gcc.gnu.org/onlinedocs/gcc/Global-Register-Variables.html
+ */
+register struct cpu *this_cpu asm("ip");
+
+void spin_init(struct lock *lock);
+void spin_lock(struct lock *lock, unsigned *irq_save);
+void spin_unlock(struct lock *lock, unsigned irq_save);
+
+void console_init(void);
+void print(const char *fmt, ...);
+
+int cpus_ready(void);
+void run_cpu(unsigned num);
+void run_cpus(unsigned mask);
+void halt_cpu(unsigned num);
+void halt_cpus(unsigned mask);
+
+#endif
diff --git a/demo/link.ld b/demo/link.ld
new file mode 100644
index 0000000..5f400c7
--- /dev/null
+++ b/demo/link.ld
@@ -0,0 +1,33 @@
+MEMORY
+{
+  HPS_SDRAM (rwx) : ORIGIN = 0x00000000, LENGTH = 512M
+}
+
+SECTIONS
+{
+  ._img :
+  {
+    KEEP(*(.interrupt_vector))
+    *(.text)
+    *(.text*)
+    *(.rodata)
+    *(.rodata*)
+    *(.data)
+    *(.data*)
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+    . = ALIGN(4);
+  } > HPS_SDRAM
+
+  _stack_shift = 13;
+  _stack_size = (1 << _stack_shift) * 4;
+  _stack_end = ORIGIN(HPS_SDRAM) + LENGTH(HPS_SDRAM);
+  _stack_begin = (_stack_end - _stack_size) + (1 << _stack_shift);
+  
+  . = _stack_begin;
+  ._stack :
+  {
+    . = . + _stack_size;
+  } > HPS_SDRAM
+}
diff --git a/demo/lock.S b/demo/lock.S
new file mode 100644
index 0000000..3ec2765
--- /dev/null
+++ b/demo/lock.S
@@ -0,0 +1,26 @@
+.global spin_init
+spin_init:
+	mov     r1, #0
+	str     r1, [r0]
+	mov     pc, lr
+
+.global spin_lock
+spin_lock:
+	mrs     r2, cpsr
+	str     r2, [r1]
+	orr     r2, r2, #0xc0 @ Levanta I, F
+	msr     cpsr_c, r2
+	mov     r3, #1
+1:	ldrex   r2, [r0]
+	teq     r2, #0
+	strexeq r2, r3, [r0]
+	teqeq   r2, #0
+	bne     1b
+	mov     pc, lr
+
+.global spin_unlock
+spin_unlock:
+	mov     r2, #0
+	str     r2, [r0]
+	msr     cpsr_c, r1
+	mov     pc, lr
diff --git a/demo/main.c b/demo/main.c
new file mode 100644
index 0000000..b984d6f
--- /dev/null
+++ b/demo/main.c
@@ -0,0 +1,44 @@
+#include "demo.h"
+
+static struct cpu cpu0, cpu1, cpu2, cpu3;
+
+struct cpu *__cpus[] = { &cpu0, &cpu1, &cpu2, &cpu3 };
+
+void bsp_main(void)
+{
+	run_cpu(1);
+
+	while (!cpus_ready());
+	print("booted %u cpus", NUM_CPUS);
+
+	while (1) {
+	}
+}
+
+void ap_main(void)
+{
+	if (this_cpu->num < NUM_CPUS - 1)
+		run_cpu(this_cpu->num + 1);
+
+	halt_cpu(this_cpu->num);
+
+	while (1) {
+	}
+}
+
+void reset(void)
+{
+	if (this_cpu->num == 0) {
+		console_init();
+
+		for (struct cpu **cpu = __cpus; cpu < __cpus + NUM_CPUS; ++cpu) {
+		}
+	}
+
+	print("core taken out of reset");
+
+	if (this_cpu->num == 0)
+		bsp_main();
+	else
+		ap_main();
+}
diff --git a/demo/smp.c b/demo/smp.c
new file mode 100644
index 0000000..f0f83e7
--- /dev/null
+++ b/demo/smp.c
@@ -0,0 +1,43 @@
+#include "demo.h"
+
+#define SMP_CTRL_BASE 0x30140000
+#define SMP_CTRL      (*(volatile unsigned *)SMP_CTRL_BASE)
+
+int cpus_ready(void)
+{
+	return SMP_CTRL == 0;
+}
+
+void run_cpu(unsigned num)
+{
+	run_cpus(1 << num);
+}
+
+void run_cpus(unsigned mask)
+{
+	unsigned ctrl_word = 0;
+	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu)
+		if (mask & (1 << cpu)) {
+			print("run cpu%u", cpu);
+			ctrl_word |= 0b001 << (cpu * 8);
+		}
+
+	SMP_CTRL = ctrl_word;
+}
+
+void halt_cpu(unsigned num)
+{
+	halt_cpu(1 << num);
+}
+
+void halt_cpus(unsigned mask)
+{
+	unsigned ctrl_word = 0;
+	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu)
+		if (mask & (1 << cpu)) {
+			print("halt cpu%u%s", cpu, cpu == this_cpu->num ? " (myself)" : "");
+			ctrl_word |= 0b010 << (cpu * 8);
+		}
+
+	SMP_CTRL = ctrl_word;
+}
diff --git a/demo/start.S b/demo/start.S
new file mode 100644
index 0000000..6e5e457
--- /dev/null
+++ b/demo/start.S
@@ -0,0 +1,57 @@
+.section .interrupt_vector
+
+__reset:
+	b _start
+
+__undefined:
+	b undefined
+
+__swi:
+	b swi
+
+__prefetch_abort:
+	b prefetch_abort
+
+__data_abort:
+	b data_abort
+
+__reserved:
+	b .
+
+__irq:
+	b irq
+
+__fiq:
+	b .
+
+.text
+
+.global _start
+_start:
+	ldr r0, =_boot_num
+	ldr r1, [r0]
+	add r2, r1, #1
+	str r2, [r0]
+	ldr r2, =__cpus
+	lsl r3, r1, #2
+	ldr ip, [r2, r3]
+	str r1, [ip]
+	ldr r2, =_stack_shift
+	lsl r1, r1, r2
+	ldr sp, =_stack_begin
+	add sp, sp, r1
+	mov lr, #0
+	bl  reset
+	b   .
+
+_boot_num:
+	.word 0
+
+irq:
+	subs pc, lr, #4
+
+.weak undefined, swi, data_abort, prefetch_abort, irq
+.set swi, __swi
+.set undefined, __undefined
+.set data_abort, __data_abort
+.set prefetch_abort, __prefetch_abort
diff --git a/demo/util.c b/demo/util.c
new file mode 100644
index 0000000..457a32c
--- /dev/null
+++ b/demo/util.c
@@ -0,0 +1,7 @@
+int strcmp(const char *s1, const char *s2)
+{
+    while (*s1 && *s1 == *s2)
+        s1++, s2++;
+
+    return (int)*(const unsigned char *)s1 - (int)*(const unsigned char *)s2;
+}