diff --git a/gdb/gdbserver/ChangeLog b/gdb/gdbserver/ChangeLog
index b5e9c9630ae..867a5a29f2f 100644
--- a/gdb/gdbserver/ChangeLog
+++ b/gdb/gdbserver/ChangeLog
@@ -1,3 +1,12 @@
+2016-07-21  Yao Qi  <yao.qi@linaro.org>
+
+	* gdbthread.h (make_cleanup_restore_current_thread): Declare.
+	* inferiors.c (do_restore_current_thread_cleanup): New function.
+	(make_cleanup_restore_current_thread): Likewise.
+	* linux-low.c (install_software_single_step_breakpoints): Call
+	make_cleanup_restore_current_thread.  Switch current_thread to
+	thread.
+
 2016-07-21  Yao Qi  <yao.qi@linaro.org>
 
 	* mem-break.c (struct reinsert_breakpoint) <ptid>: New field.
diff --git a/gdb/gdbserver/gdbthread.h b/gdb/gdbserver/gdbthread.h
index 4dcb1b7a4ff..d2326fb0590 100644
--- a/gdb/gdbserver/gdbthread.h
+++ b/gdb/gdbserver/gdbthread.h
@@ -87,4 +87,7 @@ struct thread_info *find_any_thread_of_pid (int pid);
 /* Get current thread ID (Linux task ID).  */
 #define current_ptid (current_thread->entry.id)
 
+/* Create a cleanup to restore current_thread.  */
+struct cleanup *make_cleanup_restore_current_thread (void);
+
 #endif /* GDB_THREAD_H */
diff --git a/gdb/gdbserver/inferiors.c b/gdb/gdbserver/inferiors.c
index 1f5149ff486..7888f3c1852 100644
--- a/gdb/gdbserver/inferiors.c
+++ b/gdb/gdbserver/inferiors.c
@@ -411,3 +411,15 @@ current_process (void)
   gdb_assert (current_thread != NULL);
   return get_thread_process (current_thread);
 }
+
+static void
+do_restore_current_thread_cleanup (void *arg)
+{
+  current_thread = (struct thread_info *) arg;
+}
+
+struct cleanup *
+make_cleanup_restore_current_thread (void)
+{
+  return make_cleanup (do_restore_current_thread_cleanup, current_thread);
+}
diff --git a/gdb/gdbserver/linux-low.c b/gdb/gdbserver/linux-low.c
index c0b2ac09cdb..9c675a42611 100644
--- a/gdb/gdbserver/linux-low.c
+++ b/gdb/gdbserver/linux-low.c
@@ -4202,10 +4202,14 @@ install_software_single_step_breakpoints (struct lwp_info *lwp)
 {
   int i;
   CORE_ADDR pc;
-  struct regcache *regcache = get_thread_regcache (current_thread, 1);
+  struct thread_info *thread = get_lwp_thread (lwp);
+  struct regcache *regcache = get_thread_regcache (thread, 1);
   VEC (CORE_ADDR) *next_pcs = NULL;
-  struct cleanup *old_chain = make_cleanup (VEC_cleanup (CORE_ADDR), &next_pcs);
+  struct cleanup *old_chain = make_cleanup_restore_current_thread ();
 
+  make_cleanup (VEC_cleanup (CORE_ADDR), &next_pcs);
+
+  current_thread = thread;
   next_pcs = (*the_low_target.get_next_pcs) (regcache);
 
   for (i = 0; VEC_iterate (CORE_ADDR, next_pcs, i, pc); ++i)