diff --git a/src/device.c b/src/device.c
index f94ada2..948aa03 100644
--- a/src/device.c
+++ b/src/device.c
@@ -20,19 +20,53 @@
  */
 
 #include <stdio.h>
+#include <stdlib.h>
 #include <unistd.h>
 #include <sys/stat.h>
+#include <sys/types.h>
 #include "gotty/options.h"
+#include "gotty/print.h"
+#include "gotty/tty.h"
 
 void wait_for_device(void)
 {
+    int ready, n;
     struct stat status;
+    fd_set rdfs;
+    struct timeval tv;
+    char c, c0=0, c1=0;
 
     /* Loop until device pops up */
     while (true)
     {
-        sleep(1);
-        if (stat(option.device, &status) == 0)
-            return;
+        /* Wait up to 1 seconds */
+        tv.tv_sec = 1;
+        tv.tv_usec = 0;
+
+        FD_ZERO(&rdfs);
+        FD_SET(STDIN_FILENO, &rdfs);
+
+        /* Block until input becomes available or timeout */
+        ready = select(STDIN_FILENO + 1, &rdfs, NULL, NULL, &tv);
+        if (ready)
+        {
+            /* Input from stdin ready */
+
+            /* Read one character */
+            n = read(STDIN_FILENO, &c, 1);
+
+            /* Exit upon ctrl-g ctrl-q sequence */
+            c1 = c0;
+            c0 = c;
+            if ((c0 == CTRLQ) && (c1 == CTRLG))
+                exit(EXIT_SUCCESS);
+        } else
+        {
+            /* Timeout */
+
+            /* Test for device file */
+            if (stat(option.device, &status) == 0)
+                return;
+        }
     }
 }
diff --git a/src/include/gotty/tty.h b/src/include/gotty/tty.h
index 8aef162..9d9ea67 100644
--- a/src/include/gotty/tty.h
+++ b/src/include/gotty/tty.h
@@ -22,7 +22,11 @@
 #ifndef TTY_H
 #define TTY_H
 
+#define CTRLG 0x07
+#define CTRLQ 0x11
+
 void configure_stdout(void);
+void restore_stdout(void);
 int connect_tty(void);
 
 #endif
diff --git a/src/main.c b/src/main.c
index ba1886c..86f7e08 100644
--- a/src/main.c
+++ b/src/main.c
@@ -34,6 +34,9 @@ int main(int argc, char *argv[])
     /* Configure output terminal */
     configure_stdout();
 
+    /* Restore output terminal on exit */
+    atexit(&restore_stdout);
+
     /* Connect to tty device */
     if (option.no_autoconnect)
         status = connect_tty();
diff --git a/src/tty.c b/src/tty.c
index 8b186ba..dba9745 100644
--- a/src/tty.c
+++ b/src/tty.c
@@ -35,11 +35,9 @@
 
 #define MAX(x, y) (((x) > (y)) ? (x) : (y))
 
-#define CTRLG 0x07
-#define CTRLQ 0x11
-
-static int fd;
+static int connected = false;
 struct termios stdio, old_stdio, old_tio;
+static int fd;
 
 void configure_stdout(void)
 {
@@ -68,18 +66,20 @@ void configure_stdout(void)
     tcsetattr(STDOUT_FILENO, TCSAFLUSH, &stdio);
 }
 
-void restore(void)
+void restore_stdout(void)
 {
-    static int i=0;
-
     tcflush(STDOUT_FILENO, TCIOFLUSH);
     tcsetattr(STDOUT_FILENO, TCSANOW, &old_stdio);
     tcsetattr(STDOUT_FILENO, TCSAFLUSH, &old_stdio);
+}
 
+void restore_tty(void)
+{
     tcsetattr(fd, TCSANOW, &old_tio);
     tcsetattr(fd, TCSAFLUSH, &old_tio);
 
-    gotty_printf("Disconnected\n");
+    if (connected)
+        gotty_printf("Disconnected\n");
 }
 
 int connect_tty(void)
@@ -106,15 +106,16 @@ int connect_tty(void)
     }
 
     gotty_printf("Connected");
+    connected = true;
 
     /* Save current port settings */
     if (tcgetattr(fd, &old_tio) < 0)
         return EXIT_FAILURE;
 
-    /* Make sure we restore settings on exit */
+    /* Make sure we restore tty settings on exit */
     if (first)
     {
-        atexit(&restore);
+        atexit(&restore_tty);
         first = false;
     }
 
@@ -152,9 +153,11 @@ int connect_tty(void)
                 fflush(stdout);
             } else
             {
+                /* Error reading - device is likely unplugged */
                 if (!option.no_autoconnect)
                     gotty_printf("Disconnected");
                 close(fd);
+                connected = false;
                 return EXIT_FAILURE;
             }
         }