#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");
    }
}