26 lines
514 B
C
26 lines
514 B
C
|
|
#include "uart.h"
|
|
|
|
volatile int core_ready[4];
|
|
|
|
int get_core_id(void) {
|
|
uint64_t mpidr;
|
|
asm volatile("mrs %0, mpidr_el1" : "=r"(mpidr));
|
|
return mpidr & 0b11;
|
|
}
|
|
|
|
void core_entry(void) {
|
|
int cid = get_core_id();
|
|
uart_puts("Core ");
|
|
uart_putc('0' + cid);
|
|
uart_puts(" ready\n");
|
|
core_ready[cid] = 1;
|
|
while (1);
|
|
}
|
|
|
|
void smp_init(void) {
|
|
for (int i = 1; i < 4; i++) {
|
|
*(volatile uint64_t *)(0x4000008 + i * 8) = (uint64_t)core_entry;
|
|
asm volatile("sev");
|
|
}
|
|
}
|