From b5021c723a5ee267982c4779bb1b1d644e4d2d5c Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Tue, 22 Feb 2022 23:27:39 -0700
Subject: Started multithreading

---
 include/macros.inc | 26 ++++++++++++++++++++++++++
 src/boot.S         | 19 +++++++++++++------
 src/sys/kernel.S   | 32 ++++++++++++++++++++++++++++++++
 3 files changed, 71 insertions(+), 6 deletions(-)

diff --git a/include/macros.inc b/include/macros.inc
index 18573af..ea44278 100644
--- a/include/macros.inc
+++ b/include/macros.inc
@@ -59,3 +59,29 @@ fiq_stack_core\coreid:
 	.space 4096
 sys_stack_core\coreid:
 .endm
+
+.macro core_task coreid
+__l_\@:	clrex
+	ldr r0, =mbox_core\coreid
+	ldrex r1, [r0]
+	cmp r1, #0
+	wfeeq
+	beq __l_\@
+	clrex
+	mov r0, #\coreid
+	blx r1
+	ldr r0, =mbox_core\coreid
+	mov r1, #0
+	str r1, [r0]
+	b __l_\@
+.endm
+
+.macro assign_ctask label, coreid
+	ldr r0, =mbox_core\coreid
+	ldr r1, =\label
+__l_\@:	ldr r2, [r0]
+	cmp r2, #0
+	bne __l_\@
+	str r1, [r0]
+	sev
+.endm
diff --git a/src/boot.S b/src/boot.S
index 1b4b249..9d431d7 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -13,11 +13,11 @@ reset:
 	mrc p15, #0, r1, c0, c0, #5
 	and r1, r1, #3
 	cmp r1, #1
-	beq core1run
+	beq runcore1
 	cmp r1, #2
-	beq core2run
+	beq runcore2
 	cmp r1, #3
-	bge core3run
+	bge runcore3
 
 	init_core 0
 
@@ -52,14 +52,17 @@ reset:
 	ldr r3, =kernel_main
 	blx r3
 
-core1run:
+runcore1:
 	init_core 1
+	core_task 1
 	b io_halt
-core2run:
+runcore2:
 	init_core 2
+	core_task 2
 	b io_halt
-core3run:
+runcore3:
 	init_core 3
+	core_task 3
 	b io_halt
 
 .globl io_halt
@@ -88,9 +91,13 @@ irq_handler:        .word irq
 fiq_handler:        .word fiq
 
 .section .data
+.globl mbox_core0
 mbox_core0: .word 0
+.globl mbox_core1
 mbox_core1: .word 0
+.globl mbox_core2
 mbox_core2: .word 0
+.globl mbox_core3
 mbox_core3: .word 0
 
 .section .bss.estacks
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index 3ffc6a5..6452a4f 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -1,11 +1,19 @@
 .section ".text.kernel"
 
+.include "macros.inc"
+
 .globl kernel_main
 kernel_main:
 	///https://wiki.osdev.org/ARM_Paging
 	// Query the ID_MMFR0 register
 	mrc p15, 0, r0, c0, c1, 4
 	bl sysinit
+
+	assign_ctask testf, 2
+	assign_ctask testf, 1
+	assign_ctask testf, 2
+	assign_ctask testf, 3
+	assign_ctask testf, 1
 	// Intentional undefined instruction
 	// .word 0xf7f0a000
 	cpsie aif, #0x10
@@ -13,3 +21,27 @@ kernel_main:
 1:
 	wfe
 	b 1b
+
+testf:
+	push {lr}
+	cmp r0, #2
+	blo 1f
+	beq 2f
+	bgt 3f
+1:
+	ldr r0, =t1
+	bl uart_print
+	pop {pc}
+2:
+	ldr r0, =t2
+	bl uart_print
+	pop {pc}
+3:
+	ldr r0, =t3
+	bl uart_print
+	pop {pc}
+
+.section .data
+t1: .asciz "C1\n"
+t2: .asciz "C2\n"
+t3: .asciz "C3\n"
-- 
cgit v1.2.1