From 7ee87dc2db42969754c78e14c456ed55a923d7a2 Mon Sep 17 00:00:00 2001
From: teemuatlut <teemu.mantykallio@live.fi>
Date: Sat, 10 Feb 2018 19:16:08 +0200
Subject: [PATCH] Init CS pins before loading settings

---
 Marlin/Marlin_main.cpp |  3 +++
 Marlin/tmc_util.cpp    | 39 +++++++++++++++++++++++++++++++++++++++
 Marlin/tmc_util.h      |  4 ++++
 3 files changed, 46 insertions(+)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 225812a67af..9b305e483f2 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -13636,6 +13636,9 @@ void setup() {
   SERIAL_PROTOCOLLNPGM("start");
   SERIAL_ECHO_START();
 
+  #if ENABLED(HAVE_TMC2130)
+    tmc_init_cs_pins();
+  #endif
   #if ENABLED(HAVE_TMC2208)
     tmc2208_serial_begin();
   #endif
diff --git a/Marlin/tmc_util.cpp b/Marlin/tmc_util.cpp
index db7af565402..14339bf61b0 100644
--- a/Marlin/tmc_util.cpp
+++ b/Marlin/tmc_util.cpp
@@ -565,4 +565,43 @@ void _tmc_say_sgt(const char name[], const uint32_t sgt) {
 
 #endif // SENSORLESS_HOMING
 
+#if ENABLED(HAVE_TMC2130)
+  #define SET_CS_PIN(st) OUT_WRITE(st##_CS_PIN, HIGH)
+  void tmc_init_cs_pins() {
+    #if ENABLED(X_IS_TMC2130)
+      SET_CS_PIN(X);
+    #endif
+    #if ENABLED(Y_IS_TMC2130)
+      SET_CS_PIN(Y);
+    #endif
+    #if ENABLED(Z_IS_TMC2130)
+      SET_CS_PIN(Z);
+    #endif
+    #if ENABLED(X2_IS_TMC2130)
+      SET_CS_PIN(X2);
+    #endif
+    #if ENABLED(Y2_IS_TMC2130)
+      SET_CS_PIN(Y2);
+    #endif
+    #if ENABLED(Z2_IS_TMC2130)
+      SET_CS_PIN(Z2);
+    #endif
+    #if ENABLED(E0_IS_TMC2130)
+      SET_CS_PIN(E0);
+    #endif
+    #if ENABLED(E1_IS_TMC2130)
+      SET_CS_PIN(E1);
+    #endif
+    #if ENABLED(E2_IS_TMC2130)
+      SET_CS_PIN(E2);
+    #endif
+    #if ENABLED(E3_IS_TMC2130)
+      SET_CS_PIN(E3);
+    #endif
+    #if ENABLED(E4_IS_TMC2130)
+      SET_CS_PIN(E4);
+    #endif
+  }
+#endif // HAVE_TMC2130
+
 #endif // HAS_TRINAMIC
diff --git a/Marlin/tmc_util.h b/Marlin/tmc_util.h
index 14690067efc..de51de93d1a 100644
--- a/Marlin/tmc_util.h
+++ b/Marlin/tmc_util.h
@@ -100,4 +100,8 @@ void monitor_tmc_driver();
   void tmc_sensorless_homing(TMC2130Stepper &st, bool enable=true);
 #endif
 
+#if ENABLED(HAVE_TMC2130)
+  void tmc_init_cs_pins();
+#endif
+
 #endif // _TMC_UTIL_H_