tmc2130: Verify SPI register writes

The tmc2130 (and tmc5160) will respond back with the value written
during the next SPI command.  Use this feature to verify that the
value written matches the value sent.

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

4 files changed, 41 insertions, 11 deletions

diff --git a/klippy/extras/bus.py b/klippy/extras/bus.py
index 09259b9f..9b3e476a 100644
--- a/klippy/extras/bus.py
+++ b/klippy/extras/bus.py
@@ -92,8 +92,14 @@ class MCU_SPI:
             return
         self.spi_send_cmd.send([self.oid, data],
                                minclock=minclock, reqclock=reqclock)
-    def spi_transfer(self, data):
-        return self.spi_transfer_cmd.send([self.oid, data])
+    def spi_transfer(self, data, minclock=0, reqclock=0):
+        return self.spi_transfer_cmd.send([self.oid, data],
+                                          minclock=minclock, reqclock=reqclock)
+    def spi_transfer_with_preface(self, preface_data, data,
+                                  minclock=0, reqclock=0):
+        return self.spi_transfer_cmd.send_with_preface(
+            self.spi_send_cmd, [self.oid, preface_data], [self.oid, data],
+            minclock=minclock, reqclock=reqclock)
 
 # Helper to setup an spi bus from settings in a config section
 def MCU_SPI_from_config(config, mode, pin_option="cs_pin",
diff --git a/klippy/extras/tmc2130.py b/klippy/extras/tmc2130.py
index e827b32a..38405b21 100644
--- a/klippy/extras/tmc2130.py
+++ b/klippy/extras/tmc2130.py
@@ -183,7 +183,17 @@ class MCU_TMC_SPI_chain:
             minclock = self.spi.get_mcu().print_time_to_clock(print_time)
         data = [(reg | 0x80) & 0xff, (val >> 24) & 0xff, (val >> 16) & 0xff,
                 (val >> 8) & 0xff, val & 0xff]
-        self.spi.spi_send(self._build_cmd(data, chain_pos), minclock)
+        if self.printer.get_start_args().get('debugoutput') is not None:
+            self.spi.spi_send(self._build_cmd(data, chain_pos), minclock)
+            return val
+        write_cmd = self._build_cmd(data, chain_pos)
+        dummy_read = self._build_cmd([0x00, 0x00, 0x00, 0x00, 0x00], chain_pos)
+        params = self.spi.spi_transfer_with_preface(write_cmd, dummy_read,
+                                                    minclock=minclock)
+        pr = bytearray(params['response'])
+        pr = pr[(self.chain_len - chain_pos) * 5 :
+                (self.chain_len - chain_pos + 1) * 5]
+        return (pr[1] << 24) | (pr[2] << 16) | (pr[3] << 8) | pr[4]
 
 # Helper to setup an spi daisy chain bus from settings in a config section
 def lookup_tmc_spi_chain(config):
@@ -225,7 +235,12 @@ class MCU_TMC_SPI:
     def set_register(self, reg_name, val, print_time=None):
         reg = self.name_to_reg[reg_name]
         with self.mutex:
-            self.tmc_spi.reg_write(reg, val, self.chain_pos, print_time)
+            for retry in range(5):
+                v = self.tmc_spi.reg_write(reg, val, self.chain_pos, print_time)
+                if v == val:
+                    return
+        raise self.printer.command_error(
+            "Unable to write tmc spi '%s' register %s" % (self.name, reg_name))
 
 
 ######################################################################
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 7c6529a2..93e71572 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -347,7 +347,8 @@ class RetryAsyncCommand:
         if params['#sent_time'] >= self.min_query_time:
             self.min_query_time = self.reactor.NEVER
             self.reactor.async_complete(self.completion, params)
-    def get_response(self, cmd, cmd_queue, minclock=0, reqclock=0):
+    def get_response(self, cmds, cmd_queue, minclock=0, reqclock=0):
+        cmd, = cmds
         self.serial.raw_send_wait_ack(cmd, minclock, reqclock, cmd_queue)
         first_query_time = query_time = self.reactor.monotonic()
         while 1:
@@ -378,14 +379,19 @@ class CommandQueryWrapper:
         if cmd_queue is None:
             cmd_queue = serial.get_default_command_queue()
         self._cmd_queue = cmd_queue
-    def send(self, data=(), minclock=0, reqclock=0):
-        cmd = self._cmd.encode(data)
+    def _do_send(self, cmds, minclock, reqclock):
         xh = self._xmit_helper(self._serial, self._response, self._oid)
         reqclock = max(minclock, reqclock)
         try:
-            return xh.get_response(cmd, self._cmd_queue, minclock, reqclock)
+            return xh.get_response(cmds, self._cmd_queue, minclock, reqclock)
         except serialhdl.error as e:
             raise self._error(str(e))
+    def send(self, data=(), minclock=0, reqclock=0):
+        return self._do_send([self._cmd.encode(data)], minclock, reqclock)
+    def send_with_preface(self, preface_cmd, preface_data=(), data=(),
+                          minclock=0, reqclock=0):
+        cmds = [preface_cmd._cmd.encode(preface_data), self._cmd.encode(data)]
+        return self._do_send(cmds, minclock, reqclock)
 
 # Wrapper around command sending
 class CommandWrapper:
diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py
index c9eccb50..d17993b9 100644
--- a/klippy/serialhdl.py
+++ b/klippy/serialhdl.py
@@ -199,7 +199,7 @@ class SerialReader:
     def send_with_response(self, msg, response):
         cmd = self.msgparser.create_command(msg)
         src = SerialRetryCommand(self, response)
-        return src.get_response(cmd, self.default_cmd_queue)
+        return src.get_response([cmd], self.default_cmd_queue)
     def alloc_command_queue(self):
         return self.ffi_main.gc(self.ffi_lib.serialqueue_alloc_commandqueue(),
                                 self.ffi_lib.serialqueue_free_commandqueue)
@@ -249,11 +249,14 @@ class SerialRetryCommand:
         self.serial.register_response(self.handle_callback, name, oid)
     def handle_callback(self, params):
         self.last_params = params
-    def get_response(self, cmd, cmd_queue, minclock=0, reqclock=0):
+    def get_response(self, cmds, cmd_queue, minclock=0, reqclock=0):
         retries = 5
         retry_delay = .010
         while 1:
-            self.serial.raw_send_wait_ack(cmd, minclock, reqclock, cmd_queue)
+            for cmd in cmds[:-1]:
+                self.serial.raw_send(cmd, minclock, reqclock, cmd_queue)
+            self.serial.raw_send_wait_ack(cmds[-1], minclock, reqclock,
+                                          cmd_queue)
             params = self.last_params
             if params is not None:
                 self.serial.register_response(None, self.name, self.oid)