diff --git a/slsDetectorServers/matterhornServer/CMakeLists.txt b/slsDetectorServers/matterhornServer/CMakeLists.txt
index 47d7fa1f6..39411031a 100644
--- a/slsDetectorServers/matterhornServer/CMakeLists.txt
+++ b/slsDetectorServers/matterhornServer/CMakeLists.txt
@@ -3,6 +3,7 @@
 set(MATTERHORN_SOURCES
     ${CMAKE_CURRENT_SOURCE_DIR}/src/MatterhornApp.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/SPICommunication.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/HelperFunctions.cpp
 )
 
 if(SLS_USE_SIMULATOR)
diff --git a/slsDetectorServers/matterhornServer/include/BaseMatterhornServer.h b/slsDetectorServers/matterhornServer/include/BaseMatterhornServer.h
index 764e44e4e..3bb22069d 100644
--- a/slsDetectorServers/matterhornServer/include/BaseMatterhornServer.h
+++ b/slsDetectorServers/matterhornServer/include/BaseMatterhornServer.h
@@ -345,22 +345,15 @@ BaseMatterhornServer<DerivedServer>::set_counter_mask(ServerInterface &socket) {
         return ReturnCode::FAIL;
     }
 
-    // counter mask update to num_counters and starting_counter
-    uint32_t counter_to_set{};
-    // TODO: update properly
-    /*
-    switch (counter_mask) {
-    case 0b1:
-        counter_to_set = 0b0000; // counter 0 enabled
-    case 0b10:
-        counter_to_set = 0b0001; // counter 1 enabled
-    case 0b100:
-        counter_to_set = 0b0010; // counter 2 enabled
-    case 0b1000:
-        counter_to_set = 0b0011; // counter 3 enabled
-    case 0b11:
+    // counter mask update to num consecutive counters and starting_counter
+    uint32_t spi_counter_mask{};
+    try {
+        spi_counter_mask = convertCounterMaskToSPICounterMask(counter_mask);
+    } catch (const std::invalid_argument &e) {
+        LOG(logERROR) << "Failed to convert counter mask to SPI counter mask: "
+                      << e.what();
+        return ReturnCode::FAIL;
     }
-    */
 
     try {
         auto reg_value = spiCommunication.SPIread(
@@ -370,7 +363,7 @@ BaseMatterhornServer<DerivedServer>::set_counter_mask(ServerInterface &socket) {
                 // Command overload for some of the SPI registers
 
         setSPIRegisterField(reg_value, SPIRegisters::NUM_COUNTERS,
-                            counter_mask);
+                            spi_counter_mask);
 
         spiCommunication.SPIwrite(SPIRegisters::NUM_COUNTERS.register_, 0,
                                   reg_value);
diff --git a/slsDetectorServers/matterhornServer/include/HelperFunctions.hpp b/slsDetectorServers/matterhornServer/include/HelperFunctions.hpp
index a1a076f28..cf297ad8e 100644
--- a/slsDetectorServers/matterhornServer/include/HelperFunctions.hpp
+++ b/slsDetectorServers/matterhornServer/include/HelperFunctions.hpp
@@ -12,48 +12,11 @@
  * and number of counters to read
  * @return actual counter mask to be written to the SPI register
  */
-uint32_t convertCounterMaskToSPICounterMask(const uint32_t counter_mask) {
-    uint32_t spi_counter_mask = 0;
-
-    // start
-
-    /*
-    for(uint8_t i = 0; i < 4; ++i) {
-        if (counter_mask & (1 << i) && (spi_counter_mask == 0)) {
-            spi_counter_mask |= (i % 4); // set the starting counter based on
-                                         // the counter mask
-        }
-        else if ((counter_mask & (1 << i))) {
-            spi_counter_mask |= (1 << (2 + (i % 4))); // set the number of
-    counters
-                                                     // to read based on the
-                                                     // counter mask and the
-                                                     // starting counter
-        }
-    }
-    */
-
-    // case distinction easiest!!!
-}
+uint32_t convertCounterMaskToSPICounterMask(const uint32_t counter_mask);
 
 /**
  * @brief converts the actual counter mask read from the SPI register to the
  * counter mask to be sent to the client e.g. bit set to 1 if counter enabled
  * @return counter mask to be sent to the client
  */
-uint32_t convertSPICounterMaskToCounterMask(const uint32_t spi_counter_mask) {
-
-    uint32_t counter_mask = 0;
-
-    uint8_t start_counter = spi_counter_mask & 0b11; // extract starting counter
-    uint8_t num_counters =
-        (spi_counter_mask >> 2) & 0b11; // extract number of counters
-
-    constexpr uint8_t max_counters = 4;
-
-    for (uint8_t i = 0; i < num_counters; ++i) {
-        counter_mask |= (1 << ((start_counter + i) % max_counters));
-    }
-
-    return counter_mask;
-}
\ No newline at end of file
+uint32_t convertSPICounterMaskToCounterMask(const uint32_t spi_counter_mask);
\ No newline at end of file
diff --git a/slsDetectorServers/matterhornServer/src/HelperFunctions.cpp b/slsDetectorServers/matterhornServer/src/HelperFunctions.cpp
new file mode 100644
index 000000000..69a49cdf2
--- /dev/null
+++ b/slsDetectorServers/matterhornServer/src/HelperFunctions.cpp
@@ -0,0 +1,71 @@
+#include "HelperFunctions.hpp"
+#include <stdexcept>
+
+uint32_t convertCounterMaskToSPICounterMask(const uint32_t counter_mask) {
+    uint32_t spi_counter_mask = 0;
+
+    switch (counter_mask) {
+    case 0b1:
+        spi_counter_mask = 0b0000; // counter 0 enabled
+        break;
+    case 0b10:
+        spi_counter_mask = 0b0001; // counter 1 enabled
+        break;
+    case 0b100:
+        spi_counter_mask = 0b0010; // counter 2 enabled
+        break;
+    case 0b1000:
+        spi_counter_mask = 0b0011; // counter 3 enabled
+        break;
+    case 0b11:
+        spi_counter_mask = 0b0100; // counter 0 and 1 enabled
+        break;
+    case 0b110:
+        spi_counter_mask = 0b0101; // counter 1 and 2 enabled
+        break;
+    case 0b1100:
+        spi_counter_mask = 0b0110; // counter 2 and 3 enabled
+        break;
+    case 0b1001:
+        spi_counter_mask = 0b0111; // counter 0 and 3 enabled
+        break;
+    case 0b111:
+        spi_counter_mask = 0b1000; // counter 0, 1 and 2 enabled
+        break;
+    case 0b1110:
+        spi_counter_mask = 0b1001; // counter 1, 2 and 3 enabled
+        break;
+    case 0b1101:
+        spi_counter_mask = 0b1010; // counter 0, 2 and 3 enabled
+        break;
+    case 0b1011:
+        spi_counter_mask = 0b1011; // counter 0, 1 and 3 enabled
+        break;
+    case 0b1111:
+        spi_counter_mask = 0b1100; // counter 0, 1, 2 and 3 enabled
+        break;
+    default:
+        throw std::invalid_argument(
+            "Invalid counter mask: Only contiguous counters are enabled "
+            "(including wrap around)");
+    }
+
+    return spi_counter_mask;
+}
+
+uint32_t convertSPICounterMaskToCounterMask(const uint32_t spi_counter_mask) {
+
+    uint32_t counter_mask = 0;
+
+    uint8_t start_counter = spi_counter_mask & 0b11; // extract starting counter
+    uint8_t num_counters =
+        (spi_counter_mask >> 2) & 0b11; // extract number of counters
+
+    constexpr uint8_t max_counters = 4;
+
+    for (uint8_t i = 0; i < num_counters; ++i) {
+        counter_mask |= (1 << ((start_counter + i) % max_counters));
+    }
+
+    return counter_mask;
+}
\ No newline at end of file