diff --git a/klippy/gcode.py b/klippy/gcode.py
index d607a0cc..5b1d8904 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -72,6 +72,13 @@ class GCodeParser:
         self.build_handlers()
         if is_ready and self.is_fileinput and self.fd_handle is None:
             self.fd_handle = self.reactor.register_fd(self.fd, self.process_data)
+    def note_mcu_error(self):
+        if self.toolhead is not None:
+            self.toolhead.motor_off()
+        if self.heater_nozzle is not None:
+            self.heater_nozzle.set_temp(0., 0.)
+        if self.heater_bed is not None:
+            self.heater_bed.set_temp(0., 0.)
     def dump_debug(self):
         logging.info("Dumping gcode input %d blocks" % (
             len(self.input_log),))
diff --git a/klippy/klippy.py b/klippy/klippy.py
index 0dbc8f60..25f6b99b 100644
--- a/klippy/klippy.py
+++ b/klippy/klippy.py
@@ -184,6 +184,10 @@ class Printer:
         self.state_message = "Firmware shutdown: %s%s" % (
             msg, message_shutdown)
         self.gcode.set_printer_ready(False)
+    def note_mcu_error(self, msg):
+        self.state_message = "%s%s" % (msg, message_restart)
+        self.gcode.set_printer_ready(False)
+        self.gcode.note_mcu_error()
 
 
 ######################################################################
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 5d61d5b1..2b0fe259 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -168,7 +168,7 @@ class MCU_endstop:
                 raise error("Timeout during endstop homing")
         if self._mcu.is_shutdown:
             raise error("MCU is shutdown")
-        last_clock = self._mcu.get_last_clock()
+        last_clock, last_clock_time = self._mcu.get_last_clock()
         if last_clock >= self._next_query_clock:
             self._next_query_clock = last_clock + self._retry_query_ticks
             msg = self._query_cmd.encode(self._oid)
@@ -278,8 +278,8 @@ class MCU_adc:
         self._max_sample = min(0xffff, int(math.ceil(maxval * max_adc)))
         self._inv_max_adc = 1.0 / max_adc
     def _init_callback(self):
-        cur_clock = self._mcu.get_last_clock()
-        clock = cur_clock + int(self._mcu_freq * (1.0 + self._oid * 0.01)) # XXX
+        last_clock, last_clock_time = self._mcu.get_last_clock()
+        clock = last_clock + int(self._mcu_freq * (1.0 + self._oid * 0.01)) # XXX
         msg = self._query_cmd.encode(
             self._oid, clock, self._sample_ticks, self._sample_count
             , self._report_clock, self._min_sample, self._max_sample)
@@ -295,6 +295,7 @@ class MCU_adc:
         self._callback = callback
 
 class MCU:
+    COMM_TIMEOUT = 3.5
     def __init__(self, printer, config):
         self._printer = printer
         self._config = config
@@ -304,6 +305,8 @@ class MCU:
         self.serial = serialhdl.SerialReader(printer.reactor, serialport, baud)
         self.is_shutdown = False
         self._is_fileoutput = False
+        self._timeout_timer = printer.reactor.register_timer(
+            self.timeout_handler)
         # Config building
         self._emergency_stop_cmd = None
         self._num_oids = 0
@@ -341,7 +344,10 @@ class MCU:
     def connect(self):
         if not self._is_fileoutput:
             self.serial.connect()
+            self._printer.reactor.update_timer(
+                self._timeout_timer, time.time() + self.COMM_TIMEOUT)
         self._mcu_freq = float(self.serial.msgparser.config['CLOCK_FREQ'])
+        self._emergency_stop_cmd = self.lookup_command("emergency_stop")
         self.register_msg(self.handle_shutdown, 'shutdown')
         self.register_msg(self.handle_shutdown, 'is_shutdown')
         self.register_msg(self.handle_mcu_stats, 'stats')
@@ -355,6 +361,15 @@ class MCU:
                 return 0.250
             self.set_print_start_time = dummy_set_print_start_time
             self.get_print_buffer_time = dummy_get_print_buffer_time
+    def timeout_handler(self, eventtime):
+        last_clock, last_clock_time = self.serial.get_last_clock()
+        timeout = last_clock_time + self.COMM_TIMEOUT
+        if eventtime < timeout:
+            return timeout
+        logging.info("Timeout with firmware (eventtime=%f last_status=%f)" % (
+            eventtime, last_clock_time))
+        self._printer.note_mcu_error("Lost communication with firmware")
+        return self._printer.reactor.NEVER
     def disconnect(self):
         self.serial.disconnect()
         if self._steppersync is not None:
@@ -386,7 +401,6 @@ class MCU:
                 continue
             self.add_config_cmd(line)
     def build_config(self):
-        self._emergency_stop_cmd = self.lookup_command("emergency_stop")
         # Build config commands
         self._add_custom()
         self._config_cmds.insert(0, "allocate_oids count=%d" % (
diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py
index 7f2b9bda..7120c43c 100644
--- a/klippy/serialhdl.py
+++ b/klippy/serialhdl.py
@@ -152,7 +152,7 @@ class SerialReader:
         return last_ack_clock - clock_diff
     def get_last_clock(self):
         with self.lock:
-            return self.last_ack_clock
+            return self.last_ack_clock, self.last_ack_time
     # Command sending
     def send(self, cmd, minclock=0, reqclock=0, cq=None):
         if cq is None: