diff --git a/apps/tests.c b/apps/tests.c
index 26c1aba8..c84d117c 100644
--- a/apps/tests.c
+++ b/apps/tests.c
@@ -301,22 +301,65 @@ static int pi(void* arg)
 	return 0;
 }
 
-static int measure_ctx_switch(void)
-{
-	uint64_t max = 0;
-	uint64_t start, t1, t2;
-	uint64_t freq  = get_cpu_frequency();
+#define REPS 10000
 
-	start = t1 = rdtsc();
-	do {
-		asm volatile ("cpuid");
-		t2 = rdtsc();
-		if (t2 - t1 > max)
-			max = t2 - t1;
-		t1 = t2;
-	} while(t2-start < 10*freq);
+volatile uint64_t t1, t2;
+volatile int stop = !!0;
+volatile int sid = 0;
+
+static int measure_ctx_switch(void* arg)
+{
+	int id = !!(int)arg;
+	int oid = !id;
+	uint64_t freq  = get_cpu_frequency() *1000 *1000;
+	uint64_t diff, min = (uint64_t)-1, max = 0, avg = 0;
+	int i;
+	uint32_t a=0,b,c,d;
+
+	// Size of a timeslice in ticks
+	uint64_t timeslice = freq / TIMER_FREQ;
+  
+  	kprintf("ID: %d, ", id);
+#ifdef SW_TASK_SWITCH
+	kprintf("Measuring SW task switch.\n");
+#else
+	kprintf("Measuring HW task switch.\n");
+#endif
+
+	for (i=0; i < REPS && stop == 0; i++) {
+	  while(id == sid && stop == 0) {
+	    t2 = rdtsc();
+	    cpuid(0,&a,&b,&c,&d);
+	  }
+
+	  cpuid(0,&a,&b,&c,&d);
+	  diff = rdtsc() -t2;
+
+	  // The last measurement is garbage
+	  if (stop) break;
+	  // The first ones are garbage, too
+	  if (i < 5) goto next_try;
+	  if (diff >= timeslice) {
+	    i--;
+	    goto next_try;
+	  }
+
+	  kprintf("%i: diff= %llu, i= %i\n", id, diff, i);
+	  if (diff > max) max = diff;
+	  if (diff < min) min = diff;
+	  avg += diff;
+
+next_try:
+	  sid = id;
+	}
+	avg /= i-5;
+
+	stop = 1;
 
 	kprintf("maximum gap: %llu ticks\n", max);
+	kprintf("minimum gap: %llu ticks\n", min);
+	kprintf("average gap: %llu ticks\n", avg);
+	kprintf("Timeslice size: %llu ticks\n", timeslice);
 
 	return 0;
  }
@@ -332,7 +375,8 @@ int test_init(void)
 	sem_init(&consuming, 0);
 	mailbox_int32_init(&mbox);
 
-	create_kernel_task(NULL, measure_ctx_switch, NULL, NORMAL_PRIO);
+	create_kernel_task(NULL, measure_ctx_switch, (int)0, NORMAL_PRIO);
+	create_kernel_task(NULL, measure_ctx_switch, (int)1, NORMAL_PRIO);
 	//create_kernel_task(NULL, foo, "Hello from foo1", NORMAL_PRIO);
 	//create_kernel_task(NULL, foo, "Hello from foo2", NORMAL_PRIO);
 	//create_kernel_task(NULL, join_test, NULL, NORMAL_PRIO);
diff --git a/kernel/tasks.c b/kernel/tasks.c
index c7c560df..430b4cee 100644
--- a/kernel/tasks.c
+++ b/kernel/tasks.c
@@ -1378,8 +1378,7 @@ get_task_out:
 			orig_task->flags &= ~TASK_FPU_USED;
 		}
 
-		//kprintf("schedule from %u to %u with prio %u on core %u\n",
-		//	orig_task->id, curr_task->id, (uint32_t)curr_task->prio, CORE_ID);
+		//kprintf("schedule from %u to %u with prio %u on core %u\n", orig_task->id, curr_task->id, (uint32_t)curr_task->prio, CORE_ID);
 #ifndef SW_TASK_SWITCH
 		switch_task(curr_task->id);
 #endif