void cpu_up(int id)
{
extern char _bootother_start[];
extern uint64_t _bootother_size;
extern void (*apstart)(void);
char *stack;
unsigned char *code = (unsigned char*)VADDR_DIRECT(0x7000);
struct cpu *c = per_cpu_ptr(cpus, id);
assert(c->id != myid());
assert(c->id == id);
memcpy(code, _bootother_start, _bootother_size);
stack = (char*)page2kva(alloc_pages_cpu(c, KSTACKPAGE));
assert(stack != NULL);
kprintf("LAPIC %d, CODE %p PA: %p, STACK: %p\n", c->hwid, code, PADDR_DIRECT(code), stack);
warmreset(PADDR_DIRECT(code));
*(uint32_t*)(code-4) = (uint32_t)PADDR_DIRECT(&apstart);
*(uint64_t*)(code-12) = (uint64_t)stack + KSTACKSIZE;
*(uint64_t*)(code-20) = (uint64_t)boot_cr3;
// bootother.S sets this to 0x0a55face early on
*(uint32_t*)(code-64) = 0;
bcpuid = c->id;
atomic_set(&bsync, 0);
lapic_start_ap(c, PADDR_DIRECT(code));
while(atomic_read(&bsync) == 0)
nop_pause();
rstrreset();
}
void
kmm_pgfault(struct trapframe *tf)
{
// uint64_t err = tf->tf_err;
uintptr_t addr = rcr2();
if (addr >= PBASE && addr < PBASE + PSIZE)
{
pgd_t *pgd = KADDR_DIRECT(PTE_ADDR(rcr3()));
pud_t *pud;
pmd_t *pmd;
pte_t *ptd;
/* PHYSICAL ADDRRESS ACCESSING */
if (last_pgd != NULL)
{
pud = KADDR_DIRECT(PGD_ADDR(last_pgd[PGX(last_addr)]));
pmd = KADDR_DIRECT(PUD_ADDR(pud[PUX(last_addr)]));
ptd = KADDR_DIRECT(PMD_ADDR(pmd[PMX(last_addr)]));
ptd[PTX(last_addr)] = 0;
if (ptd == temp_ptd)
{
pmd[PUX(last_addr)] = 0;
if (pmd == temp_pmd)
{
pud[PUX(last_addr)] = 0;
if (pud == temp_pud)
last_pgd[PGX(last_addr)] = 0;
}
if (last_pgd == pgd)
{
invlpg((void *)last_addr);
}
}
}
if (pgd[PGX(last_addr)] == 0)
pgd[PGX(last_addr)] = PADDR_DIRECT(temp_pud) | PTE_W | PTE_P;
pud = KADDR_DIRECT(PGD_ADDR(pgd[PGX(last_addr)]));
if (pud[PUX(last_addr)] == 0)
pud[PUX(last_addr)] = PADDR_DIRECT(temp_pmd) | PTE_W | PTE_P;
pmd = KADDR_DIRECT(PUD_ADDR(pud[PUX(last_addr)]));
if (pmd[PMX(last_addr)] == 0)
pmd[PMX(last_addr)] = PADDR_DIRECT(temp_ptd) | PTE_W | PTE_P;
ptd = KADDR_DIRECT(PMD_ADDR(pmd[PMX(last_addr)]));
ptd[PTX(last_addr)] = PADDR_DIRECT(addr) | PTE_W | PTE_P;
last_pgd = pgd;
last_addr = addr;
/* XXX? */
// invlpg((void *)addr);
}
}
void
kfree(void *ptr)
{
if (ptr == NULL) return;
local_irq_save();
if ((uintptr_t)ptr & (PGSIZE - 1))
{
uintptr_t *head = ((uintptr_t *)ptr - 1);
kmm_ctrl_s *ctrl = (kmm_ctrl_s *)*head;
*head = ctrl->head;
ctrl->head = (uintptr_t)head;
}
else
{
kfree_pages((uintptr_t)PADDR_DIRECT(ptr) - PGSIZE, *((uintptr_t *)ptr - 1));
}
local_irq_restore();
}
