INITCODE void arch_smp_init(void)
{
cpu_id_t c;
uint32_t *lapic_id;
char *ap_code = (char *)0x9000;
size_t size = &ap_boot_end - &ap_boot_start;
struct ap_config *apcfg;
/* FIXME DK: real-mode page must be reserved dynamically during mm initialization. */
apcfg = (struct ap_config *)&ap_config;
apcfg->page_addr = 0x9000;
apcfg->jmp_rip = (uint32_t)((uint64_t)smp_start32 & 0xffffffffU);
apcfg->gdtr.base += apcfg->page_addr;
memcpy(&apcfg->gdt[APGDT_KCOFF_DESCR], &apcfg->gdt[APGDT_KCNORM_DESCR],
sizeof(uint64_t));
apcfg->gdt[APGDT_KCOFF_DESCR] |= (uint64_t)apcfg->page_addr << 16;
/* Change bootstrap entry point (from kernel_main to main_smpap_routine) */
*(uint64_t *)&kernel_jump_addr = (uint64_t)main_smpap_routine;
/* And finally copy AP initialization code to the proper place */
memcpy(ap_code, &ap_boot_start, size);
/* ok setup new gdt */
for_each_cpu(c) {
int r;
if (!c) {
continue; /* Skip BSP CPU */
}
lapic_id = raw_percpu_get_var(lapic_ids, c);
kprintf("SEND INIT IPI TO CPU %d, cpuid=%d\n", *lapic_id, c);
if (lapic_init_ipi(*lapic_id)) {
panic("Can't send init interrupt\n");
}
interrupts_disable();
for (r = 0; r < 100; r++) {
default_hwclock->delay(1000);
if (is_cpu_online(c)) {
kprintf("online!\n");
break;
}
}
interrupts_enable();
if (!is_cpu_online(c)) {
kprintf("f**k! => %d\n", is_cpu_online(c));
for (;;);
panic("CPU %d is not online\n", c);
}
}
}
int smp_delete_all_threads(space_t * space)
{
//IPI_PRINTF("%s (%x)\n", __FUNCTION__, victim);
cpu_mailbox_t * mailbox = get_mailbox();
for (dword_t cpu = 0; cpu < CONFIG_SMP_MAX_CPU; cpu++)
{
if (cpu == get_cpu_id())
continue;
if (!is_cpu_online(cpu))
continue;
mailbox->param[0] = (dword_t)space;
dword_t status = mailbox->send_command(cpu, SMP_CMD_DELETE_ALL_THREADS);
switch(status)
{
case MAILBOX_OK:
return 1;
case MAILBOX_UNWIND_REMOTE:
/* we have to perform a remote unwind */
IPI_PRINTF("%s: remote unwind %x\n", mailbox->tcb);
unwind_ipc(mailbox->tcb);
break;
case MAILBOX_ERROR:
enter_kdebug("smp_delete_task: error deleting task");
break;
default:
enter_kdebug("smp_delete_task: unexpected return value");
break;
}
}
return 0;
}
void smp_startup_processors()
{
#define gdt_idx(x) ((x) >> 3)
/* the kernel code - kernel_space_exec */
smp_boot_gdt[gdt_idx(X86_KCS)].set_seg(0, ~0, 0, GDT_DESC_TYPE_CODE);
/* kernel data - linear_kernel_space */
smp_boot_gdt[gdt_idx(X86_KDS)].set_seg(0, ~0, 0, GDT_DESC_TYPE_DATA);
#if defined(CONFIG_DEBUG_TRACE_SMP)
printf("smp_start_processors\n");
#endif
memcpy((dword_t*)SMP_STARTUP_PAGE, (dword_t*)_start_smp, (dword_t)(_end_smp) - (dword_t)(_start_smp));
//enter_kdebug("after smp-copy");
wbinvd();
for (dword_t cpu = 0; cpu < CONFIG_SMP_MAX_CPU; cpu++)
{
// the boot cpu is not necessarily cpu 0
if (cpu == get_boot_cpu())
continue;
// start only processor which are in the processor map
if (!(processor_map & (1 << cpu)))
continue;
send_startup_ipi(cpu);
// wait for cpu to be online - aprox. 100 * 100us =
for (int i=0; i<100; i++) {
udelay(100);
if (is_cpu_online(cpu)) break;
}
if (!is_cpu_online(cpu)) {
printf("CPU %d failed to start\n", cpu);
}
else
{
printf("CPU %d is online\n", cpu);
}
}
//enter_kdebug("startup done");
}
/* returns: 0 = success
* < 0 = failure
* > 0 = partial success
*/
static int cpu_configure(cpu_set_t *cpu_set, size_t setsize, int configure)
{
unsigned int cpu;
int rc, current;
size_t fails = 0;
for (cpu = 0; cpu < setsize; cpu++) {
if (!CPU_ISSET(cpu, cpu_set))
continue;
if (!path_exist(_PATH_SYS_CPU "/cpu%d", cpu)) {
warnx(_("CPU %u does not exist"), cpu);
fails++;
continue;
}
if (!path_exist(_PATH_SYS_CPU "/cpu%d/configure", cpu)) {
warnx(_("CPU %u is not configurable"), cpu);
fails++;
continue;
}
current = path_read_s32(_PATH_SYS_CPU "/cpu%d/configure", cpu);
if ((current == 1) && (configure == 1)) {
printf(_("CPU %u is already configured\n"), cpu);
continue;
}
if ((current == 0) && (configure == 0)) {
printf(_("CPU %u is already deconfigured\n"), cpu);
continue;
}
if ((current == 1) && (configure == 0) && onlinecpus &&
is_cpu_online(cpu)) {
warnx(_("CPU %u deconfigure failed (CPU is enabled)"), cpu);
fails++;
continue;
}
if (configure) {
rc = path_write_str("1", _PATH_SYS_CPU "/cpu%d/configure", cpu);
if (rc == -1) {
warn(_("CPU %u configure failed"), cpu);
fails++;
} else
printf(_("CPU %u configured\n"), cpu);
} else {
rc = path_write_str("0", _PATH_SYS_CPU "/cpu%d/configure", cpu);
if (rc == -1) {
warn(_("CPU %u deconfigure failed"), cpu);
fails++;
} else
printf(_("CPU %u deconfigured\n"), cpu);
}
}
return fails == 0 ? 0 : fails == setsize ? -1 : 1;
}
void smp_flush_tlb()
{
#warning inefficient implementation of tlb shootdown
cpu_mailbox_t * mailbox = get_mailbox();
for (dword_t cpu = 0; cpu < CONFIG_SMP_MAX_CPU; cpu++)
{
if (cpu == get_cpu_id())
continue;
if (!is_cpu_online(cpu))
continue;
dword_t status = mailbox->send_command(cpu, SMP_CMD_FLUSH_TLB);
if (status != MAILBOX_OK)
enter_kdebug("smp_flush_tlb");
}
}
static int cpu_configure(cpu_set_t *cpu_set, size_t setsize, int configure)
{
unsigned int cpu;
int rc, current;
for (cpu = 0; cpu < setsize; cpu++) {
if (!CPU_ISSET(cpu, cpu_set))
continue;
if (!path_exist(_PATH_SYS_CPU "/cpu%d", cpu)) {
printf(_("CPU %d does not exist\n"), cpu);
continue;
}
if (!path_exist(_PATH_SYS_CPU "/cpu%d/configure", cpu)) {
printf(_("CPU %d is not configurable\n"), cpu);
continue;
}
current = path_getnum(_PATH_SYS_CPU "/cpu%d/configure", cpu);
if ((current == 1) && (configure == 1)) {
printf(_("CPU %d is already configured\n"), cpu);
continue;
}
if ((current == 0) && (configure == 0)) {
printf(_("CPU %d is already deconfigured\n"), cpu);
continue;
}
if ((current == 1) && (configure == 0) && onlinecpus &&
is_cpu_online(cpu)) {
printf(_("CPU %d deconfigure failed "
"(CPU is enabled)\n"), cpu);
continue;
}
if (configure) {
rc = path_writestr("1", _PATH_SYS_CPU "/cpu%d/configure", cpu);
if (rc == -1)
printf(_("CPU %d configure failed (%m)\n"), cpu);
else
printf(_("CPU %d configured\n"), cpu);
} else {
rc = path_writestr("0", _PATH_SYS_CPU "/cpu%d/configure", cpu);
if (rc == -1)
printf(_("CPU %d deconfigure failed (%m)\n"), cpu);
else
printf(_("CPU %d deconfigured\n"), cpu);
}
}
return EXIT_SUCCESS;
}
void smp_move_thread(tcb_t * tcb, dword_t cpu)
{
cpu_mailbox_t * mailbox = get_mailbox();
//mailbox->command = SMP_CMD_THREAD_MOVE;
mailbox->tcb = tcb;
//mailbox->status = MAILBOX_NULL;
mailbox->param[0] = tcb->queue_state;
//IPI_PRINTF("smp move thread %p from cpu %d to cpu %d\n", tcb, tcb->cpu, cpu);
/* do not move thread if already on cpu */
if (tcb->cpu == cpu)
return;
/* do not migrate to inactive cpus */
if (!is_cpu_online(cpu))
return;
retry_migration:
if (tcb->cpu == get_cpu_id())
{
/* we have the thread - so, we can give it away */
thread_dequeue_present(tcb);
thread_dequeue_ready(tcb);
thread_dequeue_wakeup(tcb);
IPI_PRINTF("before thread put (current: %x, pdir=%x, tcb: %x)\n",
get_current_tcb(), get_current_pagetable(), tcb);
mailbox->send_command(cpu, SMP_CMD_THREAD_PUT);
IPI_PRINTF("thread_put done (current: %x, pdir=%x, cpu: %d/%d)\n",
get_current_tcb(), get_current_pagetable(),
get_cpu_id(), get_apic_cpu_id());
}
else
{
/* we don't have the thread - ask the cpu */
dword_t status;
status = mailbox->send_command(tcb->cpu, SMP_CMD_THREAD_GET);
/* thread may have moved meanwhile */
if (status != MAILBOX_OK)
goto retry_migration;
if (cpu == get_cpu_id())
{
/* the thread comes to us */
tcb->cpu = cpu;
thread_adapt_queue_state(tcb, mailbox->param[1]);
/* adjust the page directory for this tcb */
//printf("pgdir: %x\n", tcb->page_dir);
thread_adapt_pagetable(tcb, get_cpu_id());
//printf("pgdir: %x\n", tcb->page_dir);
}
else
{
status = mailbox->send_command(cpu, SMP_CMD_THREAD_PUT);
if (status != MAILBOX_OK)
{
enter_kdebug("3-cpu thread migration failed");
return;
}
}
}
}
