diff --git a/Makefile b/Makefile
index d60b54a..4b21aab 100644
--- a/Makefile
+++ b/Makefile
@@ -39,7 +39,7 @@ deps := $(OBJS:%.o=.%.o.d)
 
 .PHONY: all check clean distclean
 
-all: $(CMSIS)/$(PLAT) $(NAME).lds $(NAME).bin
+all: FORCE $(CMSIS)/$(PLAT) $(NAME).lds $(NAME).bin
 
 # generic build rules
 include mk/flags.mk
@@ -73,3 +73,5 @@ distclean: clean
 include platform/$(PLAT)/build.mk
 
 -include $(deps)
+
+FORCE:
diff --git a/README.md b/README.md
index 5ff1810..5895b19 100644
--- a/README.md
+++ b/README.md
@@ -15,3 +15,17 @@ $ python -m tests
 # Run partial test with command line tools
 $ python -m tests fs_1 cond_2
 ```
+
+## Makefile
+
+You can switch scheduler by makefile flags:
+
+```
+# Using bitmap scheduler
+
+$ make SCHED=BITMAP
+
+# Using RR scheduler
+
+$ make SCHED=RR
+```
diff --git a/include/kernel/sched.h b/include/kernel/sched.h
index 540be02..3ade7d9 100644
--- a/include/kernel/sched.h
+++ b/include/kernel/sched.h
@@ -18,13 +18,13 @@ struct thread_info;
 #define SCHED_OPT_TICK 3
 
 struct sched {
-    int (*init)(void);
+    int (*init)(struct thread_info *thread);
     int (*enqueue)(struct thread_info *thread);
     int (*dequeue)(struct thread_info *thread);
     int (*elect)(int switch_type);
 };
 
-int sched_select(int sched_type);
+int sched_select(int sched_type, struct thread_info *thread);
 int sched_enqueue(struct thread_info *thread);
 int sched_dequeue(struct thread_info *thread);
 int sched_elect(int flags);
diff --git a/include/kernel/thread.h b/include/kernel/thread.h
index 8557c31..16146f3 100644
--- a/include/kernel/thread.h
+++ b/include/kernel/thread.h
@@ -35,6 +35,8 @@ struct thread_info {
     struct list_head ti_list; /* global list of threads */
     struct list_head ti_q; /* shared by sched runq, mutex waitq, thread joinq */
 
+    int ti_queued; /* in runqueue */
+
     /* http://www.domaigne.com/blog/computing/joinable-and-detached-threads/ */
     void *ti_retval;
     int ti_detached;
diff --git a/kernel/cond.c b/kernel/cond.c
index 7d09f21..e1f1ba7 100644
--- a/kernel/cond.c
+++ b/kernel/cond.c
@@ -26,6 +26,7 @@ int sys_pthread_cond_wait(pthread_cond_t *cond, kernel_mutex_t *mutex)
     CURRENT_THREAD_INFO(curr_thread);
     curr_thread->ti_private = cond;
     curr_thread->ti_state = THREAD_STATE_BLOCKED;
+    sched_dequeue(curr_thread);
     list_add_tail(&curr_thread->ti_q, &cond_head);
     sys_pthread_mutex_unlock(mutex);
 
diff --git a/kernel/main.c b/kernel/main.c
index 5177108..6a04ed5 100644
--- a/kernel/main.c
+++ b/kernel/main.c
@@ -45,6 +45,11 @@ void init_IRQ(void);
 struct task_info idle_task;
 struct task_info main_task;
 
+/* Select scheduler */
+#ifndef SCHED_CLASS
+#define SCHED_CLASS SCHED_CLASS_BITMAP
+#endif
+
 void print_version(void)
 {
     char buf[] = {0, 0};
@@ -130,9 +135,6 @@ struct thread_info *start_kernel(void)
     show_page_bitmap();  // init_pages();
     kmem_cache_init();
 
-    /* select a scheduling policy */
-    sched_select(SCHED_CLASS_BITMAP);
-
     /* idle_thread is not added to the runqueue */
     task_init(&idle_task);
     thread_idle =
@@ -156,6 +158,9 @@ struct thread_info *start_kernel(void)
     printk("Created main_thread at <%p> with priority=%d\n", thread_main,
            thread_main->ti_priority);
 
+    /* select a scheduling policy */
+    sched_select(SCHED_CLASS, thread_main);
+
     /* Reclaim the early-stack physical memory.  In the current context, no
      * page allocation after this point are allowed.    */
     printk("Reclaim early stack's physical memory (%d Bytes, order=%d).\n",
diff --git a/kernel/mutex.c b/kernel/mutex.c
index e55ccb7..4549fe8 100644
--- a/kernel/mutex.c
+++ b/kernel/mutex.c
@@ -17,6 +17,7 @@ int sys_pthread_mutex_lock(kernel_mutex_t *mutex)
     CURRENT_THREAD_INFO(curr_thread);
     curr_thread->ti_private = mutex;
     curr_thread->ti_state = THREAD_STATE_BLOCKED;
+    sched_dequeue(curr_thread);
     list_add_tail(&curr_thread->ti_q, &mutex_head);
     sched_elect(SCHED_OPT_NONE);
 
diff --git a/kernel/sched.c b/kernel/sched.c
index 8d92733..426bb50 100644
--- a/kernel/sched.c
+++ b/kernel/sched.c
@@ -6,20 +6,24 @@ extern const struct sched sched_bitmap;
 
 static const struct sched *sched;
 
-int sched_select(int sched_type)
+int sched_select(int sched_type, struct thread_info *thread)
 {
+    printk("Scheduler: ");
     switch (sched_type) {
     case SCHED_CLASS_RR:
+        printk("Round-Robin\n");
         sched = &sched_rr;
         break;
     case SCHED_CLASS_BITMAP:
+        printk("Bitmap\n");
         sched = &sched_bitmap;
         break;
     default:
+        printk("Unknown");
         return -1;
     }
 
-    return sched->init();
+    return sched->init(thread);
 }
 
 int sched_enqueue(struct thread_info *thread)
diff --git a/kernel/sched/bitmap.c b/kernel/sched/bitmap.c
index 314e079..503f63e 100644
--- a/kernel/sched/bitmap.c
+++ b/kernel/sched/bitmap.c
@@ -18,7 +18,7 @@ static struct {
     .expire = &_expire,
 };
 
-static int sched_bitmap_init(void)
+static int sched_bitmap_init(__unused struct thread_info *thread)
 {
     INIT_BITMAP(sched_struct.active);
     INIT_BITMAP(sched_struct.expire);
diff --git a/kernel/sched/rr.c b/kernel/sched/rr.c
index fa0ddbf..ff47197 100644
--- a/kernel/sched/rr.c
+++ b/kernel/sched/rr.c
@@ -7,74 +7,61 @@
 static LIST_HEAD(rr_runq);
 extern struct thread_info *thread_idle;
 
-int sched_rr_init(void)
+int sched_rr_init(struct thread_info *thread)
 {
+    if (thread)
+        sched_rr_enqueue(thread);
     return 0;
 }
 
-static struct thread_info *find_next_thread(struct thread_info *thread)
+static struct thread_info *find_next_thread(void)
 {
-    if (list_is_last(&thread->ti_q, &rr_runq))
-        return list_first_entry(&rr_runq, struct thread_info, ti_q);
-
-    return list_next_entry(thread, ti_q);
+    return list_first_entry(&rr_runq, struct thread_info, ti_q);
 }
 
 int sched_rr_enqueue(struct thread_info *thread)
 {
-    list_add(&thread->ti_q, &rr_runq);
+    if (thread->ti_queued)
+        return -1;
+
+    list_add_tail(&thread->ti_q, &rr_runq);
+    thread->ti_queued = 1;
 
     return 0;
 }
 
 int sched_rr_dequeue(struct thread_info *thread)
 {
-    CURRENT_THREAD_INFO(current);
+    if (!thread->ti_queued)
+        return -1;
 
-    if (current == thread) {
-        struct thread_info *next = thread_idle;
-        if (!list_is_singular(&rr_runq)) {
-            next = find_next_thread(current);
-        }
-        list_del(&thread->ti_q);
-        thread_restore(next);  // FIXME: rename to switch_to_no_save
-    } else {
-        list_del(&thread->ti_q);
-    }
+    list_del(&thread->ti_q);
+    thread->ti_queued = 0;
 
     return 0;
 }
 
-/* This function is used when the runqueue has been modified externally, and it
-   is not possible to fetch the next thread.    */
-static int sched_rr_elect_reset(void)
+void sched_rr_requeue(struct thread_info *thread)
 {
-    CURRENT_THREAD_INFO(current);
-    struct thread_info *next = thread_idle;
-
-    if (!list_empty(&rr_runq))
-        next = list_first_entry(&rr_runq, struct thread_info, ti_q);
-    switch_to(next, current);
-
-    return 0;
+    sched_rr_dequeue(thread);
+    sched_rr_enqueue(thread);
 }
 
-int sched_rr_elect(int switch_type)
+int sched_rr_elect(__unused int switch_type)
 {
     CURRENT_THREAD_INFO(current);
     struct thread_info *next;
 
-    if (switch_type & SCHED_OPT_RESET)
-        return sched_rr_elect_reset();
-
     if (list_empty(&rr_runq)) {
-        // go to thread idle.
-        next = thread_idle;
-    } else {
-        next = find_next_thread(current);
+        switch_to(thread_idle, current);
+        return -1;
     }
 
-    /* keep running the previous thread */
+    /* XXX: Will RR stuck inside the loop? */
+    while ((next = find_next_thread()) == current && !list_is_singular(&rr_runq))
+        sched_rr_requeue(next);
+
+    /* Shortcut if next is current */
     if (next == current)
         return -1;
 
diff --git a/libc/v7m-pthread.S b/libc/v7m-pthread.S
index 5127cfa..be4bfe2 100644
--- a/libc/v7m-pthread.S
+++ b/libc/v7m-pthread.S
@@ -22,10 +22,10 @@ ENTRY(pthread_mutex_lock)
 	bne	0b
 	dmb			@ ARMv7-M ARM, A3.4.6
 	movs	r0, #0		@ it also update EQ flag
-1:	itt ne
-	movne	r1, #SYS_PTHREAD_MUTEX_LOCK
-	svcne	#1
-	bx	lr
+1:	beq 2f
+	mov	r1, #SYS_PTHREAD_MUTEX_LOCK
+	svc	#1
+2:	bx	lr
 ENDPROC(pthread_mutex_lock)
 
 	@ int pthread_mutex_trylock(pthread_mutex_t *mutex)
@@ -38,9 +38,9 @@ ENTRY(pthread_mutex_trylock)
 	teq	r1, #0			@ 'strex' success?
     bne 1f
 	movs	r0, #0		@ it also update EQ flag
-1:  it ne
-    movne	r0, #-1
-	bx	lr
+1:  beq 2f
+    mov	r0, #-1
+2:	bx	lr
 ENDPROC(pthread_mutex_trylock)
 
 	@ int pthread_mutex_unlock(pthread_mutex_t *mutex)
@@ -54,8 +54,8 @@ ENTRY(pthread_mutex_unlock)
 	bne	0b
 	dmb			@ ARMv7-M ARM, A3.4.6
 	movs	r0, #0		@ it also update EQ flag
-1:	itt ne
-	movne	r1, #SYS_PTHREAD_MUTEX_UNLOCK
-	svcne	#1
-	bx	lr
-ENDPROC(pthread_mutex_unlock)
+1:  beq 2f
+	mov	r1, #SYS_PTHREAD_MUTEX_UNLOCK
+	svc	#1
+2:	bx	lr
+ENDPROC(pthread_mutex_lock)
diff --git a/mk/flags.mk b/mk/flags.mk
index 2381754..eb02345 100644
--- a/mk/flags.mk
+++ b/mk/flags.mk
@@ -25,6 +25,16 @@ CFLAGS += \
 CFLAGS += \
     -D CONFIG_KERNEL_STACK_CHECKING
 
+# Scheduler
+ifdef SCHED
+    CFLAGS += -D SCHED_CLASS=SCHED_CLASS_$(SCHED)
+RESCHED:
+	$(VECHO) "  RM\t\tkernel/main.o\n"
+	$(Q)$(RM) kernel/main.o
+
+FORCE: RESCHED
+endif
+
 LDFLAGS += \
     -nostartfiles -specs=nano.specs \
     -Wl,-Map=$(NAME).map -Wl,-Tpiko.lds -Wl,--gc-sections