kernel: Restore logical processor disabling

headers/private/kernel/kscheduler.h

@@ -83,6 +83,8 @@ status_t scheduler_set_operation_mode(scheduler_mode mode);
 */
 void scheduler_dump_thread_data(Thread* thread);
 
+void scheduler_set_cpu_enabled(int32 cpu, bool enabled);
+
 void scheduler_add_listener(struct SchedulerListener* listener);
 void scheduler_remove_listener(struct SchedulerListener* listener);
 

headers/private/kernel/thread.h

@@ -87,6 +87,8 @@ status_t thread_preboot_init_percpu(struct kernel_args *args, int32 cpuNum);
 void thread_yield(void);
 void thread_exit(void);
 
+void thread_map(void (*function)(Thread* thread, void* data), void* data);
+
 int32 thread_max_threads(void);
 int32 thread_used_threads(void);
 

src/system/kernel/cpu.cpp

@@ -18,6 +18,7 @@
 #include <cpufreq.h>
 
 #include <boot/kernel_args.h>
+#include <kscheduler.h>
 #include <thread_types.h>
 #include <util/AutoLock.h>
 
@@ -273,7 +274,6 @@ _user_cpu_enabled(int32 cpu)
 status_t
 _user_set_cpu_enabled(int32 cpu, bool enabled)
 {
-	status_t status = B_OK;
 	cpu_status state;
 	int32 i, count;
 
@@ -283,8 +283,7 @@ _user_set_cpu_enabled(int32 cpu, bool enabled)
 	// We need to lock here to make sure that no one can disable
 	// the last CPU
 
-	state = disable_interrupts();
-	acquire_spinlock(&sSetCpuLock);
+	InterruptsSpinLocker locker(sSetCpuLock);
 
 	if (!enabled) {
 		// check if this is the last CPU to be disabled
@@ -294,14 +293,37 @@ _user_set_cpu_enabled(int32 cpu, bool enabled)
 		}
 
 		if (count == 1)
-			status = B_NOT_ALLOWED;
+			return B_NOT_ALLOWED;
 	}
 
-	if (status == B_OK)
-		gCPU[cpu].disabled = !enabled;
+	bool oldState = gCPU[cpu].disabled;
 
-	release_spinlock(&sSetCpuLock);
-	restore_interrupts(state);
-	return status;
+	if (oldState != !enabled)
+		scheduler_set_cpu_enabled(cpu, enabled);
+
+	if (!enabled) {
+		if (smp_get_current_cpu() == cpu) {
+			locker.Unlock();
+			thread_yield();
+			locker.Lock();
+		}
+
+		// someone reenabled the CPU while we were rescheduling
+		if (!gCPU[cpu].disabled)
+			return B_OK;
+
+		ASSERT(smp_get_current_cpu() != cpu);
+		while (!thread_is_idle_thread(gCPU[cpu].running_thread)) {
+			locker.Unlock();
+			thread_yield();
+			locker.Lock();
+
+			if (!gCPU[cpu].disabled)
+				return B_OK;
+			ASSERT(smp_get_current_cpu() != cpu);
+		}
+	}
+
+	return B_OK;
 }
 

src/system/kernel/int.cpp

@@ -391,14 +391,17 @@ restore_interrupts(cpu_status status)
 static
 uint32 assign_cpu(void)
 {
-	int32 nextID = atomic_add(&sLastCPU, 1);
-	cpu_topology_node* node = get_cpu_topology();
+	cpu_topology_node* node;
+	do {
+		int32 nextID = atomic_add(&sLastCPU, 1);
+		node = get_cpu_topology();
 
-	while (node->level != CPU_TOPOLOGY_SMT) {
-		int levelSize = node->children_count;
-		node = node->children[nextID % levelSize];
-		nextID /= levelSize;
-	}
+		while (node->level != CPU_TOPOLOGY_SMT) {
+			int levelSize = node->children_count;
+			node = node->children[nextID % levelSize];
+			nextID /= levelSize;
+		}
+	} while (gCPU[node->id].disabled);
 
 	return node->id;
 }
@@ -680,6 +683,12 @@ void assign_io_interrupt_to_cpu(long vector, int32 newCPU)
 
 	int32 oldCPU = sVectors[vector].assigned_cpu.cpu;
 
+	if (newCPU == -1)
+		newCPU = assign_cpu();
+	dprintf_no_syslog("IRQ %ld CPU %" B_PRId32 " -> CPU %" B_PRId32 "\n", vector, oldCPU, newCPU);
+	if (newCPU == oldCPU)
+		return;
+
 	ASSERT(oldCPU != -1);
 	cpu_ent* cpu = &gCPU[oldCPU];
 
@@ -693,6 +702,5 @@ void assign_io_interrupt_to_cpu(long vector, int32 newCPU)
 	sVectors[vector].assigned_cpu.cpu = newCPU;
 	arch_int_assign_to_cpu(vector, newCPU);
 	list_add_item(&cpu->irqs, &sVectors[vector].assigned_cpu);
-	locker.Unlock();
 }
 

src/system/kernel/scheduler/scheduler.cpp

@@ -399,6 +399,11 @@ update_load_heaps(int32 core)
 
 	CoreEntry* entry = &gCoreEntries[core];
 
+	if (entry->fCPUCount == 0) {
+		entry->fLoad = 0;
+		return;
+	}
+
 	WriteSpinLocker coreLocker(gCoreHeapsLock);
 
 	int32 newKey = get_core_load(entry);
@@ -626,12 +631,9 @@ thread_goes_away(Thread* thread)
 
 	scheduler_thread_data* schedulerThreadData = thread->scheduler_data;
 
-	ASSERT(schedulerThreadData->previous_core >= 0);
-	int32 core = schedulerThreadData->previous_core;
-
 	schedulerThreadData->went_sleep = system_time();
 	schedulerThreadData->went_sleep_active
-		= atomic_get64(&gCoreEntries[core].fActiveTime);
+		= atomic_get64(&gCoreEntries[smp_get_current_cpu()].fActiveTime);
 }
 
 
@@ -1028,7 +1030,7 @@ track_cpu_activity(Thread* oldThread, Thread* nextThread, int32 thisCore)
 		atomic_add64(&gCoreEntries[thisCore].fActiveTime, active);
 	}
 
-	if (!gSingleCore)
+	if (!gSingleCore && !gCPU[smp_get_current_cpu()].disabled)
 		compute_cpu_load(smp_get_current_cpu());
 
 	int32 oldPriority = get_effective_priority(oldThread);
@@ -1150,9 +1152,24 @@ _scheduler_reschedule(void)
 	schedulerOldThreadData->lost_cpu = false;
 
 	// select thread with the biggest priority and enqueue back the old thread
-	Thread* nextThread
-		= choose_next_thread(thisCPU, enqueueOldThread ? oldThread : NULL,
-			putOldThreadAtBack);
+	Thread* nextThread;
+	if (gCPU[thisCPU].disabled) {
+		if (!thread_is_idle_thread(oldThread)) {
+			SpinLocker runQueueLocker(gCoreEntries[thisCore].fQueueLock);
+
+			nextThread = gPinnedRunQueues[thisCPU].GetHead(B_IDLE_PRIORITY);
+		 	gPinnedRunQueues[thisCPU].Remove(nextThread);
+			nextThread->scheduler_data->enqueued = false;
+
+			putOldThreadAtBack = oldThread->pinned_to_cpu == 0;
+		} else
+			nextThread = oldThread;
+	} else {
+		nextThread
+			= choose_next_thread(thisCPU, enqueueOldThread ? oldThread : NULL,
+				putOldThreadAtBack);
+	}
+
 	if (nextThread != oldThread) {
 		if (enqueueOldThread) {
 			if (putOldThreadAtBack)
@@ -1174,7 +1191,8 @@ _scheduler_reschedule(void)
 		oldThread, nextThread);
 
 	// update CPU heap
-	update_cpu_priority(thisCPU, get_effective_priority(nextThread));
+	if (!gCPU[thisCPU].disabled)
+		update_cpu_priority(thisCPU, get_effective_priority(nextThread));
 
 	nextThread->state = B_THREAD_RUNNING;
 	nextThread->next_state = B_THREAD_READY;
@@ -1281,6 +1299,22 @@ scheduler_start(void)
 }
 
 
+static inline void
+acquire_big_scheduler_lock(void)
+{
+	for (int32_t i = 0; i < smp_get_num_cpus(); i++)
+		acquire_write_spinlock(&gCPUEntries[i].fSchedulerModeLock);
+}
+
+
+static inline void
+release_big_scheduler_lock(void)
+{
+	for (int32_t i = 0; i < smp_get_num_cpus(); i++)
+		release_write_spinlock(&gCPUEntries[i].fSchedulerModeLock);
+}
+
+
 status_t
 scheduler_set_operation_mode(scheduler_mode mode)
 {
@@ -1292,20 +1326,144 @@ scheduler_set_operation_mode(scheduler_mode mode)
 	dprintf("scheduler: switching to %s mode\n", sSchedulerModes[mode]->name);
 
 	InterruptsLocker _;
-	for (int32_t i = 0; i < smp_get_num_cpus(); i++)
-		acquire_write_spinlock(&gCPUEntries[i].fSchedulerModeLock);
+	acquire_big_scheduler_lock();
 
 	sCurrentModeID = mode;
 	sCurrentMode = sSchedulerModes[mode];
 	sCurrentMode->switch_to_mode();
 
-	for (int32_t i = 0; i < smp_get_num_cpus(); i++)
-		release_write_spinlock(&gCPUEntries[i].fSchedulerModeLock);
+	release_big_scheduler_lock();
 
 	return B_OK;
 }
 
 
+static void
+unassign_thread(Thread* thread, void* data)
+{
+	int32 core = *(int32*)data;
+
+	if (thread->scheduler_data->previous_core == core
+		&& thread->pinned_to_cpu == 0) {
+		thread->scheduler_data->previous_core = -1;
+	}
+}
+
+
+void
+scheduler_set_cpu_enabled(int32 cpu, bool enabled)
+{
+	dprintf("scheduler: %s CPU %" B_PRId32 "\n",
+		enabled ? "enabling" : "disabling", cpu);
+
+	InterruptsLocker _;
+	acquire_big_scheduler_lock();
+
+	gCPU[cpu].disabled = !enabled;
+
+	CoreEntry* core = &gCoreEntries[gCPUToCore[cpu]];
+	PackageEntry* package = &gPackageEntries[gCPUToPackage[cpu]];
+
+	int32 oldCPUCount = core->fCPUCount;
+	ASSERT(oldCPUCount >= 0);
+	if (enabled)
+		core->fCPUCount++;
+	else {
+		update_cpu_priority(cpu, B_IDLE_PRIORITY);
+		core->fCPUCount--;
+	}
+
+	if (core->fCPUCount == 0) {
+		// core has been disabled
+		ASSERT(!enabled);
+
+		int32 load = CoreLoadHeap::GetKey(core);
+		if (load > kHighLoad) {
+			gCoreHighLoadHeap->ModifyKey(core, -1);
+			ASSERT(gCoreHighLoadHeap->PeekMinimum() == core);
+			gCoreHighLoadHeap->RemoveMinimum();
+		} else {
+			gCoreLoadHeap->ModifyKey(core, -1);
+			ASSERT(gCoreLoadHeap->PeekMinimum() == core);
+			gCoreLoadHeap->RemoveMinimum();
+		}
+
+		package->fIdleCores.Remove(core);
+		package->fIdleCoreCount--;
+		package->fCoreCount--;
+
+		if (package->fCoreCount == 0)
+			gIdlePackageList->Remove(package);
+
+		// get rid of threads
+		thread_map(unassign_thread, &core->fCoreID);
+
+		while (gRunQueues[core->fCoreID].PeekMaximum() != NULL) {
+			Thread* thread = gRunQueues[core->fCoreID].PeekMaximum();
+			gRunQueues[core->fCoreID].Remove(thread);
+			thread->scheduler_data->enqueued = false;
+
+			ASSERT(thread->scheduler_data->previous_core == -1);
+			enqueue(thread, false);
+		}
+	} else if (oldCPUCount == 0) {
+		// core has been reenabled
+		ASSERT(enabled);
+
+		gCPUEntries[cpu].fLoad = 0;
+		core->fLoad = 0;
+		gCoreLoadHeap->Insert(core, 0);
+
+		package->fCoreCount++;
+		package->fIdleCoreCount++;
+		package->fIdleCores.Add(core);
+
+		if (package->fCoreCount == 1)
+			gIdlePackageList->Add(package);		
+	}
+
+	if (enabled) {
+		gCPUPriorityHeaps[core->fCoreID].Insert(&gCPUEntries[cpu],
+			B_IDLE_PRIORITY);
+		gCPUEntries[cpu].fLoad = 0;
+	} else {
+		gCPUPriorityHeaps[core->fCoreID].ModifyKey(&gCPUEntries[cpu],
+			THREAD_MAX_SET_PRIORITY + 1);
+		ASSERT(gCPUPriorityHeaps[core->fCoreID].PeekMaximum()
+			== &gCPUEntries[cpu]);
+		gCPUPriorityHeaps[core->fCoreID].RemoveMaximum();
+
+		core->fLoad -= gCPUEntries[cpu].fLoad;
+	}
+
+	if (!enabled) {
+		cpu_ent* entry = &gCPU[cpu];
+
+		// get rid of irqs
+		SpinLocker locker(entry->irqs_lock);
+		irq_assignment* irq
+			= (irq_assignment*)list_get_first_item(&entry->irqs);
+		while (irq != NULL) {
+			locker.Unlock();
+
+			assign_io_interrupt_to_cpu(irq->irq, -1);
+
+			locker.Lock();
+			irq = (irq_assignment*)list_get_first_item(&entry->irqs);
+		}
+		locker.Unlock();
+
+		// don't wait until the thread quantum ends
+		if (smp_get_current_cpu() != cpu) {
+			smp_send_ici(cpu, SMP_MSG_RESCHEDULE, 0, 0, 0, NULL,
+				SMP_MSG_FLAG_ASYNC);
+		}
+	}
+
+	release_big_scheduler_lock();
+}
+
+
 static void
 traverse_topology_tree(cpu_topology_node* node, int packageID, int coreID)
 {

src/system/kernel/thread.cpp

@@ -2408,8 +2408,6 @@ peek_next_thread_id()
 void
 thread_yield(void)
 {
-	// Yielding is for being nice, not for making things work.
-#if !KDEBUG
 	Thread *thread = thread_get_current_thread();
 	if (thread == NULL)
 		return;
@@ -2418,7 +2416,18 @@ thread_yield(void)
 
 	thread->has_yielded = true;
 	scheduler_reschedule();
-#endif
+}
+
+
+void
+thread_map(void (*function)(Thread* thread, void* data), void* data)
+{
+	InterruptsSpinLocker threadHashLocker(sThreadHashLock);
+
+	for (ThreadHashTable::Iterator it = sThreadHash.GetIterator();
+		Thread* thread = it.Next();) {
+		function(thread, data);
+	}
 }