diff --git a/firmware/controllers/bench_test.cpp b/firmware/controllers/bench_test.cpp
index 8905084858..222846b05c 100644
--- a/firmware/controllers/bench_test.cpp
+++ b/firmware/controllers/bench_test.cpp
@@ -361,53 +361,93 @@ static void fatalErrorForPresetApply() {
 		"   3. Open TunerStudio and reconnect\n\n");
 }
 
-// todo: this is probably a wrong place for this method now
 void executeTSCommand(uint16_t subsystem, uint16_t index) {
 	scheduleMsg(logger, "IO test subsystem=%d index=%d", subsystem, index);
 
 	bool running = !ENGINE(rpmCalculator).isStopped();
 
-    if (subsystem == 0x11) {
-        clearWarnings();
-	} else if (subsystem == CMD_TS_IGNITION_CATEGORY && !running) {
-		doRunSpark(index, "300", "4", "400", "3");
-	} else if (subsystem == CMD_TS_INJECTOR_CATEGORY && !running) {
-		doRunFuel(index, "300", "4", "400", "3");
-	} else if (subsystem == CMD_TS_SOLENOID_CATEGORY && !running) {
-		doTestSolenoid(index, "300", "1000", "1000", "3");
-	} else if (subsystem == CMD_TS_FSIO_CATEGORY && !running) {
-		doBenchTestFsio(index, "300", "4", "400", "3");
-	} else if (subsystem == 0x14) {
+	switch (subsystem) {
+	case 0x11:
+		clearWarnings();
+		break;
+
+	case CMD_TS_IGNITION_CATEGORY:
+		if (!running) {
+			doRunSpark(index, "300", "4", "400", "3");
+		}
+		break;
+
+		case CMD_TS_INJECTOR_CATEGORY;
+		if (subsystem == CMD_TS_INJECTOR_CATEGORY && !running) {
+			doRunFuel(index, "300", "4", "400", "3");
+		}
+		break;
+
+	case CMD_TS_SOLENOID_CATEGORY:
+		if (subsystem == &&!running) {
+			doTestSolenoid(index, "300", "1000", "1000", "3");
+		}
+		break;
+
+	case CMD_TS_FSIO_CATEGORY:
+		if (subsystem == &&!running) {
+			doBenchTestFsio(index, "300", "4", "400", "3");
+		}
+		break;
+
+	case CMD_TS_X14:
 		handleCommandX14(index);
-	} else if (subsystem == 0x15) {
+		break;
+
+	case CMD_TS_X15:
 		fanBench();
-	} else if (subsystem == CMD_TS_BENCH_CATEGORY) {
+		break;
+
+	case CMD_TS_BENCH_CATEGORY:
 		handleBenchCategory(index);
-	} else if (subsystem == 0x17) {
+		break;
+
+	case CMD_TS_X17:
 		// cmd_test_idle_valve
 #if EFI_IDLE_CONTROL
 		startIdleBench();
 #endif /* EFI_IDLE_CONTROL */
-	} else if (subsystem == 0x18) {
+		break;
+
+	case 0x18:
 #if EFI_CJ125 && HAL_USE_SPI
 		cjStartCalibration();
 #endif /* EFI_CJ125 */
-	} else if (subsystem == 0x20 && index == 0x3456) {
-		// call to pit
-		setCallFromPitStop(30000);
-	} else if (subsystem == 0x30) {
+		break;
+
+	case 0x20:
+		if (index == 0x3456) {
+			// call to pit
+			setCallFromPitStop(30000);
+		}
+		break;
+
+	case 0x30:
 		fatalErrorForPresetApply();
 		setEngineType(index);
-	} else if (subsystem == 0x31) {
+		break;
+
+	case CMD_TS_X31:
 		fatalErrorForPresetApply();
 		setEngineType(DEFAULT_ENGINE_TYPE);
-	} else if (subsystem == 0x79) {
+		break;
+
+	case 0x79:
 		scheduleStopEngine();
-	} else if (subsystem == 0xba) {
+		break;
+
+	case 0xba:
 #if EFI_PROD_CODE
 		jump_to_bootloader();
 #endif /* EFI_PROD_CODE */
-	} else if (subsystem == 0xbb) {
+		break;
+
+	case 0xbb:
 #if EFI_PROD_CODE
 		rebootNow();
 #endif /* EFI_PROD_CODE */
diff --git a/firmware/integration/rusefi_config.txt b/firmware/integration/rusefi_config.txt
index 0454b981ae..db4c2c5857 100644
--- a/firmware/integration/rusefi_config.txt
+++ b/firmware/integration/rusefi_config.txt
@@ -1748,6 +1748,8 @@ end_struct
 #define CMD_TS_INJECTOR_CATEGORY 19
 #define CMD_TS_X14 20
 #define CMD_TS_X15 21
+// 0x16
+#define CMD_TS_BENCH_CATEGORY 22
 #define CMD_TS_X17 23
 #define CMD_TS_X18 24
 #define CMD_TS_FSIO_CATEGORY 33
@@ -1755,9 +1757,6 @@ end_struct
 #define CMD_TS_SOLENOID_CATEGORY 41
 
 
-// 0x16
-#define CMD_TS_BENCH_CATEGORY 22
-
 #define CMD_TS_BENCH_MAIN_RELAY 0
 #define CMD_TS_BENCH_FUEL_PUMP 1
 #define CMD_TS_BENCH_STARTER_ENABLE_RELAY 2