diff --git a/CMakeLists.txt b/CMakeLists.txt
index 23a91e675..609c575e8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -252,7 +252,7 @@ set(FW_SOURCES
     SdFatUtil.cpp
     SdFile.cpp
     SdVolume.cpp
-    sm4.c
+    sm4.cpp
     sound.cpp
     speed_lookuptable.cpp
     spi.c
diff --git a/Firmware/sm4.c b/Firmware/sm4.cpp
similarity index 65%
rename from Firmware/sm4.c
rename to Firmware/sm4.cpp
index 04befb77c..009875f95 100644
--- a/Firmware/sm4.c
+++ b/Firmware/sm4.cpp
@@ -1,5 +1,4 @@
-//sm4.c - simple 4-axis stepper control
-
+//sm4.cpp - simple 4-axis stepper control
 #include "sm4.h"
 #include <avr/io.h>
 #include <avr/pgmspace.h>
@@ -49,28 +48,6 @@ sm4_update_pos_cb_t sm4_update_pos_cb = 0;
 
 sm4_calc_delay_cb_t sm4_calc_delay_cb = 0;
 
-uint16_t sm4_cpu_time = 0;
-
-
-uint8_t sm4_get_dir(uint8_t axis)
-{
-	switch (axis)
-	{
-#if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
-	case 0: return (PORTL & 2)?XDIR;
-	case 1: return (PORTL & 1)?YDIR;
-	case 2: return (PORTL & 4)?ZDIR;
-	case 3: return (PORTL & 64)?EDIR;
-#elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
-	case 0: return (PORTL & 1)?XDIR;
-	case 1: return (PORTL & 2)?YDIR;
-	case 2: return (PORTL & 4)?ZDIR;
-	case 3: return (PORTL & 64)?EDIR;
-#endif
-	}
-	return 0;
-}
-
 void sm4_set_dir(uint8_t axis, uint8_t dir)
 {
 	switch (axis)
@@ -90,30 +67,9 @@ void sm4_set_dir(uint8_t axis, uint8_t dir)
 	asm("nop");
 }
 
-uint8_t sm4_get_dir_bits(void)
-{
-    register uint8_t dir_bits = 0;
-    register uint8_t portL = PORTL;
-	//TODO -optimize in asm
-#if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
-	if (portL & 2) dir_bits |= 1;
-	if (portL & 1) dir_bits |= 2;
-	if (portL & 4) dir_bits |= 4;
-	if (portL & 64) dir_bits |= 8;
-	dir_bits ^= dir_mask;
-#elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
-	if (portL & 1) dir_bits |= 1;
-	if (portL & 2) dir_bits |= 2;
-	if (portL & 4) dir_bits |= 4;
-	if (portL & 64) dir_bits |= 8;
-	dir_bits ^= dir_mask;
-#endif
-	return dir_bits;
-}
-
 void sm4_set_dir_bits(uint8_t dir_bits)
 {
-    register uint8_t portL = PORTL;
+	uint8_t portL = PORTL;
 	portL &= 0xb8; //set direction bits to zero
 	//TODO -optimize in asm
 #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
@@ -139,7 +95,7 @@ void sm4_do_step(uint8_t axes_mask)
 #ifdef TMC2130_DEDGE_STEPPING
 	PINC = (axes_mask & 0x0f); // toggle step signals by mask
 #else
-    register uint8_t portC = PORTC & 0xf0;
+	uint8_t portC = PORTC & 0xf0;
 	PORTC = portC | (axes_mask & 0x0f); //set step signals by mask
 	asm("nop");
 	PORTC = portC; //set step signals to zero
@@ -148,60 +104,6 @@ void sm4_do_step(uint8_t axes_mask)
 #endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3) || (MOTHERBOARD == BOARD_EINSY_1_0a))
 }
 
-uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
-{
-	uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz) + ((uint32_t)de*de))) + 0.5);
-	uint16_t nd = dd;
-	uint16_t cx = dd;
-	uint16_t cy = dd;
-	uint16_t cz = dd;
-	uint16_t ce = dd;
-	uint16_t x = 0;
-	uint16_t y = 0;
-	uint16_t z = 0;
-	uint16_t e = 0;
-	while (nd)
-	{
-		if (sm4_stop_cb && (*sm4_stop_cb)()) break;
-		uint8_t sm = 0; //step mask
-		if (cx <= dx)
-		{
-			sm |= 1;
-			cx += dd;
-			x++;
-		}
-		if (cy <= dy)
-		{
-			sm |= 2;
-			cy += dd;
-			y++;
-		}
-		if (cz <= dz)
-		{
-			sm |= 4;
-			cz += dd;
-			z++;
-		}
-		if (ce <= de)
-		{
-			sm |= 8;
-			ce += dd;
-			e++;
-		}
-		cx -= dx;
-		cy -= dy;
-		cz -= dz;
-		ce -= de;
-		sm4_do_step(sm);
-		uint16_t delay = SM4_DEFDELAY;
-		if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
-		if (delay) delayMicroseconds(delay);
-		nd--;
-	}
-	if (sm4_update_pos_cb) (*sm4_update_pos_cb)(x, y, z, e);
-	return nd;
-}
-
 uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz){
 	uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5);
 	uint16_t nd = dd;
diff --git a/Firmware/sm4.h b/Firmware/sm4.h
index 9b5531548..da834f73d 100644
--- a/Firmware/sm4.h
+++ b/Firmware/sm4.h
@@ -5,12 +5,6 @@
 #include <inttypes.h>
 #include "config.h"
 
-
-#if defined(__cplusplus)
-extern "C" {
-#endif //defined(__cplusplus)
-
-
 // callback prototype for stop condition (return 0 - continue, return 1 - stop)
 typedef uint8_t (*sm4_stop_cb_t)();
 
@@ -30,16 +24,9 @@ extern sm4_update_pos_cb_t sm4_update_pos_cb;
 // callback pointer - calc_delay
 extern sm4_calc_delay_cb_t sm4_calc_delay_cb;
 
-
-// returns direction for single axis (0 - positive, 1 - negative)
-extern uint8_t sm4_get_dir(uint8_t axis);
-
 // set direction for single axis (0 - positive, 1 - negative)
 extern void sm4_set_dir(uint8_t axis, uint8_t dir);
 
-// returns direction of all axes as bitmask (0 - positive, 1 - negative)
-extern uint8_t sm4_get_dir_bits(void);
-
 // set direction for all axes as bitmask (0 - positive, 1 - negative)
 extern void sm4_set_dir_bits(uint8_t dir_bits);
 
@@ -47,11 +34,6 @@ extern void sm4_set_dir_bits(uint8_t dir_bits);
 extern void sm4_do_step(uint8_t axes_mask);
 
 // xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped)
-extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
 extern uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz);
 
-
-#if defined(__cplusplus)
-}
-#endif //defined(__cplusplus)
 #endif //_SM4_H