diff --git a/src/hal/avr/shr16.cpp b/src/hal/avr/shr16.cpp
index 0237d60..24326cd 100644
--- a/src/hal/avr/shr16.cpp
+++ b/src/hal/avr/shr16.cpp
@@ -38,14 +38,19 @@ void SHR16::SetLED(uint16_t led) {
 }
 
 void SHR16::SetTMCEnabled(uint8_t index, bool ena) {
-    //    ena ^= 7;
-    //	ena = ((ena & 1) << 1) | ((ena & 2) << 2) | ((ena & 4) << 3); // 0. << 1 == 1., 1. << 2 == 3., 2. << 3 == 5.
-    //	Write((shr16_v & ~SHR16_ENA_MSK) | ena);
+    const uint16_t mask = 1 << (2 * index + 1);
+    if (ena)
+        Write(shr16_v & ~mask);
+    else
+        Write(shr16_v | mask);
 }
 
 void SHR16::SetTMCDir(uint8_t index, bool dir) {
-    //    dir = (dir & 1) | ((dir & 2) << 1) | ((dir & 4) << 2); // 0., 1. << 1 == 2., 2. << 2 == 4.
-    //	Write((shr16_v & ~SHR16_DIR_MSK) | dir);
+    const uint16_t mask = 1 << (2 * index);
+    if (dir)
+        Write(shr16_v & ~mask);
+    else
+        Write(shr16_v | mask);
 }
 
 } // namespace shr16