* Removed unused SMP_MSG_RESCHEDULE ICI message.

* Introduced flag "invoke_scheduler" in the per CPU structure. It is
  evaluated in hardware_interrupt() (x86 only ATM).
* Introduced SMP_MSG_RESCHEDULE_IF_IDLE message, which enters the
  scheduler when the CPU currently runs an idle thread.
* Don't do dprintf() "CPU x halted!" when handling a SMP_MSG_CPU_HALT
  ICI message. It uses nested spinlocks and could thus potentially
  deadlock itself (acquire_spinlock() processes ICI messages, so it
  could already hold one of the locks). This is a pretty likely scenario
  on machines with more than two CPUs, but is also possible when the
  panic()ing thread holds the threads spinlock. Probably fixes #2572.
* Reworked the way the kernel debugger is entered and added a "cpu"
  command that allows switching the CPU once in KDL. It is thus possible
  to get a stack trace of the thread not on the panic()ing CPU.
* When a thread is added to the run queue, we do now check, if another
  CPU is idle and ask it to reschedule, if it is. Before this change, the
  CPU was continuing to idle until the quantum of the idle thread
  expired. Speeds up the libbe.so build about 8% on my machine (haven't
  tested the full Haiku image build yet).
* When spinlock debugging is enabled (DEBUG_SPINLOCKS) we also record
  the spinlock acquirer on non-smp machines. Added "spinlock" debugger
  command to get the info.
* Added debugger commands "ici" and "ici_message", printing info on
  pending ICI message respectively on a given one.
* Process not only a single ICI message in acquire_spinlock() and other
  places, but all pending ones.
* Also process ICI messages when waiting for a free one -- avoids a
  potential deadlock.
* Mask out non-existing CPUs in send_multicast_ici(). panic() instead of
  just returning when there's no target CPU left.


git-svn-id: file:///srv/svn/repos/haiku/haiku/trunk@28223 a95241bf-73f2-0310-859d-f6bbb57e9c96

headers/private/kernel/cpu.h

@@ -35,6 +35,7 @@ typedef struct cpu_ent {
 	bigtime_t last_kernel_time;
 	bigtime_t last_user_time;
 
+	bool invoke_scheduler;
 	bool disabled;
 
 	// arch-specific stuff

headers/private/kernel/debug.h

@@ -108,6 +108,7 @@ extern void		debug_stop_screen_debug_output(void);
 extern void		debug_set_page_fault_info(addr_t faultAddress, addr_t pc,
 					uint32 flags);
 extern debug_page_fault_info* debug_get_page_fault_info();
+extern void debug_trap_cpu_in_kdl(bool returnIfHandedOver);
 
 extern void		kputs(const char *string);
 extern void		kputs_unfiltered(const char *string);

headers/private/kernel/smp.h

@@ -20,9 +20,9 @@ enum {
 	SMP_MSG_INVALIDATE_PAGE_LIST,
 	SMP_MSG_USER_INVALIDATE_PAGES,
 	SMP_MSG_GLOBAL_INVALIDATE_PAGES,
-	SMP_MSG_RESCHEDULE,
 	SMP_MSG_CPU_HALT,
 	SMP_MSG_CALL_FUNCTION,
+	SMP_MSG_RESCHEDULE_IF_IDLE
 };
 
 enum {

src/system/kernel/arch/x86/arch_int.c

@@ -914,7 +914,7 @@ hardware_interrupt(struct iframe* frame)
 	if (levelTriggered)
 		sCurrentPIC->end_of_interrupt(vector);
 
-	if (ret == B_INVOKE_SCHEDULER) {
+	if (ret == B_INVOKE_SCHEDULER || thread->cpu->invoke_scheduler) {
 		cpu_status state = disable_interrupts();
 		GRAB_THREAD_LOCK();
 

src/system/kernel/debug/debug.cpp

@@ -1,4 +1,5 @@
 /*
+ * Copyright 2008, Ingo Weinhold, [email protected].
  * Copyright 2002-2008, Axel Dörfler, [email protected].
  * Distributed under the terms of the MIT License.
  *
@@ -10,6 +11,7 @@
 
 #include "blue_screen.h"
 
+#include <cpu.h>
 #include <debug.h>
 #include <debug_paranoia.h>
 #include <driver_settings.h>
@@ -45,6 +47,8 @@ static const char* const kKDLPrompt = "kdebug> ";
 
 extern "C" int kgets(char *buffer, int length);
 
+void call_modules_hook(bool enter);
+
 
 int dbg_register_file[B_MAX_CPU_COUNT][14];
 	/* XXXmpetit -- must be made generic */
@@ -94,6 +98,10 @@ static int32 sCurrentLine = 0;
 static debugger_demangle_module_info* sDemangleModule;
 
 static struct thread* sDebuggedThread;
+static vint32 sInDebugger = 0;
+static bool sPreviousDprintfState;
+static volatile bool sHandOverKDL = false;
+static vint32 sHandOverKDLToCPU = -1;
 
 #define distance(a, b) ((a) < (b) ? (b) - (a) : (a) - (b))
 
@@ -714,6 +722,83 @@ kernel_debugger_loop(void)
 }
 
 
+static void
+enter_kernel_debugger(const char* message)
+{
+	while (atomic_add(&sInDebugger, 1) > 0) {
+		// The debugger is already running, find out where...
+		if (sDebuggerOnCPU == smp_get_current_cpu()) {
+			// We are re-entering the debugger on the same CPU.
+			break;
+		}
+
+		// Some other CPU must have entered the debugger and tried to halt
+		// us. Process ICIs to ensure we get the halt request. Then we are
+		// blocking there until everyone leaves the debugger and we can
+		// try to enter it again.
+		atomic_add(&sInDebugger, -1);
+		smp_intercpu_int_handler();
+	}
+
+	arch_debug_save_registers(&dbg_register_file[smp_get_current_cpu()][0]);
+	sPreviousDprintfState = set_dprintf_enabled(true);
+
+	if (sDebuggerOnCPU != smp_get_current_cpu() && smp_get_num_cpus() > 1) {
+		// First entry on a MP system, send a halt request to all of the other
+		// CPUs. Should they try to enter the debugger they will be cought in
+		// the loop above.
+		smp_send_broadcast_ici(SMP_MSG_CPU_HALT, 0, 0, 0, NULL,
+			SMP_MSG_FLAG_SYNC);
+	}
+
+	if (sBlueScreenOutput) {
+		if (blue_screen_enter(false) == B_OK)
+			sBlueScreenEnabled = true;
+	}
+
+	sDebugOutputFilter = &gDefaultDebugOutputFilter;
+
+	sDebuggedThread = NULL;
+
+	if (message)
+		kprintf("PANIC: %s\n", message);
+
+	sCurrentKernelDebuggerMessage = message;
+
+	// sort the commands
+	sort_debugger_commands();
+
+	call_modules_hook(true);
+}
+
+
+static void
+exit_kernel_debugger()
+{
+	call_modules_hook(false);
+	set_dprintf_enabled(sPreviousDprintfState);
+
+	sDebugOutputFilter = NULL;
+
+	sBlueScreenEnabled = false;
+	atomic_add(&sInDebugger, -1);
+}
+
+
+static void
+hand_over_kernel_debugger()
+{
+	// Wait until the hand-over is complete.
+	// The other CPU gets our sInDebugger reference and will release it when
+	// done. Note, that there's a small race condition: the other CPU could
+	// hand over to another CPU without us noticing. Since this is only
+	// initiated by the user, it is harmless, though.
+	sHandOverKDL = true;
+	while (sHandOverKDLToCPU >= 0)
+		PAUSE();
+}
+
+
 static int
 cmd_dump_kdl_message(int argc, char **argv)
 {
@@ -783,6 +868,36 @@ cmd_dump_syslog(int argc, char **argv)
 }
 
 
+static int
+cmd_switch_cpu(int argc, char **argv)
+{
+	if (argc > 2) {
+		print_debugger_command_usage(argv[0]);
+		return 0;
+	}
+
+	if (argc == 1) {
+		kprintf("running on CPU %ld\n", smp_get_current_cpu());
+		return 0;
+	}
+
+	int32 newCPU = parse_expression(argv[1]);
+	if (newCPU < 0 || newCPU >= smp_get_num_cpus()) {
+		kprintf("invalid CPU index\n");
+		return 0;
+	}
+
+	if (newCPU == smp_get_current_cpu()) {
+		kprintf("already running on CPU %ld\n", newCPU);
+		return 0;
+	}
+
+	sHandOverKDLToCPU = newCPU;
+
+	return B_KDEBUG_QUIT;
+}
+
+
 static status_t
 syslog_sender(void *data)
 {
@@ -949,6 +1064,7 @@ syslog_init(struct kernel_args *args)
 		"Dumps the syslog buffer.\n",
 		"[-n]\nDumps the whole syslog buffer, or, if -n is specified, only "
 		"the part that hasn't been send yet.\n", 0);
+
 	return B_OK;
 
 err2:
@@ -1058,6 +1174,10 @@ debug_init(kernel_args *args)
 status_t
 debug_init_post_vm(kernel_args *args)
 {
+	add_debugger_command_etc("cpu", &cmd_switch_cpu,
+		"Switches to another CPU.\n",
+		"<cpu>\n"
+		"Switches to CPU with the index <cpu>.\n", 0);
 	add_debugger_command_etc("message", &cmd_dump_kdl_message,
 		"Reprint the message printed when entering KDL",
 		"\n"
@@ -1162,6 +1282,25 @@ debug_get_page_fault_info()
 }
 
 
+void
+debug_trap_cpu_in_kdl(bool returnIfHandedOver)
+{
+	cpu_status state = disable_interrupts();
+
+	while (sInDebugger != 0) {
+		if (sHandOverKDL && sHandOverKDLToCPU == smp_get_current_cpu()) {
+			if (returnIfHandedOver)
+				return;
+
+			kernel_debugger(NULL);
+		} else
+			PAUSE();
+	}
+
+	restore_interrupts(state);
+}
+
+
 //	#pragma mark - public API
 
 
@@ -1190,68 +1329,31 @@ panic(const char *format, ...)
 void
 kernel_debugger(const char *message)
 {
-	static vint32 inDebugger = 0;
-	cpu_status state;
-	bool dprintfState;
+	cpu_status state = disable_interrupts();
 
-	state = disable_interrupts();
-	while (atomic_add(&inDebugger, 1) > 0) {
-		// The debugger is already running, find out where...
-		if (sDebuggerOnCPU != smp_get_current_cpu()) {
-			// Some other CPU must have entered the debugger and tried to halt
-			// us. Process ICIs to ensure we get the halt request. Then we are
-			// blocking there until everyone leaves the debugger and we can
-			// try to enter it again.
-			atomic_add(&inDebugger, -1);
-			smp_intercpu_int_handler();
-		} else {
-			// We are re-entering the debugger on the same CPU.
+	while (true) {
+		if (sHandOverKDLToCPU == smp_get_current_cpu()) {
+			sHandOverKDLToCPU = -1;
+			sHandOverKDL = false;
+		} else
+			enter_kernel_debugger(message);
+
+		kernel_debugger_loop();
+
+		if (sHandOverKDLToCPU < 0) {
+			exit_kernel_debugger();
 			break;
 		}
+
+		hand_over_kernel_debugger();
+
+		debug_trap_cpu_in_kdl(true);
+
+		if (sHandOverKDLToCPU != smp_get_current_cpu())
+			break;
 	}
 
-	arch_debug_save_registers(&dbg_register_file[smp_get_current_cpu()][0]);
-	dprintfState = set_dprintf_enabled(true);
-
-	if (sDebuggerOnCPU != smp_get_current_cpu() && smp_get_num_cpus() > 1) {
-		// First entry on a MP system, send a halt request to all of the other
-		// CPUs. Should they try to enter the debugger they will be cought in
-		// the loop above.
-		smp_send_broadcast_ici(SMP_MSG_CPU_HALT, 0, 0, 0,
-			(void *)&inDebugger, SMP_MSG_FLAG_SYNC);
-	}
-
-	if (sBlueScreenOutput) {
-		if (blue_screen_enter(false) == B_OK)
-			sBlueScreenEnabled = true;
-	}
-
-	sDebugOutputFilter = &gDefaultDebugOutputFilter;
-
-	sDebuggedThread = NULL;
-
-	if (message)
-		kprintf("PANIC: %s\n", message);
-
-	sCurrentKernelDebuggerMessage = message;
-
-	// sort the commands
-	sort_debugger_commands();
-
-	call_modules_hook(true);
-
-	kernel_debugger_loop();
-
-	call_modules_hook(false);
-	set_dprintf_enabled(dprintfState);
-
-	sDebugOutputFilter = NULL;
-
-	sBlueScreenEnabled = false;
-	atomic_add(&inDebugger, -1);
 	restore_interrupts(state);
-
-	// ToDo: in case we change dbg_register_file - don't we want to restore it?
 }
 
 

src/system/kernel/scheduler.cpp

@@ -222,6 +222,7 @@ private:
 
 // The run queue. Holds the threads ready to run ordered by priority.
 static struct thread *sRunQueue = NULL;
+static cpu_mask_t sIdleCPUs = 0;
 
 
 static int
@@ -506,6 +507,43 @@ scheduler_enqueue_in_run_queue(struct thread *thread)
 		sRunQueue = thread;
 
 	thread->next_priority = thread->priority;
+
+	if (thread->priority != B_IDLE_PRIORITY) {
+		int32 currentCPU = smp_get_current_cpu();
+		if (sIdleCPUs != 0) {
+			if (thread->pinned_to_cpu > 0) {
+				// thread is pinned to a CPU -- notify it, if it is idle
+				int32 targetCPU = thread->previous_cpu->cpu_num;
+				if ((sIdleCPUs & (1 << targetCPU)) != 0) {
+					sIdleCPUs &= ~(1 << targetCPU);
+					smp_send_ici(targetCPU, SMP_MSG_RESCHEDULE_IF_IDLE, 0, 0,
+						0, NULL, SMP_MSG_FLAG_ASYNC);
+				}
+			} else {
+				// Thread is not pinned to any CPU -- take it ourselves, if we
+				// are idle, otherwise notify the next idle CPU. In either case
+				// we clear the idle bit of the chosen CPU, so that the
+				// scheduler_enqueue_in_run_queue() won't try to bother the
+				// same CPU again, if invoked before it handled the interrupt.
+				cpu_mask_t idleCPUs = CLEAR_BIT(sIdleCPUs, currentCPU);
+				if ((sIdleCPUs & (1 << currentCPU)) != 0) {
+					sIdleCPUs = idleCPUs;
+				} else {
+					int32 targetCPU = 0;
+					for (; targetCPU < B_MAX_CPU_COUNT; targetCPU++) {
+						cpu_mask_t mask = 1 << targetCPU;
+						if ((idleCPUs & mask) != 0) {
+							sIdleCPUs &= ~mask;
+							break;
+						}
+					}
+
+					smp_send_ici(targetCPU, SMP_MSG_RESCHEDULE_IF_IDLE, 0, 0,
+						0, NULL, SMP_MSG_FLAG_ASYNC);
+				}
+			}
+		}
+	}
 }
 
 
@@ -581,6 +619,8 @@ scheduler_reschedule(void)
 
 	TRACE(("reschedule(): cpu %d, cur_thread = %ld\n", smp_get_current_cpu(), thread_get_current_thread()->id));
 
+	oldThread->cpu->invoke_scheduler = false;
+
 	oldThread->state = oldThread->next_state;
 	switch (oldThread->next_state) {
 		case B_THREAD_RUNNING:
@@ -707,6 +747,13 @@ scheduler_reschedule(void)
 		add_timer(quantumTimer, &reschedule_event, quantum,
 			B_ONE_SHOT_RELATIVE_TIMER | B_TIMER_ACQUIRE_THREAD_LOCK);
 
+		// update the idle bit for this CPU in the CPU mask
+		int32 cpuNum = smp_get_current_cpu();
+		if (nextThread->priority == B_IDLE_PRIORITY)
+			sIdleCPUs = SET_BIT(sIdleCPUs, cpuNum);
+		else
+			sIdleCPUs = CLEAR_BIT(sIdleCPUs, cpuNum);
+
 		if (nextThread != oldThread)
 			context_switch(oldThread, nextThread);
 	}

src/system/kernel/smp.cpp

@@ -1,4 +1,5 @@
 /*
+ * Copyright 2008, Ingo Weinhold, ingo_weinhold@gmx.de.
  * Copyright 2002-2008, Axel Dörfler, axeld@pinc-software.de.
  * Distributed under the terms of the MIT License.
  *
@@ -76,36 +77,137 @@ static struct {
 	void		*caller;
 	spinlock	*lock;
 } sLastCaller[NUM_LAST_CALLERS];
-static int32 sLastIndex = 0;
+static vint32 sLastIndex = 0;
 
 
 static void
 push_lock_caller(void *caller, spinlock *lock)
 {
-	sLastCaller[sLastIndex].caller = caller;
-	sLastCaller[sLastIndex].lock = lock;
+	int32 index = atomic_add(&sLastIndex, 1) % NUM_LAST_CALLERS;
 
-	if (++sLastIndex >= NUM_LAST_CALLERS)
-		sLastIndex = 0;
+	sLastCaller[index].caller = caller;
+	sLastCaller[index].lock = lock;
 }
 
 
 static void *
 find_lock_caller(spinlock *lock)
 {
-	int32 i;
+	int32 lastIndex = sLastIndex % NUM_LAST_CALLERS;
 
-	for (i = 0; i < NUM_LAST_CALLERS; i++) {
-		int32 index = (NUM_LAST_CALLERS + sLastIndex - 1 - i) % NUM_LAST_CALLERS;
+	for (int32 i = 0; i < NUM_LAST_CALLERS; i++) {
+		int32 index = (NUM_LAST_CALLERS + lastIndex - 1 - i) % NUM_LAST_CALLERS;
 		if (sLastCaller[index].lock == lock)
 			return sLastCaller[index].caller;
 	}
 
 	return NULL;
 }
+
+
+int
+dump_spinlock(int argc, char** argv)
+{
+	if (argc != 2) {
+		print_debugger_command_usage(argv[0]);
+		return 0;
+	}
+
+	uint64 address;
+	if (!evaluate_debug_expression(argv[1], &address, false))
+		return 0;
+
+	spinlock* lock = (spinlock*)(addr_t)address;
+	kprintf("spinlock %p:\n", lock);
+	bool locked = B_SPINLOCK_IS_LOCKED(lock);
+	if (locked) {
+		kprintf("  locked from %p\n", find_lock_caller(lock));
+	} else
+		kprintf("  not locked\n");
+
+	return 0;
+}
+
+
 #endif	// DEBUG_SPINLOCKS
 
 
+int
+dump_ici_messages(int argc, char** argv)
+{
+	// count broadcast messages
+	int32 count = 0;
+	int32 doneCount = 0;
+	int32 unreferencedCount = 0;
+	smp_msg* message = sBroadcastMessages;
+	while (message != NULL) {
+		count++;
+		if (message->done)
+			doneCount++;
+		if (message->ref_count <= 0)
+			unreferencedCount++;
+		message = message->next;
+	}
+
+	kprintf("ICI broadcast messages: %ld, first: %p\n", count,
+		sBroadcastMessages);
+	kprintf("  done:         %ld\n", doneCount);
+	kprintf("  unreferenced: %ld\n", unreferencedCount);
+
+	// count per-CPU messages
+	for (int32 i = 0; i < sNumCPUs; i++) {
+		count = 0;
+		message = sCPUMessages[i];
+		while (message != NULL) {
+			count++;
+			message = message->next;
+		}
+
+		kprintf("CPU %ld messages: %ld, first: %p\n", i, count,
+			sCPUMessages[i]);
+	}
+
+	return 0;
+}
+
+
+int
+dump_ici_message(int argc, char** argv)
+{
+	if (argc != 2) {
+		print_debugger_command_usage(argv[0]);
+		return 0;
+	}
+
+	uint64 address;
+	if (!evaluate_debug_expression(argv[1], &address, false))
+		return 0;
+
+	smp_msg* message = (smp_msg*)(addr_t)address;
+	kprintf("ICI message %p:\n", message);
+	kprintf("  next:        %p\n", message->next);
+	kprintf("  message:     %ld\n", message->message);
+	kprintf("  data:        %ld\n", message->data);
+	kprintf("  data2:       %ld\n", message->data2);
+	kprintf("  data3:       %ld\n", message->data3);
+	kprintf("  data_ptr:    %p\n", message->data_ptr);
+	kprintf("  flags:       %lx\n", message->flags);
+	kprintf("  ref_count:   %lx\n", message->ref_count);
+	kprintf("  done:        %s\n", message->done ? "true" : "false");
+	kprintf("  proc_bitmap: %lx\n", message->proc_bitmap);
+
+	return 0;
+}
+
+
+static inline void
+process_all_pending_ici(int32 currentCPU)
+{
+	while (process_pending_ici(currentCPU) != B_ENTRY_NOT_FOUND)
+		;
+}
+
+
 void
 acquire_spinlock(spinlock *lock)
 {
@@ -115,16 +217,20 @@ acquire_spinlock(spinlock *lock)
 			panic("acquire_spinlock: attempt to acquire lock %p with interrupts enabled\n", lock);
 #if B_DEBUG_SPINLOCK_CONTENTION
 		while (atomic_add(&lock->lock, 1) != 0)
-			process_pending_ici(currentCPU);
+			process_all_pending_ici(currentCPU);
 #else
 		while (1) {
 			while (*lock != 0) {
-				process_pending_ici(currentCPU);
+				process_all_pending_ici(currentCPU);
 				PAUSE();
 			}
 			if (atomic_set((int32 *)lock, 1) == 0)
 				break;
 		}
+
+#if DEBUG_SPINLOCKS
+		push_lock_caller(arch_debug_get_caller(), lock);
+#endif
 #endif
 	} else {
 #if DEBUG_SPINLOCKS
@@ -156,7 +262,7 @@ acquire_spinlock_nocheck(spinlock *lock)
 		}
 #else
 		while (1) {
-			while(*lock != 0)
+			while (*lock != 0)
 				PAUSE();
 			if (atomic_set((int32 *)lock, 1) == 0)
 				break;
@@ -220,8 +326,12 @@ find_free_message(struct smp_msg **msg)
 	TRACE(("find_free_message: entry\n"));
 
 retry:
-	while (sFreeMessageCount <= 0)
+	while (sFreeMessageCount <= 0) {
+		state = disable_interrupts();
+		process_all_pending_ici(smp_get_current_cpu());
+		restore_interrupts(state);
 		PAUSE();
+	}
 	state = disable_interrupts();
 	acquire_spinlock(&sFreeMessageSpinlock);
 
@@ -327,7 +437,10 @@ finish_message_processing(int currentCPU, struct smp_msg *msg, int source_mailbo
 
 		TRACE(("cleaning up message %p\n", msg));
 
-		if (msg == *mbox) {
+		if (source_mailbox != MAILBOX_BCAST) {
+			// local mailbox -- the message has already been removed in
+			// check_for_message()
+		} else if (msg == *mbox) {
 			(*mbox) = msg->next;
 		} else {
 			// we need to walk to find the message in the list.
@@ -347,7 +460,7 @@ finish_message_processing(int currentCPU, struct smp_msg *msg, int source_mailbo
 			if (msg1 == msg && last != NULL)
 				last->next = msg->next;
 			else
-				dprintf("last == NULL or msg != msg1!!!\n");
+				panic("last == NULL or msg != msg1");
 		}
 
 		release_spinlock(spinlock);
@@ -370,13 +483,13 @@ static int32
 process_pending_ici(int32 currentCPU)
 {
 	struct smp_msg *msg;
-	vint32 *haltValue = NULL;
+	bool haltCPU = false;
 	int sourceMailbox = 0;
 	int retval = B_HANDLED_INTERRUPT;
 
 	msg = check_for_message(currentCPU, &sourceMailbox);
 	if (msg == NULL)
-		return retval;
+		return B_ENTRY_NOT_FOUND;
 
 	TRACE(("  cpu %ld message = %ld\n", currentCPU, msg->message));
 
@@ -393,12 +506,8 @@ process_pending_ici(int32 currentCPU)
 		case SMP_MSG_GLOBAL_INVALIDATE_PAGES:
 			arch_cpu_global_TLB_invalidate();
 			break;
-		case SMP_MSG_RESCHEDULE:
-			retval = B_INVOKE_SCHEDULER;
-			break;
 		case SMP_MSG_CPU_HALT:
-			haltValue = (vint32 *)msg->data_ptr;
-			dprintf("CPU %ld halted!\n", currentCPU);
+			haltCPU = true;
 			break;
 		case SMP_MSG_CALL_FUNCTION:
 		{
@@ -406,6 +515,13 @@ process_pending_ici(int32 currentCPU)
 			func(msg->data, currentCPU, msg->data2, msg->data3);
 			break;
 		}
+		case SMP_MSG_RESCHEDULE_IF_IDLE:
+		{
+			struct thread* thread = thread_get_current_thread();
+			if (thread->priority == B_IDLE_PRIORITY)
+				thread->cpu->invoke_scheduler = true;
+			break;
+		}
 
 		default:
 			dprintf("smp_intercpu_int_handler: got unknown message %ld\n", msg->message);
@@ -415,14 +531,8 @@ process_pending_ici(int32 currentCPU)
 	finish_message_processing(currentCPU, msg, sourceMailbox);
 
 	// special case for the halt message
-	if (haltValue) {
-		cpu_status state = disable_interrupts();
-
-		while (*haltValue != 0)
-			PAUSE();
-
-		restore_interrupts(state);
-	}
+	if (haltCPU)
+		debug_trap_cpu_in_kdl(false);
 
 	return retval;
 }
@@ -476,16 +586,15 @@ spinlock_contention_syscall(const char* subsystem, uint32 function,
 int
 smp_intercpu_int_handler(void)
 {
-	int retval;
 	int currentCPU = smp_get_current_cpu();
 
 	TRACE(("smp_intercpu_int_handler: entry on cpu %d\n", currentCPU));
 
-	retval = process_pending_ici(currentCPU);
+	process_all_pending_ici(currentCPU);
 
 	TRACE(("smp_intercpu_int_handler: done\n"));
 
-	return retval;
+	return B_HANDLED_INTERRUPT;
 }
 
 
@@ -535,7 +644,7 @@ smp_send_ici(int32 targetCPU, int32 message, uint32 data, uint32 data2, uint32 d
 			// the interrupt handler will ref count it to <0
 			// if the message is sync after it has removed it from the mailbox
 			while (msg->done == false) {
-				process_pending_ici(currentCPU);
+				process_all_pending_ici(currentCPU);
 				PAUSE();
 			}
 			// for SYNC messages, it's our responsibility to put it
@@ -556,9 +665,12 @@ smp_send_multicast_ici(cpu_mask_t cpuMask, int32 message, uint32 data,
 		return;
 
 	int currentCPU = smp_get_current_cpu();
-	cpuMask &= ~((cpu_mask_t)1 << currentCPU);
-	if (cpuMask == 0)
+	cpuMask &= ~((cpu_mask_t)1 << currentCPU)
+		& (((cpu_mask_t)1 << sNumCPUs) - 1);
+	if (cpuMask == 0) {
+		panic("smp_send_multicast_ici(): 0 CPU mask");
 		return;
+	}
 
 	// count target CPUs
 	int32 targetCPUs = 0;
@@ -595,7 +707,7 @@ smp_send_multicast_ici(cpu_mask_t cpuMask, int32 message, uint32 data,
 		// the interrupt handler will ref count it to <0
 		// if the message is sync after it has removed it from the mailbox
 		while (msg->done == false) {
-			process_pending_ici(currentCPU);
+			process_all_pending_ici(currentCPU);
 			PAUSE();
 		}
 
@@ -656,7 +768,7 @@ smp_send_broadcast_ici(int32 message, uint32 data, uint32 data2, uint32 data3,
 			TRACE(("smp_send_broadcast_ici: waiting for ack\n"));
 
 			while (msg->done == false) {
-				process_pending_ici(currentCPU);
+				process_all_pending_ici(currentCPU);
 				PAUSE();
 			}
 
@@ -724,6 +836,21 @@ smp_init(kernel_args *args)
 
 	TRACE(("smp_init: entry\n"));
 
+#if DEBUG_SPINLOCKS
+	add_debugger_command_etc("spinlock", &dump_spinlock,
+		"Dump info on a spinlock",
+		"\n"
+		"Dumps info on a spinlock.\n", 0);
+#endif
+	add_debugger_command_etc("ici", &dump_ici_messages,
+		"Dump info on pending ICI messages",
+		"\n"
+		"Dumps info on pending ICI messages.\n", 0);
+	add_debugger_command_etc("ici_message", &dump_ici_message,
+		"Dump info on an ICI message",
+		"\n"
+		"Dumps info on an ICI message.\n", 0);
+
 	if (args->num_cpus > 1) {
 		sFreeMessages = NULL;
 		sFreeMessageCount = 0;