Multicore for raspi4

All cores awake and answering. Closes #1

Author: mcribbs

src/bootstrapper/boot.S

@@ -1,46 +1,50 @@
 /*
  * Bootstrapper. For details see: https://jsandler18.github.io/explanations/boot_S.html
+ * For multi core on raspi4 see: https://github.com/szediwy/raspberry-pi-os/commit/f601eb8b88a97aaaa92be90f7c01272541367fac
  */
 #include "mm.h"
-#include "smp.h"
 
 .section	".text.boot"
 
 .global _start
 _start:
 	mrs		x0, mpidr_el1
 	and		x0, x0, #0xFF				// Check core id
-	mov		x19, x0						// Save core id
-	cbz		x0, start_primary_core
-	b		start_secondary_cores
-
-start_secondary_cores:
-	mrs 	x19, mpidr_el1
-	and		x19, x19, #0xFF				// Check core id
-	mov		x0, STACK_SIZE
-	mul		x2, x0, x19					// Calculate SP for each core
-	mov		sp, x2						// Setup SP
-	mov		x1, #30000
-	mul		x0, x1, x19					// Calc delay for each core
-	bl		delay
-	mov		x0, x19
-	bl		kernel_secondary			// To the kernel secondary core entry
+	cbz		x0, init_bss
+	wfe									// If not core 0 wait for sev instruction
+	b		master
 
 proc_hang:
 	b 		proc_hang
 
-start_primary_core:
+init_bss:
 	adr		x0, __bss_start 			// Init bss
 	adr		x1, __bss_end
 	sub		x1, x1, x0
 	bl		memzero
 
 	// Setup and wake cores
-	mov		sp, #LOW_MEMORY
-	bl		kernel_main					// To the kernel primary core entry
-	mov 	x0, #2						// Tell core 2 ...
-	adr		x1, start_secondary_cores	// ... where to start
-	bl		wakeup_core
-	mov		x0, #3
-	bl 		wakeup_core
-	b		proc_hang					// should never come here
+	sev
+
+	// 	Enable the other cores see: https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/Documentation/arm64/booting.rst?h=v5.3#n255
+	mov 	x0, #0
+	adr		x0, master
+
+	mov 	x1, #0xE0
+	str		x0, [x1]
+	mov		x1,	#0xE8
+	str		x0, [x1]
+	mov 	x1, #0xF0
+	str 	x0, [x1]
+
+master:
+	mrs	x0, mpidr_el1
+	and	x0, x0, #0xFF					// Get core id
+
+	mov	x1, #SECTION_SIZE
+	mul	x1, x1, x0						// Calculate SP for core
+	add	x1, x1, #LOW_MEMORY
+	mov	sp, x1							// Setup SP
+
+	bl	kernel_main
+	b 	proc_hang

src/bootstrapper/mm.h

@@ -1,7 +1,7 @@
 #ifndef	_MM_H
 #define	_MM_H
 
-#define PAGE_SHIFT	 		12
+#define PAGE_SHIFT	 			12
 #define TABLE_SHIFT 			9
 #define SECTION_SHIFT			(PAGE_SHIFT + TABLE_SHIFT)
 
@@ -10,8 +10,6 @@
 
 #define LOW_MEMORY              (2 * SECTION_SIZE)
 
-#define STACK_SIZE              (1 << 20)
-
 #ifndef __ASSEMBLER__
 
 void memzero(unsigned long src, unsigned long n);

src/bootstrapper/smp.S

@@ -1,38 +1,30 @@
-#include <stdint.h>
-#include "../common/stdio.h"
-#include "../common/utils.h"
 #include "../peripherals/uart.h"
 
-volatile unsigned int semaphore = 0;
+static unsigned int current_processor_index = 0;
 
-void kernel_do(int id)
+void kernel_main(unsigned long processor_index)
 {
-	// Wait for previous CPU to finish printing
-	while(id != semaphore) { }
 
-	puts("Hello, from processor ");
-	putc(id + '0');
-	puts("\r\n");
+	if (processor_index == 0) {
+		uart_init();
+	}
+
+	while (processor_index != current_processor_index) { }
+
+	uart_send_string("Hello from processor ");
+	uart_send(processor_index + '0');
+	uart_send_string("!\r\n");
 
-	// Tells the next CPU to go
-	semaphore++;
+	// Tell the next core to go
+	current_processor_index++;
 
-    // Only CPU #0 do the echo
-	if (id == 0) {
-		// Wait for everyone else to finish
-		while(semaphore != 4) { }
+	// Only core 0 do the echo
+	if (processor_index == 0) {
+		// if current_processor_index == 4 then all processors are done
+		while (current_processor_index != 4) { }
 
 		while (1) {
-			putc(getc());
+			uart_send(uart_recv());
 		}
 	}
-}
-
-void kernel_main(void) {
-	uart_init();
-	kernel_do(0);
-}
-
-void kernel_secondary(int core_id) {
-	kernel_do(core_id);
 }