diff --git a/gdb/gdbserver/ChangeLog b/gdb/gdbserver/ChangeLog
index dbe7dc4287..86ca7367d6 100644
--- a/gdb/gdbserver/ChangeLog
+++ b/gdb/gdbserver/ChangeLog
@@ -1,3 +1,16 @@
+2015-11-30  Pedro Alves  <palves@redhat.com>
+
+	* mem-break.c (check_gdb_bp_preconditions): Remove current_thread
+	check.
+	(set_gdb_breakpoint): If prepare_to_access_memory fails, set *ERR
+	to -1.
+	* target.c (struct thread_search): New structure.
+	(thread_search_callback): New function.
+	(prev_general_thread): New global.
+	(prepare_to_access_memory, done_accessing_memory): New functions.
+	* target.h (prepare_to_access_memory, done_accessing_memory):
+	Replace macros with function declarations.
+
 2015-11-30  Pedro Alves  <palves@redhat.com>
 
 	PR 14618
diff --git a/gdb/gdbserver/mem-break.c b/gdb/gdbserver/mem-break.c
index c808a84d75..11c21db00e 100644
--- a/gdb/gdbserver/mem-break.c
+++ b/gdb/gdbserver/mem-break.c
@@ -1023,13 +1023,8 @@ check_gdb_bp_preconditions (char z_type, int *err)
       *err = 1;
       return 0;
     }
-  else if (current_thread == NULL)
-    {
-      *err = -1;
-      return 0;
-    }
-  else
-    return 1;
+
+  return 1;
 }
 
 /* See mem-break.h.  This is a wrapper for set_gdb_breakpoint_1 that
@@ -1047,9 +1042,11 @@ set_gdb_breakpoint (char z_type, CORE_ADDR addr, int kind, int *err)
      access memory.  */
   if (z_type == Z_PACKET_SW_BP)
     {
-      *err = prepare_to_access_memory ();
-      if (*err != 0)
-	return NULL;
+      if (prepare_to_access_memory () != 0)
+	{
+	  *err = -1;
+	  return NULL;
+	}
     }
 
   bp = set_gdb_breakpoint_1 (z_type, addr, kind, err);
diff --git a/gdb/gdbserver/target.c b/gdb/gdbserver/target.c
index 80512d8646..b376ce8f86 100644
--- a/gdb/gdbserver/target.c
+++ b/gdb/gdbserver/target.c
@@ -37,6 +37,113 @@ set_desired_thread (int use_general)
   return (current_thread != NULL);
 }
 
+/* Structure used to look up a thread to use as current when accessing
+   memory.  */
+
+struct thread_search
+{
+  /* The PTID of the current general thread.  This is an input
+     parameter.  */
+  ptid_t current_gen_ptid;
+
+  /* The first thread found.  */
+  struct thread_info *first;
+
+  /* The first stopped thread found.  */
+  struct thread_info *stopped;
+
+  /* The current general thread, if found.  */
+  struct thread_info *current;
+};
+
+/* Callback for find_inferior.  Search for a thread to use as current
+   when accessing memory.  */
+
+static int
+thread_search_callback (struct inferior_list_entry *entry, void *args)
+{
+  struct thread_info *thread = (struct thread_info *) entry;
+  struct thread_search *s = (struct thread_search *) args;
+
+  if (ptid_get_pid (entry->id) == ptid_get_pid (s->current_gen_ptid)
+      && mythread_alive (ptid_of (thread)))
+    {
+      if (s->stopped == NULL && thread_stopped (thread))
+	s->stopped = thread;
+
+      if (s->first == NULL)
+	s->first = thread;
+
+      if (s->current == NULL && ptid_equal (s->current_gen_ptid, entry->id))
+	s->current = thread;
+    }
+
+  return 0;
+}
+
+/* The thread that was current before prepare_to_access_memory was
+   called.  done_accessing_memory uses this to restore the previous
+   selected thread.  */
+static ptid_t prev_general_thread;
+
+/* See target.h.  */
+
+int
+prepare_to_access_memory (void)
+{
+  struct thread_search search;
+  struct thread_info *thread;
+
+  memset (&search, 0, sizeof (search));
+  search.current_gen_ptid = general_thread;
+  prev_general_thread = general_thread;
+
+  if (the_target->prepare_to_access_memory != NULL)
+    {
+      int res;
+
+      res = the_target->prepare_to_access_memory ();
+      if (res != 0)
+	return res;
+    }
+
+  find_inferior (&all_threads, thread_search_callback, &search);
+
+  /* Prefer a stopped thread.  If none is found, try the current
+     thread.  Otherwise, take the first thread in the process.  If
+     none is found, undo the effects of
+     target->prepare_to_access_memory() and return error.  */
+  if (search.stopped != NULL)
+    thread = search.stopped;
+  else if (search.current != NULL)
+    thread = search.current;
+  else if (search.first != NULL)
+    thread = search.first;
+  else
+    {
+      done_accessing_memory ();
+      return 1;
+    }
+
+  current_thread = thread;
+  general_thread = ptid_of (thread);
+
+  return 0;
+}
+
+/* See target.h.  */
+
+void
+done_accessing_memory (void)
+{
+  if (the_target->done_accessing_memory != NULL)
+    the_target->done_accessing_memory ();
+
+  /* Restore the previous selected thread.  */
+  general_thread = prev_general_thread;
+  current_thread = find_thread_ptid (general_thread);
+}
+
 int
 read_inferior_memory (CORE_ADDR memaddr, unsigned char *myaddr, int len)
 {
diff --git a/gdb/gdbserver/target.h b/gdb/gdbserver/target.h
index 8903da59e8..a09ba2fd54 100644
--- a/gdb/gdbserver/target.h
+++ b/gdb/gdbserver/target.h
@@ -651,17 +651,11 @@ int start_non_stop (int nonstop);
 ptid_t mywait (ptid_t ptid, struct target_waitstatus *ourstatus, int options,
 	       int connected_wait);
 
-#define prepare_to_access_memory()		\
-  (the_target->prepare_to_access_memory		\
-   ? (*the_target->prepare_to_access_memory) () \
-   : 0)
+/* Prepare to read or write memory from the inferior process.  See the
+   corresponding target_ops methods for more details.  */
 
-#define done_accessing_memory()				\
-  do							\
-    {							\
-      if (the_target->done_accessing_memory)     	\
-	(*the_target->done_accessing_memory) ();  	\
-    } while (0)
+int prepare_to_access_memory (void);
+void done_accessing_memory (void);
 
 #define target_core_of_thread(ptid)		\
   (the_target->core_of_thread ? (*the_target->core_of_thread) (ptid) \