diff --git a/src/include/tio/log.h b/src/include/tio/log.h
index 41b5103..9e83acd 100644
--- a/src/include/tio/log.h
+++ b/src/include/tio/log.h
@@ -24,6 +24,7 @@
 
 void log_open(const char *filename);
 void log_write(char c);
+void log_write_str(const char * str);
 void log_close(void);
 void log_exit(void);
 
diff --git a/src/log.c b/src/log.c
index 0bad55d..f15c1ed 100644
--- a/src/log.c
+++ b/src/log.c
@@ -49,6 +49,13 @@ void log_write(char c)
         fputc(c, fp);
 }
 
+void log_write_str(const char * str)
+{
+    if (fp != NULL)
+        fputs(str, fp);
+}
+
+
 void log_close(void)
 {
     if (fp != NULL)
diff --git a/src/tty.c b/src/tty.c
index bee6f9f..17a4bdb 100644
--- a/src/tty.c
+++ b/src/tty.c
@@ -35,6 +35,7 @@
 #include <stdbool.h>
 #include <errno.h>
 #include <time.h>
+#include <sys/time.h>
 #include "config.h"
 #include "tio/tty.h"
 #include "tio/print.h"
@@ -641,8 +642,16 @@ int tty_connect(void)
                     /* Print timestamp on new line, if desired. */
                     if (next_timestamp && input_char != '\n' && input_char != '\r')
                     {
-                        fprintf(stdout, ANSI_COLOR_GRAY "[%s] " ANSI_COLOR_RESET, current_time());
+                        char current_str[128];
+                        struct timeval tv;
+                        struct tm *tm;
+                        gettimeofday(&tv, NULL);
+                        tm = localtime(&tv.tv_sec);
+                        sprintf(current_str, "[%-2d:%02d:%02d.%03ld] ",  tm->tm_hour,  tm->tm_min, tm->tm_sec, tv.tv_usec/1000);
+                        fprintf(stdout, ANSI_COLOR_GRAY "%s" ANSI_COLOR_RESET, current_str);
                         next_timestamp = 0;
+                        if (option.log && option.timestamp)
+                            log_write_str(current_str);
                     }
 
                     /* Map input character */