From 5fd1c9853d243cccdb53c75cae0a1a48b882368d Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 27 Aug 2021 19:24:14 -0400
Subject: [PATCH] tmc: Add support for a get_status() method

Export mcu_phase_offset and drv_status information.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 docs/Status_Reference.md | 15 +++++++++++++++
 klippy/extras/tmc.py     | 26 +++++++++++++++++++++++++-
 klippy/extras/tmc2130.py |  1 +
 klippy/extras/tmc2208.py |  1 +
 klippy/extras/tmc2209.py |  1 +
 klippy/extras/tmc2660.py |  1 +
 klippy/extras/tmc5160.py |  1 +
 7 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/docs/Status_Reference.md b/docs/Status_Reference.md
index 969f8704..68abf2f6 100644
--- a/docs/Status_Reference.md
+++ b/docs/Status_Reference.md
@@ -311,6 +311,21 @@ objects:
   temperature seen by the sensor since the Klipper host software was
   last restarted.
 
+## tmc drivers
+
+The following information is available in
+[TMC stepper driver](Config_Reference.md#tmc-stepper-driver-configuration)
+objects (eg, `[tmc2208 stepper_x]`):
+- `mcu_phase_offset`: The micro-controller stepper position
+  corresponding with the driver's "zero" phase. This field may be null
+  if the phase offset is not known.
+- `phase_offset_position`: The "commanded position" corresponding to
+  the driver's "zero" phase. This field may be null if the phase
+  offset is not known.
+- `drv_status`: The results of the last driver status query. (Only
+  non-zero fields are reported.) This field will be null if the driver
+  is not enabled (and thus is not periodically queried).
+
 ## toolhead
 
 The following information is available in the `toolhead` object
diff --git a/klippy/extras/tmc.py b/klippy/extras/tmc.py
index 0279a040..f8eae1cf 100644
--- a/klippy/extras/tmc.py
+++ b/klippy/extras/tmc.py
@@ -74,6 +74,11 @@ class FieldHelper:
             if sval and sval != "0":
                 fields.append(" %s=%s" % (field_name, sval))
         return "%-11s %08x%s" % (reg_name + ":", reg_value, "".join(fields))
+    def get_reg_fields(self, reg_name, reg_value):
+        # Provide fields found in a register
+        reg_fields = self.all_fields.get(reg_name, {})
+        return {field_name: self.get_field(field_name, reg_value, reg_name)
+                for field_name, mask in reg_fields.items()}
 
 
 ######################################################################
@@ -88,6 +93,7 @@ class TMCErrorCheck:
         self.mcu_tmc = mcu_tmc
         self.fields = mcu_tmc.get_fields()
         self.check_timer = None
+        self.last_drv_status = self.last_status = None
         # Setup for GSTAT query
         reg_name = self.fields.lookup_register("drv_err")
         if reg_name is not None:
@@ -134,7 +140,7 @@ class TMCErrorCheck:
             if val & mask != last_value & mask:
                 fmt = self.fields.pretty_format(reg_name, val)
                 logging.info("TMC '%s' reports %s", self.stepper_name, fmt)
-                reg_info[0] = last_value = val
+            reg_info[0] = last_value = val
             if not val & err_mask:
                 if not cs_actual_mask or val & cs_actual_mask:
                     break
@@ -186,6 +192,16 @@ class TMCErrorCheck:
             if cleared_flags & reset_mask:
                 return True
         return False
+    def get_status(self, eventtime=None):
+        if self.check_timer is None:
+            return {'drv_status': None}
+        last_value, reg_name = self.drv_status_reg_info[:2]
+        if last_value != self.last_drv_status:
+            self.last_drv_status = last_value
+            fields = self.fields.get_reg_fields(reg_name, last_value)
+            fields = {n: v for n, v in fields.items() if v}
+            self.last_status = {'drv_status': fields}
+        return self.last_status
 
 
 ######################################################################
@@ -353,6 +369,14 @@ class TMCCommandHelper:
             self._init_registers()
         except self.printer.command_error as e:
             logging.info("TMC %s failed to init: %s", self.name, str(e))
+    # get_status information export
+    def get_status(self, eventtime=None):
+        if self.stepper is not None and self.mcu_phase_offset is not None:
+            cpos = self.stepper.mcu_to_commanded_position(self.mcu_phase_offset)
+        res = {'mcu_phase_offset': self.mcu_phase_offset,
+               'phase_offset_position': cpos}
+        res.update(self.echeck_helper.get_status(eventtime))
+        return res
     # DUMP_TMC support
     def setup_register_dump(self, read_registers, read_translate=None):
         self.read_registers = read_registers
diff --git a/klippy/extras/tmc2130.py b/klippy/extras/tmc2130.py
index b23b2a64..c29622ec 100644
--- a/klippy/extras/tmc2130.py
+++ b/klippy/extras/tmc2130.py
@@ -261,6 +261,7 @@ class TMC2130:
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
         self.get_phase_offset = cmdhelper.get_phase_offset
+        self.get_status = cmdhelper.get_status
         # Setup basic register values
         tmc.TMCStealthchopHelper(config, self.mcu_tmc, TMC_FREQUENCY)
         # Allow other registers to be set from the config
diff --git a/klippy/extras/tmc2208.py b/klippy/extras/tmc2208.py
index fcc957a5..1b5968a9 100644
--- a/klippy/extras/tmc2208.py
+++ b/klippy/extras/tmc2208.py
@@ -193,6 +193,7 @@ class TMC2208:
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters, self.read_translate)
         self.get_phase_offset = cmdhelper.get_phase_offset
+        self.get_status = cmdhelper.get_status
         # Setup basic register values
         self.fields.set_field("mstep_reg_select", True)
         self.fields.set_field("multistep_filt", True)
diff --git a/klippy/extras/tmc2209.py b/klippy/extras/tmc2209.py
index e0d64c31..64bf195b 100644
--- a/klippy/extras/tmc2209.py
+++ b/klippy/extras/tmc2209.py
@@ -69,6 +69,7 @@ class TMC2209:
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
         self.get_phase_offset = cmdhelper.get_phase_offset
+        self.get_status = cmdhelper.get_status
         # Setup basic register values
         self.fields.set_field("pdn_disable", True)
         self.fields.set_field("mstep_reg_select", True)
diff --git a/klippy/extras/tmc2660.py b/klippy/extras/tmc2660.py
index 8cb1d1c1..52aa2e42 100644
--- a/klippy/extras/tmc2660.py
+++ b/klippy/extras/tmc2660.py
@@ -230,6 +230,7 @@ class TMC2660:
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
         self.get_phase_offset = cmdhelper.get_phase_offset
+        self.get_status = cmdhelper.get_status
 
         # CHOPCONF
         set_config_field = self.fields.set_config_field
diff --git a/klippy/extras/tmc5160.py b/klippy/extras/tmc5160.py
index 76c939d4..fae17226 100644
--- a/klippy/extras/tmc5160.py
+++ b/klippy/extras/tmc5160.py
@@ -295,6 +295,7 @@ class TMC5160:
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
         self.get_phase_offset = cmdhelper.get_phase_offset
+        self.get_status = cmdhelper.get_status
         # Setup basic register values
         tmc.TMCStealthchopHelper(config, self.mcu_tmc, TMC_FREQUENCY)
         #   CHOPCONF