mcu: Enable trdispatch infrastructure during homing

Separate out trsync handling from endstop handling in mcu.py code.
Enable the low-level trdispatch C code.

This is in preparation for multi-mcu homing.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 138 insertions, 66 deletions

diff --git a/klippy/mcu.py b/klippy/mcu.py
index dc0011c9..bd7267ad 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -9,37 +9,143 @@ import serialhdl, msgproto, pins, chelper, clocksync
 class error(Exception):
     pass
 
+class MCU_trsync:
+    REASON_ENDSTOP_HIT = 1
+    REASON_COMMS_TIMEOUT = 2
+    REASON_HOST_REQUEST = 3
+    REASON_PAST_END_TIME = 4
+    def __init__(self, mcu, trdispatch):
+        self._mcu = mcu
+        self._trdispatch = trdispatch
+        self._reactor = mcu.get_printer().get_reactor()
+        self._steppers = []
+        self._trdispatch_mcu = None
+        self._oid = mcu.create_oid()
+        self._cmd_queue = mcu.alloc_command_queue()
+        self._trsync_start_cmd = self._trsync_set_timeout_cmd = None
+        self._trsync_trigger_cmd = self._trsync_query_cmd = None
+        self._stepper_stop_cmd = None
+        self._trigger_completion = None
+        self._home_end_clock = None
+        mcu.register_config_callback(self._build_config)
+        printer = mcu.get_printer()
+        printer.register_event_handler("klippy:shutdown", self._shutdown)
+    def get_mcu(self):
+        return self._mcu
+    def get_oid(self):
+        return self._oid
+    def get_command_queue(self):
+        return self._cmd_queue
+    def add_stepper(self, stepper):
+        if stepper in self._steppers:
+            return
+        self._steppers.append(stepper)
+    def get_steppers(self):
+        return list(self._steppers)
+    def _build_config(self):
+        mcu = self._mcu
+        # Setup config
+        mcu.add_config_cmd("config_trsync oid=%d" % (self._oid,))
+        mcu.add_config_cmd(
+            "trsync_start oid=%d report_clock=0 report_ticks=0 expire_reason=0"
+            % (self._oid,), on_restart=True)
+        # Lookup commands
+        self._trsync_start_cmd = mcu.lookup_command(
+            "trsync_start oid=%c report_clock=%u report_ticks=%u"
+            " expire_reason=%c", cq=self._cmd_queue)
+        self._trsync_set_timeout_cmd = mcu.lookup_command(
+            "trsync_set_timeout oid=%c clock=%u", cq=self._cmd_queue)
+        self._trsync_trigger_cmd = mcu.lookup_command(
+            "trsync_trigger oid=%c reason=%c", cq=self._cmd_queue)
+        self._trsync_query_cmd = mcu.lookup_query_command(
+            "trsync_trigger oid=%c reason=%c",
+            "trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u",
+            oid=self._oid, cq=self._cmd_queue)
+        self._stepper_stop_cmd = mcu.lookup_command(
+            "stepper_stop_on_trigger oid=%c trsync_oid=%c", cq=self._cmd_queue)
+        # Create trdispatch_mcu object
+        set_timeout_tag = mcu.lookup_command_tag(
+            "trsync_set_timeout oid=%c clock=%u")
+        trigger_tag = mcu.lookup_command_tag("trsync_trigger oid=%c reason=%c")
+        state_tag = mcu.lookup_command_tag(
+            "trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u")
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self._trdispatch_mcu = ffi_main.gc(ffi_lib.trdispatch_mcu_alloc(
+            self._trdispatch, mcu._serial.serialqueue, # XXX
+            self._cmd_queue, self._oid, set_timeout_tag, trigger_tag,
+            state_tag), ffi_lib.free)
+    def _shutdown(self):
+        tc = self._trigger_completion
+        if tc is not None:
+            self._trigger_completion = None
+            tc.complete(False)
+    def _handle_trsync_state(self, params):
+        if not params['can_trigger']:
+            tc = self._trigger_completion
+            if tc is not None:
+                self._trigger_completion = None
+                self._reactor.async_complete(tc, True)
+        elif self._home_end_clock is not None:
+            clock = self._mcu.clock32_to_clock64(params['clock'])
+            if clock >= self._home_end_clock:
+                self._home_end_clock = None
+                self._trsync_trigger_cmd.send([self._oid,
+                                               self.REASON_PAST_END_TIME])
+    def start(self, print_time, trigger_completion, expire_timeout):
+        self._trigger_completion = trigger_completion
+        self._home_end_clock = None
+        clock = self._mcu.print_time_to_clock(print_time)
+        expire_ticks = self._mcu.seconds_to_clock(expire_timeout)
+        expire_clock = clock + expire_ticks
+        report_ticks = self._mcu.seconds_to_clock(expire_timeout * .4)
+        min_extend_ticks = self._mcu.seconds_to_clock(expire_timeout * .4 * .8)
+        ffi_main, ffi_lib = chelper.get_ffi()
+        ffi_lib.trdispatch_mcu_setup(self._trdispatch_mcu, clock, expire_clock,
+                                     expire_ticks, min_extend_ticks)
+        self._mcu.register_response(self._handle_trsync_state,
+                                    "trsync_state", self._oid)
+        self._trsync_start_cmd.send([self._oid, clock, report_ticks,
+                                     self.REASON_COMMS_TIMEOUT])
+        for s in self._steppers:
+            self._stepper_stop_cmd.send([s.get_oid(), self._oid])
+        self._trsync_set_timeout_cmd.send([self._oid, expire_clock])
+    def set_home_end_time(self, home_end_time):
+        self._home_end_clock = self._mcu.print_time_to_clock(home_end_time)
+    def stop(self):
+        self._mcu.register_response(None, "trsync_state", self._oid)
+        self._trigger_completion = None
+        if self._mcu.is_fileoutput():
+            return self.REASON_ENDSTOP_HIT
+        params = self._trsync_query_cmd.send([self._oid,
+                                              self.REASON_HOST_REQUEST])
+        for s in self._steppers:
+            s.note_homing_end(did_trigger=True) # XXX
+        return params['trigger_reason']
+
 class MCU_endstop:
     RETRY_QUERY = 1.000
     def __init__(self, mcu, pin_params):
         self._mcu = mcu
-        self._steppers = []
         self._pin = pin_params['pin']
         self._pullup = pin_params['pullup']
         self._invert = pin_params['invert']
-        self._reactor = mcu.get_printer().get_reactor()
-        self._oid = self._home_cmd = self._query_cmd = None
-        self._ts_oid = self._trsync_start_cmd = self._trsync_trigger_cmd = None
+        self._oid = self._mcu.create_oid()
+        self._home_cmd = self._query_cmd = None
         self._mcu.register_config_callback(self._build_config)
-        self._min_query_time = self._last_sent_time = self._end_home_time = 0.
-        self._trigger_completion = self._home_completion = None
+        self._trigger_completion = None
+        ffi_main, ffi_lib = chelper.get_ffi()
+        self._trdispatch = ffi_main.gc(ffi_lib.trdispatch_alloc(), ffi_lib.free)
+        self._trsync = MCU_trsync(mcu, self._trdispatch)
     def get_mcu(self):
         return self._mcu
     def add_stepper(self, stepper):
         if stepper.get_mcu() is not self._mcu:
             raise pins.error("Endstop and stepper must be on the same mcu")
-        if stepper in self._steppers:
-            return
-        self._steppers.append(stepper)
+        self._trsync.add_stepper(stepper)
     def get_steppers(self):
-        return list(self._steppers)
+        return self._trsync.get_steppers()
     def _build_config(self):
         # Setup config
-        self._ts_oid = self._mcu.create_oid()
-        self._mcu.add_config_cmd("config_trsync oid=%d" % (self._ts_oid,))
-        self._mcu.add_config_cmd("trsync_trigger oid=%d reason=0"
-                                 % (self._ts_oid,), on_restart=True)
-        self._oid = self._mcu.create_oid()
         self._mcu.add_config_cmd("config_endstop oid=%d pin=%s pull_up=%d"
                                  % (self._oid, self._pin, self._pullup))
         self._mcu.add_config_cmd(
@@ -47,14 +153,7 @@ class MCU_endstop:
             " rest_ticks=0 pin_value=0 trsync_oid=0 trigger_reason=0"
             % (self._oid,), on_restart=True)
         # Lookup commands
-        cmd_queue = self._mcu.alloc_command_queue()
-        self._trsync_start_cmd = self._mcu.lookup_command(
-            "trsync_start oid=%c report_clock=%u report_ticks=%u"
-            " expire_reason=%c", cq=cmd_queue)
-        self._trsync_trigger_cmd = self._mcu.lookup_command(
-            "trsync_trigger oid=%c reason=%c", cq=cmd_queue)
-        self._stepper_stop_cmd = self._mcu.lookup_command(
-            "stepper_stop_on_trigger oid=%c trsync_oid=%c", cq=cmd_queue)
+        cmd_queue = self._trsync.get_command_queue()
         self._home_cmd = self._mcu.lookup_command(
             "endstop_home oid=%c clock=%u sample_ticks=%u sample_count=%c"
             " rest_ticks=%u pin_value=%c trsync_oid=%c trigger_reason=%c",
@@ -67,55 +166,28 @@ class MCU_endstop:
                    triggered=True):
         clock = self._mcu.print_time_to_clock(print_time)
         rest_ticks = self._mcu.print_time_to_clock(print_time+rest_time) - clock
-        self._min_query_time = self._reactor.monotonic()
-        self._last_sent_time = 0.
-        self._home_end_time = self._reactor.NEVER
-        self._trigger_completion = self._reactor.completion()
-        self._mcu.register_response(self._handle_trsync_state,
-                                    "trsync_state", self._ts_oid)
-        report_ticks = self._mcu.seconds_to_clock(0.100)
-        self._trsync_start_cmd.send([self._ts_oid, clock, report_ticks, 0],
-                                    reqclock=clock)
-        for s in self._steppers:
-            self._stepper_stop_cmd.send([s.get_oid(), self._ts_oid])
+        reactor = self._mcu.get_printer().get_reactor()
+        self._trigger_completion = reactor.completion()
+        etrsync = self._trsync
+        etrsync.start(print_time, self._trigger_completion, .250)
+        ffi_main, ffi_lib = chelper.get_ffi()
+        ffi_lib.trdispatch_start(self._trdispatch, etrsync.REASON_HOST_REQUEST)
         self._home_cmd.send(
             [self._oid, clock, self._mcu.seconds_to_clock(sample_time),
              sample_count, rest_ticks, triggered ^ self._invert,
-             self._ts_oid, 0], reqclock=clock)
-        self._home_completion = self._reactor.register_callback(
-            self._home_retry)
+             etrsync.get_oid(), etrsync.REASON_ENDSTOP_HIT], reqclock=clock)
         return self._trigger_completion
-    def _handle_trsync_state(self, params):
-        logging.debug("trsync_state %s", params)
-        if params['#sent_time'] >= self._min_query_time:
-            if params['can_trigger']:
-                self._last_sent_time = params['#sent_time']
-            else:
-                self._min_query_time = self._reactor.NEVER
-                self._reactor.async_complete(self._trigger_completion, True)
-    def _home_retry(self, eventtime):
-        if self._mcu.is_fileoutput():
-            return True
-        while 1:
-            did_trigger = self._trigger_completion.wait(eventtime + 0.100)
-            if did_trigger is not None:
-                # Homing completed successfully
-                return True
-            # Check for timeout
-            last = self._mcu.estimated_print_time(self._last_sent_time)
-            if last > self._home_end_time or self._mcu.is_shutdown():
-                return False
     def home_wait(self, home_end_time):
-        self._home_end_time = home_end_time
-        did_trigger = self._home_completion.wait()
-        self._trsync_trigger_cmd.send([self._ts_oid, 0])
-        self._mcu.register_response(None, "trsync_state", self._ts_oid)
+        etrsync = self._trsync
+        etrsync.set_home_end_time(home_end_time)
+        if self._mcu.is_fileoutput():
+            self._trigger_completion.complete(True)
+        self._trigger_completion.wait()
         self._home_cmd.send([self._oid, 0, 0, 0, 0, 0, 0, 0])
-        for s in self._steppers:
-            s.note_homing_end(did_trigger=did_trigger)
-        if not self._trigger_completion.test():
-            self._trigger_completion.complete(False)
-        return did_trigger
+        ffi_main, ffi_lib = chelper.get_ffi()
+        ffi_lib.trdispatch_stop(self._trdispatch)
+        res = etrsync.stop()
+        return res == etrsync.REASON_ENDSTOP_HIT
     def query_endstop(self, print_time):
         clock = self._mcu.print_time_to_clock(print_time)
         if self._mcu.is_fileoutput():