diff --git a/ChangeLog b/ChangeLog
index 8b7b762..d062a68 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,11 @@
+2016-04-07  Niibe Yutaka  <gniibe@fsij.org>
+
+	* entry.c (vector_table): Since IDLE thread runs with PSP now, use
+	different value for MSP.
+
+	* chopstx.c (chx_sched): Push LR value to PC slot on stack, so
+	that it returns directly to caller.
+
 2016-04-06  Niibe Yutaka  <gniibe@fsij.org>
 
 	* chopstx.c (struct chx_pq): New struct for priority queue.
diff --git a/chopstx.c b/chopstx.c
index f9e6efc..c0c182a 100644
--- a/chopstx.c
+++ b/chopstx.c
@@ -720,26 +720,28 @@ chx_sched (uint32_t yield)
 
   /* Build stack data as if it were an exception entry.  */
   /*
-   * r0:  YIELD                 scratch
-   * r1:  .L_SCHED_RETURN       scratch
+   * r0:  0                     scratch
+   * r1:  0                     scratch
    * r2:  0                     scratch
-   * r3:  YIELD                 scratch
+   * r3:  0                     scratch
    * r12: 0                     scratch
-   * lr
-   * pc: .L_SCHED_RETURN+1
+   * lr   as-is
+   * pc:  return address (= lr)
    * psr: INITIAL_XPSR          scratch
    */
-  asm ("ldr	r1, =.L_SCHED_RETURN\n\t"
-       "add	r1, #1\n\t"	/* Thumb-mode */
-       "mov	r2, #128\n\t"
-       "lsl	r2, #17\n\t"
-       "push	{r1, r2}\n\t"
-       "mov	r2, #0\n\t"
-       "push	{r0, r2, lr}\n\t"
-       "push	{%0, r1, r2}"
+  asm ("mov	r1, lr\n\t"
+       "mov	r2, r1\n\t"
+       "mov	r3, #128\n\t"
+       "lsl	r3, #17\n\t"
+       "push	{r1, r2, r3}\n\t"
+       "mov	r1, #0\n\t"
+       "mov	r2, r1\n\t"
+       "mov	r3, r1\n\t"
+       "push	{r1, r2, r3}\n\t"
+       "push	{r1, r2}"
        : /* no output*/
        : "r" (yield)
-       : "r1", "r2", "memory");
+       : "r1", "r2", "r3", "memory");
 
   /* Save registers onto CHX_THREAD struct.  */
   asm ("mov	r1, r0\n\t"
@@ -863,9 +865,7 @@ chx_sched (uint32_t yield)
 		"mov	r12, r0\n\t"
 		"pop	{r0, r1, r2, r3}\n\t"
 		"add	sp, #12\n\t"
-		"pop	{pc}\n"
-	".L_SCHED_RETURN:\n\t"
-		"bx	lr"
+		"pop	{pc}"
 		: /* no output */
 		: "r" (tp)
 		: "memory");
diff --git a/entry.c b/entry.c
index 587374d..b002f30 100644
--- a/entry.c
+++ b/entry.c
@@ -1,7 +1,8 @@
 /*
  * entry.c - Entry routine when reset and interrupt vectors.
  *
- * Copyright (C) 2013, 2014, 2015  Flying Stone Technology
+ * Copyright (C) 2013, 2014, 2015, 2016
+ *               Flying Stone Technology
  * Author: NIIBE Yutaka <gniibe@fsij.org>
  *
  * This file is a part of Chopstx, a thread library for embedded.
@@ -93,7 +94,8 @@ uint32_t vectors_in_ram[48];
  * This routine only changes PSP and not MSP.
  */
 STATIC_ENTRY __attribute__ ((naked,section(".text.startup.0")))
-void entry (void)
+void
+entry (void)
 {
   asm volatile ("bl	clock_init\n\t"
 		/* Clear BSS section.  */
@@ -157,7 +159,7 @@ void entry (void)
 typedef void (*handler)(void);
 
 handler vector_table[] __attribute__ ((section(".startup.vectors"))) = {
-  (handler)&__main_stack_end__,
+  (handler)(&__main_stack_end__ - 32),
   entry,
   nmi,		/* nmi */
   hard_fault,		/* hard fault */