Index: cpukit/libdebugger/rtems-debugger-server.c
===================================================================
--- cpukit/libdebugger/rtems-debugger-server.c	(revision 7f95cc07079901693338ef565bb318f4d5ed5a7c)
+++ cpukit/libdebugger/rtems-debugger-server.c	(revision b2353ed92435c3f9301b795bf20bce14f0ddb01b)
@@ -1,4 +1,5 @@
 /*
- * Copyright (c) 2016 Chris Johns <chrisj@rtems.org>.  All rights reserved.
+ * Copyright (c) 2016-2017 Chris Johns <chrisj@rtems.org>.
+ * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -24,7 +25,13 @@
  */
 
+#define RTEMS_DEBUGGER_VERBOSE_LOCK 0
+#define RTEMS_DEBUGGER_PRINT_PRINTK 1
+
 #include <errno.h>
 #include <stdlib.h>
 #include <unistd.h>
+
+#include <rtems/bspIo.h>
+#include <rtems/score/smp.h>
 
 #include <rtems/rtems-debugger.h>
@@ -51,5 +58,5 @@
 typedef struct rtems_debugger_packet
 {
-  const char const*      label;
+  const char* const      label;
   rtems_debugger_command command;
 } rtems_debugger_packet;
@@ -58,6 +65,6 @@
  * Common error strings.
  */
-static const char const* r_OK = "OK";
-static const char const* r_E01 = "E01";
+static const char* const r_OK = "OK";
+static const char* const r_E01 = "E01";
 
 /*
@@ -74,11 +81,57 @@
 rtems_debugger_server* rtems_debugger;
 
+/**
+ * Print lock ot make the prints sequential. This is to debug the debugger in
+ * SMP.
+ */
+#if RTEMS_DEBUGGER_PRINT_PRINTK
+RTEMS_INTERRUPT_LOCK_DEFINE(static, printk_lock, "printk_lock")
+#endif
+
+void
+rtems_debugger_printk_lock(rtems_interrupt_lock_context* lock_context)
+{
+  rtems_interrupt_lock_acquire(&printk_lock, lock_context);
+}
+
+void
+rtems_debugger_printk_unlock(rtems_interrupt_lock_context* lock_context)
+{
+  rtems_interrupt_lock_release(&printk_lock, lock_context);
+}
+
 int
-rtems_debugger_printf(const char* format, ...)
+rtems_debugger_clean_printf(const char* format, ...)
 {
   int     len;
   va_list ap;
   va_start(ap, format);
-  len = rtems_vprintf(&rtems_debugger->printer, format, ap);
+  if (RTEMS_DEBUGGER_PRINT_PRINTK) {
+    rtems_interrupt_lock_context lock_context;
+    rtems_debugger_printk_lock(&lock_context);
+    len = vprintk(format, ap);
+    rtems_debugger_printk_unlock(&lock_context);
+  }
+  else
+    len = rtems_vprintf(&rtems_debugger->printer, format, ap);
+  va_end(ap);
+  return len;
+}
+
+int
+rtems_debugger_printf(const char* format, ...)
+{
+  int     len;
+  va_list ap;
+  va_start(ap, format);
+  if (RTEMS_DEBUGGER_PRINT_PRINTK) {
+    rtems_interrupt_lock_context lock_context;
+    rtems_debugger_printk_lock(&lock_context);
+    printk("[CPU:%d] ", (int) _SMP_Get_current_processor ());
+    len = vprintk(format, ap);
+    rtems_debugger_printk_unlock(&lock_context);
+  }
+  else
+    len = rtems_vprintf(&rtems_debugger->printer, format, ap);
   va_end(ap);
   return len;
@@ -159,35 +212,17 @@
 check_pid(DB_UINT pid)
 {
-  return pid == 0|| rtems_debugger->pid == (pid_t) pid;
-}
-
-int
+  return pid == 0 || rtems_debugger->pid == (pid_t) pid;
+}
+
+void
 rtems_debugger_lock(void)
 {
-  if (rtems_debugger->lock != 0) {
-    rtems_status_code sc;
-    sc = rtems_semaphore_obtain(rtems_debugger->lock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
-    if (sc != RTEMS_SUCCESSFUL) {
-      rtems_debugger_printf("error: rtems-db: lock: %s\n",
-                            rtems_status_text(sc));
-      return -1;
-    }
-  }
-  return 0;
-}
-
-int
+  _Mutex_recursive_Acquire(&rtems_debugger->lock);
+}
+
+void
 rtems_debugger_unlock(void)
 {
-  if (rtems_debugger->lock != 0) {
-    rtems_status_code sc;
-    sc = rtems_semaphore_release(rtems_debugger->lock);
-    if (sc != RTEMS_SUCCESSFUL) {
-      rtems_debugger_printf("error: rtems-db: unlock: %s\n",
-                            rtems_status_text(sc));
-      return -1;
-    }
-  }
-  return 0;
+  _Mutex_recursive_Release(&rtems_debugger->lock);
 }
 
@@ -195,18 +230,5 @@
 rtems_debugger_lock_create(void)
 {
-  #define LOCK_ATTRIBUTES \
-    RTEMS_PRIORITY | RTEMS_INHERIT_PRIORITY | RTEMS_BINARY_SEMAPHORE
-  rtems_status_code sc;
-  sc = rtems_semaphore_create(rtems_build_name('G', 'D', 'B', 's'),
-                              1,
-                              LOCK_ATTRIBUTES,
-                              0,
-                              &rtems_debugger->lock);
-  if (sc != RTEMS_SUCCESSFUL) {
-    rtems_debugger_printf("error: rtems-db: sema create: %s\n",
-                          rtems_status_text(sc));
-    errno = EIO;
-    return -1;
-  }
+  _Mutex_recursive_Initialize_named(&rtems_debugger->lock, "DBlock");
   return 0;
 }
@@ -215,16 +237,4 @@
 rtems_debugger_lock_destroy(void)
 {
-  rtems_debugger_lock();
-  if (rtems_debugger->lock != 0) {
-    rtems_status_code sc;
-    rtems_semaphore_release(rtems_debugger->lock);
-    sc = rtems_semaphore_delete(rtems_debugger->lock);
-    rtems_debugger->lock = 0;
-    if (sc != RTEMS_SUCCESSFUL) {
-      rtems_debugger_printf("error: rtems-db: sema delete: %s\n",
-                            rtems_status_text(sc));
-      return -1;
-    }
-  }
   return 0;
 }
@@ -334,50 +344,17 @@
 rtems_debugger_server_events_running(void)
 {
-  bool running;
-  rtems_debugger_lock();
-  running = rtems_debugger->events_running;
-  rtems_debugger_unlock();
-  return running;
-}
-
-int
-rtems_debugger_server_events_wake(void)
-{
-  rtems_status_code sc;
-  int               r = 0;
-  sc = rtems_event_send(rtems_debugger->events_task, RTEMS_EVENT_1);
-  if (sc != RTEMS_SUCCESSFUL) {
-    rtems_debugger_printf("error: rtems-db: event send: %s\n",
-                          rtems_status_text(sc));
-    errno = EIO;
-    r = -1;
-  }
-  return r;
-}
-
-static int
+  return rtems_debugger->events_running;
+}
+
+void
+rtems_debugger_server_events_signal(void)
+{
+  _Condition_Signal(&rtems_debugger->server_cond);
+}
+
+static void
 rtems_debugger_server_events_wait(void)
 {
-  rtems_event_set   out = 0;
-  rtems_status_code sc;
-  int               r = 0;
-  rtems_debugger_unlock();
-  while (true) {
-    sc = rtems_event_receive(RTEMS_EVENT_1,
-                             RTEMS_EVENT_ALL | RTEMS_WAIT,
-                             RTEMS_NO_TIMEOUT,
-                             &out);
-    if (sc != RTEMS_SUCCESSFUL) {
-      rtems_debugger_printf("error: rtems-db: event receive: %s\n",
-                            rtems_status_text(sc));
-      errno = EIO;
-      r = -1;
-      break;
-    }
-    if (out == RTEMS_EVENT_1)
-      break;
-  }
-  rtems_debugger_lock();
-  return r;
+  _Condition_Wait_recursive(&rtems_debugger->server_cond, &rtems_debugger->lock);
 }
 
@@ -433,6 +410,6 @@
     rtems_debugger_printf("rtems-db: put:%4zu: ", rtems_debugger->output_level);
     while (i < rtems_debugger->output_level)
-      rtems_debugger_printf("%c", (char) rtems_debugger->output[i++]);
-    rtems_debugger_printf("\n");
+      rtems_debugger_clean_printf("%c", (char) rtems_debugger->output[i++]);
+    rtems_debugger_clean_printf("\n");
   }
 
@@ -534,5 +511,5 @@
 
       if (rtems_debugger->remote_debug)
-        rtems_debugger_printf("%c", c);
+        rtems_debugger_clean_printf("%c", c);
 
       switch (state) {
@@ -541,5 +518,5 @@
         case '+':
           if (rtems_debugger->remote_debug) {
-            rtems_debugger_printf(" [[ACK%s]]\n",
+            rtems_debugger_clean_printf(" [[ACK%s]]\n",
                                   rtems_debugger->ack_pending ? "" : "?");
             remote_debug_header = true;
@@ -549,5 +526,5 @@
         case '-':
           if (rtems_debugger->remote_debug) {
-            rtems_debugger_printf(" [[NACK]]\n");
+            rtems_debugger_clean_printf(" [[NACK]]\n");
             remote_debug_header = true;
           }
@@ -562,5 +539,5 @@
           in = 0;
           if (junk && rtems_debugger->remote_debug) {
-            rtems_debugger_printf("\b [[junk dropped]]\nrtems-db: get:   : $");
+            rtems_debugger_clean_printf("\b [[junk dropped]]\nrtems-db: get:   : $");
             remote_debug_header = false;
           }
@@ -568,5 +545,5 @@
         case '\x3':
           if (rtems_debugger->remote_debug)
-            rtems_debugger_printf("^C [[BREAK]]\n");
+            rtems_debugger_clean_printf("^C [[BREAK]]\n");
           rtems_debugger->ack_pending = false;
           rtems_debugger->input[0] =  '^';
@@ -587,5 +564,5 @@
           in = 0;
           if (rtems_debugger->remote_debug) {
-            rtems_debugger_printf("\n");
+            rtems_debugger_clean_printf("\n");
             remote_debug_header = true;
           }
@@ -614,10 +591,10 @@
           state = 'F';
           if (rtems_debugger->remote_debug)
-            rtems_debugger_printf("\n");
+            rtems_debugger_clean_printf("\n");
           rtems_debugger_remote_send_ack();
         }
         else {
           if (rtems_debugger->remote_debug) {
-            rtems_debugger_printf(" [[invalid checksum]]\n");
+            rtems_debugger_clean_printf(" [[invalid checksum]]\n");
             remote_debug_header = true;
             rtems_debugger_remote_send_nack();
@@ -628,5 +605,5 @@
       case 'F':
           if (rtems_debugger->remote_debug)
-            rtems_debugger_printf(" [[extra data: 0x%02x]]", (int) c);
+            rtems_debugger_clean_printf(" [[extra data: 0x%02x]]", (int) c);
         break;
       default:
@@ -811,4 +788,7 @@
                 (const char*) &buffer[0],
                 strlen(p->label)) == 0) {
+      if (rtems_debugger_server_flag(RTEMS_DEBUGGER_FLAG_VERBOSE_CMDS))
+        rtems_debugger_printf("rtems-db: cmd: %s [%d] '%s'\n",
+                              p->label, size, (const char*) buffer);
       r = p->command(buffer, size);
       break;
@@ -905,5 +885,5 @@
     bool    extended;
     extended = thread_id_decode(comma + 1, &pid, &tid);
-    if (!extended || (extended && check_pid(pid))) {
+    if (extended || check_pid(pid)) {
       int r;
       r = rtems_debugger_thread_find_index(tid);
@@ -955,5 +935,5 @@
   if (p != NULL)
     ++p;
-  while (p != NULL && p != '\0') {
+  while (p != NULL && *p != '\0') {
     bool  echo = false;
     char* sc;
@@ -1023,6 +1003,6 @@
 remote_gq_attached(uint8_t* buffer, int size)
 {
-  const char const* response = "1";
-  const char*       colon = strchr((const char*) buffer, ':');
+  const char* response = "1";
+  const char* colon = strchr((const char*) buffer, ':');
   if (colon != NULL) {
     DB_UINT pid = hex_decode_uint((const uint8_t*) colon + 1);
@@ -1062,5 +1042,5 @@
 remote_gs_non_stop(uint8_t* buffer, int size)
 {
-  const char const* response = r_E01;
+  const char* response = r_E01;
   char*       p = strchr((char*) buffer, ':');
   if (p != NULL) {
@@ -1174,4 +1154,5 @@
               resume = true;
             break;
+          case 'S':
           case 's':
             if (thread != NULL) {
@@ -1203,4 +1184,5 @@
             break;
           default:
+            rtems_debugger_printf("rtems-db: vCont: unkown action: %c\n", action);
             ok = false;
             break;
@@ -1211,4 +1193,5 @@
       }
       else {
+        rtems_debugger_printf("rtems-db: vCont: no colon\n");
         ok = false;
       }
@@ -1258,6 +1241,6 @@
 remote_thread_select(uint8_t* buffer, int size)
 {
-  const char const* response = r_OK;
-  int*              index = NULL;
+  const char* response = r_OK;
+  int*        index = NULL;
 
   if (buffer[1] == 'g')
@@ -1300,8 +1283,8 @@
 remote_thread_alive(uint8_t* buffer, int size)
 {
-  const char const* response = r_E01;
-  DB_UINT           pid = 0;
-  DB_UINT           tid = 0;
-  bool              extended;
+  const char* response = r_E01;
+  DB_UINT     pid = 0;
+  DB_UINT     tid = 0;
+  bool        extended;
   extended = thread_id_decode((const char*) &buffer[1], &pid, &tid);
   if (!extended || (extended && check_pid(pid))) {
@@ -1424,5 +1407,5 @@
 {
   rtems_debugger_threads* threads = rtems_debugger->threads;
-  const char const*       response = r_E01;
+  const char*             response = r_E01;
   if (threads->selector_gen >= 0
       && threads->selector_gen < (int) threads->current.level) {
@@ -1488,7 +1471,7 @@
 remote_write_memory(uint8_t* buffer, int size)
 {
-  const char const* response = r_E01;
-  const char*       comma;
-  const char*       colon;
+  const char* response = r_E01;
+  const char* comma;
+  const char* colon;
   comma = strchr((const char*) buffer, ',');
   colon = strchr((const char*) buffer, ':');
@@ -1685,7 +1668,5 @@
 
   while (rtems_debugger_server_events_running()) {
-    r = rtems_debugger_server_events_wait();
-    if (r < 0)
-      break;
+    rtems_debugger_server_events_wait();
     if (!rtems_debugger_server_events_running())
       break;
@@ -1768,5 +1749,5 @@
 
   rtems_debugger->events_running = false;
-  rtems_debugger_server_events_wake();
+  rtems_debugger_server_events_signal();
 
   rtems_debugger_unlock();
@@ -1838,4 +1819,6 @@
   rtems_debugger->remote_debug = false;
 
+  rtems_chain_initialize_empty(&rtems_debugger->exception_threads);
+
   rtems_debugger->remote = rtems_debugger_remote_find(remote);
   if (rtems_debugger->remote== NULL) {
@@ -1852,4 +1835,5 @@
     free(rtems_debugger);
     rtems_debugger = NULL;
+    return -1;
   }
 
@@ -1908,5 +1892,4 @@
   int r;
 
-
   rtems_debugger_lock();
   rtems_debugger->server_running = true;
@@ -1950,4 +1933,5 @@
   rtems_debugger->server_running = false;
   rtems_debugger->server_finished = true;
+  _Condition_Initialize_named(&rtems_debugger->server_cond, "DBserver");
   rtems_debugger_unlock();
 
