This change contains changes that were made on a separate copy of this code,...

src/trusted/gdb_rsp/target.cc

Index: src/trusted/gdb_rsp/target.cc
===================================================================
--- src/trusted/gdb_rsp/target.cc	(revision 3954)
+++ src/trusted/gdb_rsp/target.cc	(working copy)
@@ -31,19 +31,28 @@
 
 namespace gdb_rsp {
 
+const int32_t
+Target::kUninitializedThreadId = -1;
 
+const int32_t
+Target::kMaxQCResponseLength = 11;
+
 Target::Target(const Abi* abi)
   : abi_(abi),
-    mutex_(NULL),
+    stateMutex_(NULL),
+    publicMutex_(NULL),
     sig_start_(NULL),
     sig_done_(NULL),
-    send_done_(false),
     ctx_(NULL),
     cur_signal_(-1),
-    sig_thread_(0),
-    run_thread_(-1),
-    reg_thread_(-1) {
-  if (NULL == abi_) abi_ = Abi::Get();
+    signalling_thread_(kUninitializedThreadId),
+    requested_run_thread_id_(kUninitializedThreadId),
+    current_run_thread_id_(kUninitializedThreadId),
+    requested_register_thread_id_(kUninitializedThreadId),
+    current_register_thread_id_(kUninitializedThreadId) {
+  if (NULL == abi_) {
+    abi_ = Abi::Get();
+  }
 }
 
 Target::~Target() {
@@ -51,34 +60,102 @@
 }
 
 bool Target::Init() {
-  string targ_xml = "l<target><architecture>";
-
-  targ_xml += abi_->GetName();
-  targ_xml += "</architecture></target>";
-
-  // Set a more specific result which won't change.
-  properties_["target.xml"] = targ_xml;
-  properties_["Supported"] =
-    "PacketSize=7cf;qXfer:libraries:read+;qXfer:features:read+";
-
-  mutex_ = IMutex::Allocate();
+  stateMutex_ = IMutex::Allocate();
+  publicMutex_ = IMutex::Allocate();
   sig_start_ = IEvent::Allocate();
   sig_done_ = IEvent::Allocate();
   ctx_ = new uint8_t[abi_->GetContextSize()];
 
-  if ((NULL == mutex_) || (NULL == sig_start_) || (NULL == sig_done_)
-      || (NULL == ctx_)) {
+  if ((NULL == stateMutex_) || (NULL == publicMutex_) || (NULL == sig_start_)
+      || (NULL == sig_done_) || (NULL == ctx_)) {
     Destroy();
     return false;
   }
 
+  string targ_xml = "l<target><architecture>";
+  targ_xml += abi_->GetName();
+  targ_xml += "</architecture></target>";
+
+  // Set a more specific result which won't change.
+  SetProperty("Xfer:features:read:target.xml", targ_xml);
+  SetProperty("Supported",
+    "PacketSize=7cf;qXfer:libraries:read+;qXfer:features:read+");
+
   // Allow one exception to happen
   sig_start_->Signal();
   return true;
 }
 
+bool Target::SetProperty(const std::string& name, const std::string& val) {
+  MutexLock lock(publicMutex_);
+  properties_[name] = val;
+  return true;
+}
+
+bool Target::GetProperty(const std::string& name, std::string* val) const {
+  MutexLock lock(publicMutex_);
+  PropertyMap_t::const_iterator itr = properties_.find(name);
+  if (itr == properties_.end()) return false;
+
+  *val = itr->second;
+  return true;
+}
+
+// MatchQuery looks for the longest prefix match, and returns that value.
+// A match key is in one of two forms:
+//  1 - <keyA>[:<keyB>[:<keyC>[...]]]
+//  2 - <Cmd>:<Type>:read/write:<Object>:<start>,<end>
+// We look for the second case, and assume the first if the format does not
+// match.  The second case is special in that it encodes a substring request.
+// This allows transfers of very large values on a transport with a much
+// smaller max packet size.
+size_t Target::MatchQuery(const std::string& match, std::string* val) const {
+  // Starting at longest match.  This is makes more specific cases easier to
+  // address.
+  stringvec parts = StringSplit(match, ":");
+  for (size_t cnt = parts.size(); cnt; cnt--) {
+    // Append the sub-parts and look for a match.
+    std::string cmp;
+    for (size_t i = 0; i < cnt; i++) {
+      if (i) cmp += ":";
+      cmp += parts[i];
+    }
+
+    // If we find this cached value, return it and the match length.
+    // If it's not a read command, we're done.
+    if (GetProperty(cmp, val)) {
+      // If it is a read command, we have to parse it further.
+      // Check for a "<CMD>:<TYPE>:read:<OBJ>:<start>,<len>
+      // cnt is 4 because we expect to have a separate property for each read
+      // OBJ.
+      if ((cnt == 4) && (parts[2] == "read") && (parts.size() == 5)) {
+        // Try to split the start,length field.
+        stringvec args = StringSplit(parts[4], ",");
+        if (args.size() == 2) {
+          std::string tmp = *val;
+          size_t offs = strtoul(args[0].data(), NULL, 16);
+          size_t max  = strtoul(args[1].data(), NULL, 16) + offs;
+          // This can't possibly be right.  Issue submitted.
+          size_t len = tmp.size();
+
+          // This can't possibly be right.  Issue submitted.
+          *val = "";
+          if (max >= len) max = len;
+          while (offs < max) {
+            *val += tmp[offs];
+            offs++;
+          }
+        }
+      }
+      return cnt;
+    }
+  }
+  return 0;
+}
+
 void Target::Destroy() {
-  if (mutex_) IMutex::Free(mutex_);
+  if (stateMutex_) IMutex::Free(stateMutex_);
+  if (publicMutex_) IMutex::Free(publicMutex_);
   if (sig_start_) IEvent::Free(sig_start_);
   if (sig_done_) IEvent::Free(sig_done_);
 
@@ -86,12 +163,13 @@
 }
 
 bool Target::AddTemporaryBreakpoint(uint64_t address) {
+  MutexLock lock(publicMutex_);
   const Abi::BPDef *bp = abi_->GetBreakpointDef();
 
   // If this ABI does not support breakpoints then fail
   if (NULL == bp) return false;
 
-  // If we alreay have a breakpoint here then don't add it
+  // If we already have a breakpoint here then don't add it
   BreakMap_t::iterator itr = breakMap_.find(address);
   if (itr != breakMap_.end()) return false;
 
@@ -113,6 +191,7 @@
 }
 
 bool Target::RemoveTemporaryBreakpoints() {
+  MutexLock lock(publicMutex_);
   const Abi::BPDef *bp = abi_->GetBreakpointDef();
 
   // Iterate through the map, removing breakpoints
@@ -121,7 +200,7 @@
     BreakMap_t::iterator cur = breakMap_.begin();
     uint64_t addr = cur->first;
     uint8_t *data = cur->second;
-  
+
     // Then remove it from the map
     breakMap_.erase(cur);
 
@@ -134,87 +213,112 @@
 }
 
 
-
 void Target::Signal(uint32_t id, int8_t sig, bool wait) {
-  // Wait for this signal's turn in the signal Q.
+  // Wait for an event that indicates that the Run Thread is ready to process
+  // the next signal.
   sig_start_->Wait();
   {
-    // Now lock the target, sleeping all active threads
-    MutexLock lock(mutex_);
+    // Lock the signal state information to prevent a race.
+    MutexLock lock(stateMutex_);
 
-    // Suspend all threads except this one
-    uint32_t curId;
-    bool more = GetFirstThreadId(&curId);
-    while (more) {
-      if (curId != id) {
-        IThread *thread = threads_[curId];
-        thread->Suspend();
-      }
-      more = GetNextThreadId(&curId);
-    }
-
-    // Signal the stub (Run thread) that we are ready to process
-    // a trap, by updating the signal information and releasing
-    // the lock.
-    reg_thread_ = id;
-    run_thread_ = id;
+    // Signal the stub that we are ready to process a trap
+    // by locking the signal information, and updating it.
+    signalling_thread_ = id;
     cur_signal_ = sig;
   }
 
-  // Wait for permission to continue
+  // Wait for the signal to be fully processed before exiting.
   if (wait) sig_done_->Wait();
 }
 
+void Target::Update() {
+  // Next, update the current thread info.
+  char tmp[kMaxQCResponseLength + 1];
+  snprintf(tmp, kMaxQCResponseLength, "QC%x", signalling_thread_);
+  tmp[kMaxQCResponseLength] = 0;
+  SetProperty("C", tmp);
+}
+
+void Target::Pause() {
+  // Put all active threads to sleep.
+  uint32_t curId;
+  bool more = GetFirstThreadId(&curId);
+  while (more) {
+    IThread *thread = GetThread(curId);
+    if (thread->GetState() == IThread::RUNNING) thread->Suspend();
+    more = GetNextThreadId(&curId);
+  }
+}
+
+void Target::Resume() {
+  // Now that we are done, we want to continue in the "correct order".
+  // This means letting the active thread go first, in case we are single
+  // stepping and want to catch it again.  This is a desired behavior but
+  // it is not guaranteed since another thread may already be in an
+  // exception state and next in line to notify the target.
+
+  // Wake up the run thread if it needs it, so it can go first
+  IThread* thread = GetThread(UpdateThreadId(requested_run_thread_id_,
+                                             &current_run_thread_id_));
+  if (thread->GetState() == IThread::SUSPENDED) thread->Resume();
+
+  // If we took an exception, let the handler resume and allow
+  // the next exception to trigger (although it will still
+  // block until we release the mutex).
+  if (cur_signal_) {
+    sig_done_->Signal();
+    sig_start_->Signal();
+  }
+
+  // Now wake up everyone else that needs it
+  uint32_t curId;
+  bool more = GetFirstThreadId(&curId);
+  while (more) {
+    IThread* thread = GetThread(curId);
+
+    if (thread->GetState() == IThread::SUSPENDED) thread->Resume();
+    more = GetNextThreadId(&curId);
+  }
+}
+
 void Target::Run(Session *ses) {
   bool first = true;
+
   do {
-    // Give everyone else a chance to use the lock
+    // Prevent tight loop so this thread doesn't churn up CPU.
     IPlatform::Relinquish(100);
 
     // Lock to prevent anyone else from modifying threads
     // or updating the signal information.
-    MutexLock lock(mutex_);
+    MutexLock lock(stateMutex_);
     Packet recv, reply;
 
-    uint32_t id = 0;
-
     // If no signal is waiting for this iteration...
     if (-1 == cur_signal_) {
-      // but the debugger is talking to us then force a break
+      // but the debugger is talking to us
       if (ses->DataAvailable()) {
-        // set signal to 0 to signify paused
+        // Then simulate a signal, by setting the state data
+        // Set signal to 0 to signify paused
         cur_signal_ = 0;
 
-        // put all the threads to sleep.
-        uint32_t curId;
-        bool more = GetFirstThreadId(&curId);
-        while (more) {
-          if (curId != id) {
-            IThread *thread = threads_[curId];
-            thread->Suspend();
-          }
-          more = GetNextThreadId(&curId);
-        }
+        // Set the "signaling" thread to the preferred thread
+        signalling_thread_ = UpdateThreadId(requested_run_thread_id_,
+                                            &current_run_thread_id_);
       } else {
-        // otherwise, nothing to do so try again.
+        // otherwise, we are not signaled, and we are not chatting
+        // release the lock, and try again.
         continue;
       }
-    } else {
-      // otherwise there really is an exception so get the id of the thread
-      id = GetRegThreadId();
     }
 
-    // If we got this far, then there is some kind of signal.
-    // So first, remove the breakpoints
-    RemoveTemporaryBreakpoints();
+    // We have a signal, so pause everyone
+    Pause();
 
-    // Next update the current thread info
-    char tmp[16];
-    snprintf(tmp, sizeof(tmp), "QC%x", id);
-    properties_["C"] = tmp;
+    // Update our local state information
+    Update();
 
     if (first) {
-      // First time on a connection, we don't sent the signal
+      // First time on a connection, we don't send the signal
       first = false;
     } else {
       // All other times, send the signal that triggered us
@@ -234,59 +338,27 @@
           // If this is a continue command, break out of this loop
           break;
         } else {
-          // Othwerise send the reponse
+          // Otherwise send the reponse
           ses->SendPacket(&reply);
         }
       }
     } while (ses->Connected());
 
+    // Start everyone again.
+    Resume();
 
-    // Now that we are done, we want to continue in the "correct order".
-    // This means letting the active thread go first, in case we are single
-    // stepping and want to catch it again.  This is a desired behavior but
-    // it is not guaranteed since another thread may already be in an
-    // exception state and next in line to notify the target.
-
-    // If the run thread is not the exception thread, wake it up now.
-    uint32_t run_thread = GetRunThreadId();
-    if (run_thread != id
-        && run_thread != static_cast<uint32_t>(-1)) {
-      IThread* thread = threads_[run_thread];
-      thread->Resume();
-    }
-
-    // Next, wake up the exception thread, if there is one and it needs
-    // to wake up.
-    if (id && send_done_) sig_done_->Signal();
-
-    // Now wake up everyone else
-    uint32_t curId;
-    bool more = GetFirstThreadId(&curId);
-    while (more) {
-      if ((curId != id) && (curId != GetRunThreadId())) {
-        IThread *thread = threads_[curId];
-        thread->Resume();
-      }
-      more = GetNextThreadId(&curId);
-    }
-
-    // Reset the signal value
+    // Reset our state before we return, clearing the signal.  However
+    // we do not reset run_thread or reg_thread to preserve the last context.
     cur_signal_ = -1;
+    signalling_thread_ = kUninitializedThreadId;
 
-    // If we took an exception, let the handler resume and allow
-    // the next exception to come in.
-    if (cur_signal_) {
-      sig_done_->Signal();
-      sig_start_->Signal();
-    }
-
+    // At this point, we destroy the MutexLock, allowing anyone else to
+    // and/remove threads or register a new signal.
     // Continue running until the connection is lost.
   } while (ses->Connected());
 }
 
 
-
-
 bool Target::GetFirstThreadId(uint32_t *id) {
   threadItr_ = threads_.begin();
   return GetNextThreadId(id);
@@ -302,7 +374,6 @@
 }
 
 
-
 bool Target::ProcessPacket(Packet* pktIn, Packet* pktOut) {
   char cmd;
   int32_t seq = -1;
@@ -319,9 +390,15 @@
   pktIn->GetRawChar(&cmd);
 
   switch (cmd) {
+    // In the case of a force break, drop through
+    // and print the signal.  We have already suspended
+    // the threads at this point.
+    case 0x03:
+
     // IN : $?
     // OUT: $Sxx
     case '?':
+      // This case asks why the target has halted.
       pktOut->AddRawChar('S');
       pktOut->AddWord8(cur_signal_);
       break;
@@ -329,13 +406,17 @@
     // IN : $d
     // OUT: -NONE-
     case 'd':
+      // This packet could be asking the debugger to detach but
+      // we should never receive it.
       Detach();
-      break;
+      return true;
 
     // IN : $g
     // OUT: $xx...xx
     case 'g': {
-      uint32_t id = GetRegThreadId();
+      // This packet is requesting the value of a general register.
+      uint32_t id = UpdateThreadId(requested_register_thread_id_,
+                                   &current_register_thread_id_);
       if (0 == id) {
         err = BAD_ARGS;
         break;
@@ -360,7 +441,10 @@
     // IN : $Gxx..xx
     // OUT: $OK
     case 'G': {
-      uint32_t id = GetRegThreadId();
+      // This packet is requesting that the value of a general register be
+      // changed to something else.
+      uint32_t id = UpdateThreadId(requested_register_thread_id_,
+                                   &current_register_thread_id_);
       if (0 == id) {
         err = BAD_ARGS;
         break;
@@ -384,105 +468,124 @@
     // IN : $H(c/g)(-1,0,xxxx)
     // OUT: $OK
     case 'H': {
-        char type;
-        uint64_t id;
+      // This packet is requesting that the thread be set for subsequent
+      // operations.
+      char type;
+      uint64_t id;
 
-        if (!pktIn->GetRawChar(&type)) {
-          err = BAD_FORMAT;
-          break;
-        }
-        if (!pktIn->GetNumberSep(&id, 0)) {
-          err = BAD_FORMAT;
-          break;
-        }
+      if (!pktIn->GetRawChar(&type)) {
+        err = BAD_FORMAT;
+        break;
+      }
+      if (!pktIn->GetNumberSep(&id, 0)) {
+        err = BAD_FORMAT;
+        break;
+      }
 
-        if (threads_.begin() == threads_.end()) {
-            err = BAD_ARGS;
-            break;
-        }
+      if (threads_.begin() == threads_.end()) {
+        err = BAD_ARGS;
+        break;
+      }
 
-        // If we are using "any" get the first thread
-        if (id == static_cast<uint64_t>(-1)) id = threads_.begin()->first;
+      // If we are using "any" get the first thread
+      if (id == static_cast<uint64_t>(-1)) id = threads_.begin()->first;
 
-        // Verify that we have the thread
-        if (threads_.find(static_cast<uint32_t>(id)) == threads_.end()) {
-          err = BAD_ARGS;
-          break;
-        }
-
-        pktOut->AddString("OK");
-        switch (type) {
-          case 'g':
-            reg_thread_ = static_cast<uint32_t>(id);
-            break;
-
-          case 'c':
-            run_thread_ = static_cast<uint32_t>(id);
-            break;
-
-          default:
-            err = BAD_ARGS;
-            break;
-        }
+      // Verify that we have the thread
+      if (threads_.find(static_cast<uint32_t>(id)) == threads_.end()) {
+        err = BAD_ARGS;
         break;
       }
 
-    // IN : $maaaa,llll
-    // OUT: $xx..xx
-    case 'm': {
-        uint64_t addr;
-        uint64_t wlen;
-        uint32_t len;
-        if (!pktIn->GetNumberSep(&addr, 0)) {
-          err = BAD_FORMAT;
+      pktOut->AddString("OK");
+      switch (type) {
+        case 'g':
+          requested_register_thread_id_ = static_cast<uint32_t>(id);
           break;
-        }
 
-        if (!pktIn->GetNumberSep(&wlen, 0)) {
-          err = BAD_FORMAT;
+        case 'c':
+          requested_run_thread_id_ = static_cast<uint32_t>(id);
           break;
+
+        default:
+          err = BAD_ARGS;
+          break;
         }
+      break;
+    }
 
-        len = static_cast<uint32_t>(wlen);
-        uint8_t *block = new uint8_t[len];
-        if (!port::IPlatform::GetMemory(addr, len, block)) err = FAILED;
+    case 'k':
+      port::IPlatform::LogError("Process killed by debugger.");
+      break;
 
-        pktOut->AddBlock(block, len);
+    // IN : $maaaa,llll
+    // OUT: $xx..xx
+    case 'm': {
+      // This packet is requesting a memory read of size llll, starting
+      // at address aaaa.
+      uint64_t addr;
+      uint64_t wlen;
+      uint32_t len;
+      if (!pktIn->GetNumberSep(&addr, 0)) {
+        err = BAD_FORMAT;
         break;
       }
 
+      if (!pktIn->GetNumberSep(&wlen, 0)) {
+        err = BAD_FORMAT;
+        break;
+      }
+
+      len = static_cast<uint32_t>(wlen);
+      uint8_t *block = new uint8_t[len];
+      if (!port::IPlatform::GetMemory(addr, len, block)) err = FAILED;
+
+      pktOut->AddBlock(block, len);
+      break;
+    }
+
     // IN : $Maaaa,llll:xx..xx
     // OUT: $OK
     case 'M':  {
-        uint64_t addr;
-        uint64_t wlen;
-        uint32_t len;
+      // This packet is requesting one or more bytes be overwritten,
+      // starting at a specific address.
+      uint64_t addr;
+      uint64_t wlen;
+      uint32_t len;
 
-        if (!pktIn->GetNumberSep(&addr, 0)) {
-          err = BAD_FORMAT;
-          break;
-        }
-        if (!pktIn->GetNumberSep(&wlen, 0)) {
-          err = BAD_FORMAT;
-          break;
-        }
+      if (!pktIn->GetNumberSep(&addr, 0)) {
+        err = BAD_FORMAT;
+        break;
+      }
+      if (!pktIn->GetNumberSep(&wlen, 0)) {
+        err = BAD_FORMAT;
+        break;
+      }
 
-        len = static_cast<uint32_t>(wlen);
-        uint8_t *block = new uint8_t[len];
-        pktIn->GetBlock(block, len);
+      len = static_cast<uint32_t>(wlen);
+      uint8_t *block = new uint8_t[len];
+      pktIn->GetBlock(block, len);
 
-        if (!port::IPlatform::SetMemory(addr, len, block)) err = FAILED;
+      if (!port::IPlatform::SetMemory(addr, len, block)) err = FAILED;
 
-        pktOut->AddString("OK");
-        break;
-      }
+      pktOut->AddString("OK");
+      delete[] block;
+      break;
+    }
 
     case 'q': {
+      // This packet is carrying out a general query, possibly for supported
+      // commands and the like.
       string tmp;
       const char *str = &pktIn->GetPayload()[1];
-      stringvec toks = StringSplit(str, ":;");
-      PropertyMap_t::const_iterator itr = properties_.find(toks[0]);
+      stringvec toks = StringSplit(str, ";");
+      std::string strout;
 
+      // Check if we can find this query in the local cache
+      if (MatchQuery(toks[0], &strout)) {
+        pktOut->AddString(strout.data());
+        break;
+      }
+
       // If this is a thread query
       if (!strcmp(str, "fThreadInfo") || !strcmp(str, "sThreadInfo")) {
         uint32_t curr;
@@ -502,34 +605,13 @@
         break;
       }
 
-      // Check for architecture query
-      tmp = "Xfer:features:read:target.xml";
-      if (!strncmp(str, tmp.data(), tmp.length())) {
-        stringvec args = StringSplit(&str[tmp.length()+1], ",");
-        if (args.size() != 2) break;
-
-        const char *out = properties_["target.xml"].data();
-        int offs = strtol(args[0].data(), NULL, 16);
-        int max  = strtol(args[1].data(), NULL, 16) + offs;
-        int len  = static_cast<int>(strlen(out));
-
-        if (max >= len) max = len;
-
-        while (offs < max) {
-          pktOut->AddRawChar(out[offs]);
-          offs++;
-        }
-        break;
-      }
-
-      // Check the property cache
-      if (itr != properties_.end()) {
-        pktOut->AddString(itr->second.data());
-      }
+      // Didn't find anything
       break;
     }
 
     case 'T': {
+      // This packet constitutes a check, whether a given thread is alive or
+      // dead.
       uint64_t id;
       if (!pktIn->GetNumberSep(&id, 0)) {
         err = BAD_FORMAT;
@@ -546,22 +628,19 @@
     }
 
     case 's':  {
-      IThread *thread = GetThread(GetRunThreadId());
+      // This packet is asking for a thread to step a single instruction
+      IThread *thread = GetThread(UpdateThreadId(requested_run_thread_id_,
+                                                 &current_run_thread_id_));
       if (thread) thread->SetStep(true);
       return true;
     }
 
-    case 'c':
-      return true;
-
-    default: {
-      // If the command is not recognzied, ignore it by sending an
-      // empty reply.
-      string str;
-      pktIn->GetString(&str);
-      port::IPlatform::LogError("Unknown command: %s", str.data());
-      return false;
-    }
+    default:
+      // If the command is not recognized, ignore it by sending an empty reply.
+      // Don't use LogError here or the debug stub will crash instead of
+      // negotiating a protocol with the debugger.
+      port::IPlatform::LogInfo("Unknown command: %s", pktIn->GetPayload());
+      break;
   }
 
   // If there is an error, return the error code instead of a payload
@@ -575,19 +654,18 @@
 
 
 void Target::TrackThread(IThread* thread) {
+  MutexLock lock(stateMutex_);
+
   uint32_t id = thread->GetId();
-  mutex_->Lock();
   threads_[id] = thread;
-  mutex_->Unlock();
 }
 
 void Target::IgnoreThread(IThread* thread) {
+  MutexLock lock(stateMutex_);
+
   uint32_t id = thread->GetId();
-  mutex_->Lock();
   ThreadMap_t::iterator itr = threads_.find(id);
-
   if (itr != threads_.end()) threads_.erase(itr);
-  mutex_->Lock();
 }
 
 
@@ -596,31 +674,44 @@
 }
 
 
-uint32_t Target::GetRegThreadId() const {
-  ThreadMap_t::const_iterator itr;
+int32_t Target::UpdateThreadId(int32_t requested_thread_id,
+                               int32_t *current_thread_id) {
+  IThread* thread;
 
-  switch (reg_thread_) {
-    // If we wany "any" then try the signal'd thread first
+  switch (requested_thread_id) {
     case 0:
     case 0xFFFFFFFF:
-      itr = threads_.begin();
+      // If we want "any" then try the last result first
+      if (*current_thread_id != kUninitializedThreadId) {
+        thread = GetThread(*current_thread_id);
+      }
+
+      // If not there, try the sig_thread next
+      if (NULL == thread) thread = GetThread(signalling_thread_);
+
+      // If still not there, get the first thread
+      if (NULL == thread) thread = threads_.begin()->second;
       break;
 
     default:
-      itr = threads_.find(reg_thread_);
+      // Otherwise, get the specificly requested one
+      thread = GetThread(requested_thread_id);
       break;
   }
 
-  if (itr == threads_.end()) return 0;
+  // Update last result.
+  if (NULL == thread) {
+    *current_thread_id = kUninitializedThreadId;
+  } else {
+    *current_thread_id = thread->GetId();
+  }
 
-  return itr->first;
+  // Return the result
+  return *current_thread_id;
 }
 
-uint32_t Target::GetRunThreadId() const {
-  return run_thread_;
-}
 
-IThread* Target::GetThread(uint32_t id) {
+IThread* Target::GetThread(uint32_t id) const {
   ThreadMap_t::const_iterator itr;
   itr = threads_.find(id);
   if (itr != threads_.end()) return itr->second;
@@ -630,7 +721,3 @@
 
 
 }  // namespace gdb_rsp
-
-
-
-