diff --git a/uCNC/src/README.md b/uCNC/src/README.md
index 9cf6c8b5..550d435f 100644
--- a/uCNC/src/README.md
+++ b/uCNC/src/README.md
@@ -344,12 +344,12 @@ void cnc_alarm(int8_t code)
 	cnc_stop();
 	cnc_state.alarm = code;
 #ifdef ENABLE_IO_ALARM_DEBUG
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("LIMITS:");
+	grbl_protocol_print(MSG_START);
+	grbl_protocol_print("LIMITS:");
 	serial_print_int(io_alarm_limits);
-	grbl_protocol_string("|CONTROLS:");
+	grbl_protocol_print("|CONTROLS:");
 	serial_print_int(io_alarm_controls);
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_END);
 #endif
 
 	// we add our callback here
diff --git a/uCNC/src/cnc.c b/uCNC/src/cnc.c
index 2ef87e40..94780c7d 100644
--- a/uCNC/src/cnc.c
+++ b/uCNC/src/cnc.c
@@ -16,7 +16,7 @@
 	See the	GNU General Public License for more details.
 */
 
-#include <stdio.h>
+
 #include <stdlib.h>
 #include <string.h>
 #include <stdint.h>
@@ -163,7 +163,7 @@ void cnc_run(void)
 			if (grbl_stream_getc() == EOL)
 			{
 				grbl_protocol_feedback(MSG_FEEDBACK_1);
-				grbl_protocol_print(MSG_OK);
+				grbl_protocol_error(0);
 			}
 		}
 		cnc_dotasks();
@@ -215,13 +215,9 @@ uint8_t cnc_parse_cmd(void)
 		// runs any rt command in queue
 		// this catches for example a ?\n situation sent by some GUI like UGS
 		cnc_exec_rt_commands();
-		if (!error)
-		{
-			grbl_protocol_print(MSG_OK);
-		}
-		else
+		grbl_protocol_error(error);
+		if(error)
 		{
-			grbl_protocol_error(error);
 			itp_sync();
 			mc_sync_position();
 			parser_sync_position();
diff --git a/uCNC/src/core/interpolator.c b/uCNC/src/core/interpolator.c
index 199d9908..69381d20 100644
--- a/uCNC/src/core/interpolator.c
+++ b/uCNC/src/core/interpolator.c
@@ -18,7 +18,7 @@
 */
 
 #include "../cnc.h"
-#include <stdio.h>
+
 #include <string.h>
 #include <stdint.h>
 #include <math.h>
diff --git a/uCNC/src/core/motion_control.c b/uCNC/src/core/motion_control.c
index 50a32842..31ec2e9f 100644
--- a/uCNC/src/core/motion_control.c
+++ b/uCNC/src/core/motion_control.c
@@ -1027,24 +1027,24 @@ void mc_flush_pending_motion(void)
 
 void mc_print_hmap(void)
 {
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("HMAP start corner;");
+	grbl_protocol_print(MSG_START);
+	grbl_protocol_print("HMAP start corner;");
 	serial_print_flt(hmap_x);
-	serial_putc(';');
+	grbl_protocol_putc(';');
 	serial_print_flt(hmap_y);
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_END);
 
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("HMAP end corner;");
+	grbl_protocol_print(MSG_START);
+	grbl_protocol_print("HMAP end corner;");
 	serial_print_flt(hmap_x + hmap_x_offset);
-	serial_putc(';');
+	grbl_protocol_putc(';');
 	serial_print_flt(hmap_y + hmap_y_offset);
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_END);
 
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("HMAP control points;");
+	grbl_protocol_print(MSG_START);
+	grbl_protocol_print("HMAP control points;");
 	serial_print_int(H_MAPING_ARRAY_SIZE);
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_END);
 
 	// print map
 	for (uint8_t j = 0; j < H_MAPING_GRID_FACTOR; j++)
@@ -1053,14 +1053,14 @@ void mc_print_hmap(void)
 		{
 			uint8_t map = i + (H_MAPING_GRID_FACTOR * j);
 			float new_h = hmap_offsets[map];
-			grbl_protocol_string(MSG_START);
-			grbl_protocol_string("HMAP;");
+			grbl_protocol_print(MSG_START);
+			grbl_protocol_print("HMAP;");
 			serial_print_int(i);
-			serial_putc(';');
+			grbl_protocol_putc(';');
 			serial_print_int(j);
-			serial_putc(';');
+			grbl_protocol_putc(';');
 			serial_print_flt(new_h);
-			grbl_protocol_string(MSG_END);
+			grbl_protocol_print(MSG_END);
 		}
 	}
 }
diff --git a/uCNC/src/core/parser.c b/uCNC/src/core/parser.c
index 8d24f9df..47285270 100644
--- a/uCNC/src/core/parser.c
+++ b/uCNC/src/core/parser.c
@@ -22,7 +22,7 @@
 */
 
 #include "../cnc.h"
-#include <stdio.h>
+
 #include <stdint.h>
 #include <math.h>
 #include <string.h>
@@ -2600,7 +2600,7 @@ uint8_t parser_get_float(float *value)
 		return parser_get_expression(value);
 	}
 #endif
-	return print_itof(parser_get_next_preprocessed, NULL, value);
+	return print_atof(parser_get_next_preprocessed, NULL, value);
 }
 
 static uint8_t parser_get_token(uint8_t *word, float *value)
diff --git a/uCNC/src/hal/kinematics/kinematic_cartesian.c b/uCNC/src/hal/kinematics/kinematic_cartesian.c
index 0e27135f..43f35581 100644
--- a/uCNC/src/hal/kinematics/kinematic_cartesian.c
+++ b/uCNC/src/hal/kinematics/kinematic_cartesian.c
@@ -20,7 +20,7 @@
 #include "../../cnc.h"
 
 #if (KINEMATIC == KINEMATIC_CARTESIAN)
-#include <stdio.h>
+
 #include <math.h>
 
 void kinematics_init(void)
diff --git a/uCNC/src/hal/kinematics/kinematic_corexy.c b/uCNC/src/hal/kinematics/kinematic_corexy.c
index bcb2087b..21464c5b 100644
--- a/uCNC/src/hal/kinematics/kinematic_corexy.c
+++ b/uCNC/src/hal/kinematics/kinematic_corexy.c
@@ -20,7 +20,7 @@
 #include "../../cnc.h"
 
 #if (KINEMATIC == KINEMATIC_COREXY)
-#include <stdio.h>
+
 #include <math.h>
 
 #if (COREXY_AXIS == COREXY_AXIS_XZ || COREXY_AXIS == COREXY_AXIS_YZ)
diff --git a/uCNC/src/hal/kinematics/kinematic_delta.c b/uCNC/src/hal/kinematics/kinematic_delta.c
index 561644df..2e3b2506 100644
--- a/uCNC/src/hal/kinematics/kinematic_delta.c
+++ b/uCNC/src/hal/kinematics/kinematic_delta.c
@@ -20,7 +20,7 @@
 #include "../../cnc.h"
 
 #if (KINEMATIC == KINEMATIC_DELTA)
-#include <stdio.h>
+
 #include <stdint.h>
 #include <math.h>
 
diff --git a/uCNC/src/hal/kinematics/kinematic_linear_delta.c b/uCNC/src/hal/kinematics/kinematic_linear_delta.c
index ae8ed8bd..17de9779 100644
--- a/uCNC/src/hal/kinematics/kinematic_linear_delta.c
+++ b/uCNC/src/hal/kinematics/kinematic_linear_delta.c
@@ -20,7 +20,7 @@
 #include "../../cnc.h"
 
 #if (KINEMATIC == KINEMATIC_LINEAR_DELTA)
-#include <stdio.h>
+
 #include <math.h>
 
 #define STEPPER0_FACTX (cos(STEPPER0_ANGLE * DEG_RAD_MULT));
diff --git a/uCNC/src/hal/kinematics/kinematic_scara.c b/uCNC/src/hal/kinematics/kinematic_scara.c
index 87662327..c8ef870b 100644
--- a/uCNC/src/hal/kinematics/kinematic_scara.c
+++ b/uCNC/src/hal/kinematics/kinematic_scara.c
@@ -20,7 +20,7 @@
 #include "../../cnc.h"
 
 #if (KINEMATIC == KINEMATIC_SCARA)
-#include <stdio.h>
+
 #include <stdint.h>
 #include <math.h>
 
diff --git a/uCNC/src/hal/mcus/avr/mcumap_avr.h b/uCNC/src/hal/mcus/avr/mcumap_avr.h
index afd85bc2..8518e937 100644
--- a/uCNC/src/hal/mcus/avr/mcumap_avr.h
+++ b/uCNC/src/hal/mcus/avr/mcumap_avr.h
@@ -33,7 +33,7 @@ extern "C"
 */
 #include <math.h>
 #include <inttypes.h>
-#include <stdio.h>
+
 #include <string.h>
 #include <stdlib.h>
 #include <stdint.h>
diff --git a/uCNC/src/hal/mcus/virtual/mcu_virtual.c b/uCNC/src/hal/mcus/virtual/mcu_virtual.c
index cfc62b15..121a9368 100644
--- a/uCNC/src/hal/mcus/virtual/mcu_virtual.c
+++ b/uCNC/src/hal/mcus/virtual/mcu_virtual.c
@@ -17,7 +17,7 @@
 */
 #include "../../../cnc.h"
 #if (MCU == MCU_VIRTUAL_WIN)
-#include <stdio.h>
+
 #include <conio.h>
 #include <string.h>
 #include <stdint.h>
diff --git a/uCNC/src/hal/mcus/virtual/win_port.h b/uCNC/src/hal/mcus/virtual/win_port.h
index 815f54b5..b81a9077 100644
--- a/uCNC/src/hal/mcus/virtual/win_port.h
+++ b/uCNC/src/hal/mcus/virtual/win_port.h
@@ -6,7 +6,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include <stdio.h>
+
 
 typedef int(__attribute__((__stdcall__)) * available_delegate)(struct win_port_ *);
 typedef int(__attribute__((__stdcall__)) * read_delegate)(struct win_port_ *, uint8_t *buffer, size_t len);
diff --git a/uCNC/src/hal/tools/tools/plasma_thc.c b/uCNC/src/hal/tools/tools/plasma_thc.c
index b12f9ba7..5521b00d 100644
--- a/uCNC/src/hal/tools/tools/plasma_thc.c
+++ b/uCNC/src/hal/tools/tools/plasma_thc.c
@@ -19,7 +19,7 @@
 #include <math.h>
 #include <float.h>
 #include <stdint.h>
-#include <stdio.h>
+
 
 #include "../../../cnc.h"
 
@@ -432,43 +432,43 @@ bool plasma_grbl_protocol_status(void *args)
 {
 	uint8_t state = plasma_thc_state;
 
-	grbl_protocol_string("THC:");
+	grbl_protocol_print("THC:");
 
 	plasma_thc_extension_send_status();
 
 	if (CHECKFLAG(state, PLASMA_THC_ENABLED))
 	{
-		serial_putc('E');
+		grbl_protocol_putc('E');
 	}
 	else
 	{
-		serial_putc('*');
+		grbl_protocol_putc('*');
 	}
 	if (CHECKFLAG(state, PLASMA_THC_ACTIVE))
 	{
-		serial_putc('R');
+		grbl_protocol_putc('R');
 	}
 #if ASSERT_PIN(PLASMA_ON_OUTPUT)
 	if (io_get_output(PLASMA_ON_OUTPUT))
 	{
-		serial_putc('T');
+		grbl_protocol_putc('T');
 	}
 #endif
 	if (plasma_thc_arc_ok())
 	{
-		serial_putc('A');
+		grbl_protocol_putc('A');
 	}
 	if (plasma_thc_vad_active())
 	{
-		serial_putc('V');
+		grbl_protocol_putc('V');
 	}
 	if (plasma_thc_up())
 	{
-		serial_putc('U');
+		grbl_protocol_putc('U');
 	}
 	if (plasma_thc_down())
 	{
-		serial_putc('D');
+		grbl_protocol_putc('D');
 	}
 
 	return EVENT_CONTINUE;
diff --git a/uCNC/src/hal/tools/tools/vfd_modbus.c b/uCNC/src/hal/tools/tools/vfd_modbus.c
index 5c9a6f56..eb2d18d5 100644
--- a/uCNC/src/hal/tools/tools/vfd_modbus.c
+++ b/uCNC/src/hal/tools/tools/vfd_modbus.c
@@ -267,9 +267,9 @@ static bool modvfd_command(uint8_t *cmd, modbus_response_t *response)
 	}
 
 	vfd_state.connected = 0;
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("VFD COMMUNICATION FAILED");
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_START);
+	grbl_protocol_print("VFD COMMUNICATION FAILED");
+	grbl_protocol_print(MSG_END);
 #ifdef VFD_HOLD_ON_ERROR
 	cnc_call_rt_command(CMD_CODE_FEED_HOLD);
 #endif
diff --git a/uCNC/src/interface/grbl_interface.h b/uCNC/src/interface/grbl_interface.h
index 35aa291f..89586df9 100644
--- a/uCNC/src/interface/grbl_interface.h
+++ b/uCNC/src/interface/grbl_interface.h
@@ -158,9 +158,9 @@ extern "C"
 // formated messages
 #define STR_EOL "\r\n"
 #define MSG_EOL STR_EOL
-#define MSG_OK "ok" STR_EOL
-#define MSG_ERROR "error:%d" STR_EOL
-#define MSG_ALARM "ALARM:%d" STR_EOL
+#define MSG_OK "ok"
+#define MSG_ERROR "error:%d"
+#define MSG_ALARM "ALARM:%d" MSG_EOL
 #define MSG_ECHO "[echo:"
 #if EMULATE_GRBL_STARTUP == 0
 #define MSG_STARTUP_START "uCNC "
@@ -169,16 +169,16 @@ extern "C"
 #define MSG_STARTUP_START "Grbl "
 #define MSG_STARTUP_END " [uCNC v" CNC_VERSION " '$' for help]"
 #elif EMULATE_GRBL_STARTUP == 2
-#define MSG_STARTUP "Grbl 1.1f ['$' for help]" STR_EOL
+#define MSG_STARTUP "Grbl 1.1f ['$' for help]" MSG_EOL
 #endif
 #ifndef MSG_STARTUP
-#define MSG_STARTUP MSG_STARTUP_START CNC_MAJOR_MINOR_VERSION MSG_STARTUP_END STR_EOL
+#define MSG_STARTUP MSG_STARTUP_START CNC_MAJOR_MINOR_VERSION MSG_STARTUP_END MSG_EOL
 #endif
-#define MSG_HELP "[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $C $X $H ~ ! ? ctrl-x]" STR_EOL
+#define MSG_HELP "[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $C $X $H ~ ! ? ctrl-x]" MSG_EOL
 
 // Non query feedback messages
 #define MSG_START "[MSG:"
-#define MSG_END "]" STR_EOL
+#define MSG_END "]" MSG_EOL
 #define MSG_FEEDBACK MSG_START "%S" MSG_END
 #define MSG_FEEDBACK_PRINTF MSG_START "%s" MSG_END
 #define MSG_FEEDBACK_IP MSG_START "%M" MSG_END
diff --git a/uCNC/src/interface/grbl_protocol.c b/uCNC/src/interface/grbl_protocol.c
index dd0cc30b..0a8615f7 100644
--- a/uCNC/src/interface/grbl_protocol.c
+++ b/uCNC/src/interface/grbl_protocol.c
@@ -245,7 +245,9 @@ WEAK_EVENT_HANDLER(grbl_protocol_gcode_modes)
 
 void grbl_protocol_error(uint8_t error)
 {
-	grbl_protocol_printf(MSG_ERROR, error);
+	(error) ? grbl_protocol_printf(MSG_ERROR, error) : grbl_protocol_print(MSG_OK);
+	grbl_protocol_putc('\r');
+	grbl_protocol_putc('\n');
 }
 
 void grbl_protocol_alarm(int8_t alarm)
diff --git a/uCNC/src/interface/grbl_protocol.h b/uCNC/src/interface/grbl_protocol.h
index 4e54823a..76299695 100644
--- a/uCNC/src/interface/grbl_protocol.h
+++ b/uCNC/src/interface/grbl_protocol.h
@@ -32,7 +32,7 @@ extern "C"
 // protocol->stream callback
 // this is the base function call to output via stream
 #define grbl_protocol_putc grbl_stream_putc
-#define grbl_protocol_printf(fmt, ...) print_fmt(grbl_stream_putc, NULL, __romstr__(fmt), __VA_ARGS__)
+#define grbl_protocol_printf(fmt, ...) print_fmt(grbl_stream_putc, NULL, fmt, __VA_ARGS__)
 #define grbl_protocol_print(fmt) print_fmt(grbl_stream_putc, NULL, __romstr__(fmt))
 	void grbl_protocol_error(uint8_t error);
 	void grbl_protocol_alarm(int8_t alarm);
diff --git a/uCNC/src/interface/grbl_stream.h b/uCNC/src/interface/grbl_stream.h
index 4a889398..6a013e66 100644
--- a/uCNC/src/interface/grbl_stream.h
+++ b/uCNC/src/interface/grbl_stream.h
@@ -24,7 +24,7 @@ extern "C"
 {
 #endif
 
-#include <stdio.h>
+
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdarg.h>
diff --git a/uCNC/src/interface/print.c b/uCNC/src/interface/print.c
index b812e1e4..2c3b8545 100644
--- a/uCNC/src/interface/print.c
+++ b/uCNC/src/interface/print.c
@@ -96,14 +96,8 @@ static void print_byte(print_putc_cb cb, char **buffer_ref, const uint8_t *data,
 }
 #endif
 
-static void print_int(print_putc_cb cb, char **buffer_ref, int32_t num)
+static void print_int(print_putc_cb cb, char **buffer_ref, int32_t num, uint8_t padding)
 {
-	if (num == 0)
-	{
-		print_putc(cb, buffer_ref, '0');
-		return;
-	}
-
 	uint8_t buffer[11];
 	uint8_t i = 0;
 
@@ -120,6 +114,11 @@ static void print_int(print_putc_cb cb, char **buffer_ref, int32_t num)
 		buffer[i++] = digit;
 	}
 
+	while (i < padding--)
+	{
+		print_putc(cb, buffer_ref, '0');
+	}
+
 	do
 	{
 		i--;
@@ -127,15 +126,9 @@ static void print_int(print_putc_cb cb, char **buffer_ref, int32_t num)
 	} while (i);
 }
 
-void print_flt(print_putc_cb cb, char **buffer_ref, float num)
+static void FORCEINLINE print_flt(print_putc_cb cb, char **buffer_ref, float num)
 {
-	if (num < 0)
-	{
-		print_putc(cb, buffer_ref, '-');
-		num = -num;
-	}
-
-	int32_t interger = (uint32_t)floorf(num);
+	int32_t interger = (int32_t)floorf(num);
 	num -= interger;
 	uint32_t mult = (!g_settings.report_inches) ? 1000 : 10000;
 	num *= mult;
@@ -146,49 +139,31 @@ void print_flt(print_putc_cb cb, char **buffer_ref, float num)
 		digits = 0;
 	}
 
-	print_int(cb, buffer_ref, interger);
+	print_int(cb, buffer_ref, interger, 0);
 	print_putc(cb, buffer_ref, '.');
-	if (g_settings.report_inches)
-	{
-		if (digits < 1000)
-		{
-			print_putc(cb, buffer_ref, '0');
-		}
-	}
-
-	if (digits < 100)
-	{
-		print_putc(cb, buffer_ref, '0');
-	}
-
-	if (digits < 10)
-	{
-		print_putc(cb, buffer_ref, '0');
-	}
-
-	print_int(cb, buffer_ref, digits);
+	print_int(cb, buffer_ref, digits, (!g_settings.report_inches) ? 3 : 5);
 }
 
 #ifndef PRINT_DISABLE_FMT_IP
 void print_ip(print_putc_cb cb, char **buffer_ref, uint32_t ip)
 {
 	uint8_t *ptr = (uint8_t *)&ip;
-	print_int(cb, buffer_ref, (int32_t)ptr[3]);
+	print_int(cb, buffer_ref, (int32_t)ptr[3], 0);
 	print_putc(cb, buffer_ref, '.');
-	print_int(cb, buffer_ref, (int32_t)ptr[2]);
+	print_int(cb, buffer_ref, (int32_t)ptr[2], 0);
 	print_putc(cb, buffer_ref, '.');
-	print_int(cb, buffer_ref, (int32_t)ptr[1]);
+	print_int(cb, buffer_ref, (int32_t)ptr[1], 0);
 	print_putc(cb, buffer_ref, '.');
-	print_int(cb, buffer_ref, (int32_t)ptr[0]);
+	print_int(cb, buffer_ref, (int32_t)ptr[0], 0);
 }
 #endif
 
-static char itof_getc_dummy(bool peek)
-{
-	return 0;
-}
+// static char itof_getc_dummy(bool peek)
+// {
+// 	return 0;
+// }
 
-void print_fmtva(print_putc_cb cb, char *buffer, const char *fmt, va_list *args)
+static void FORCEINLINE print_fmtva(print_putc_cb cb, char *buffer, const char *fmt, va_list *args)
 {
 	char c = 0, cval = 0;
 	const char *s;
@@ -239,7 +214,7 @@ void print_fmtva(print_putc_cb cb, char *buffer, const char *fmt, va_list *args)
 			default:
 				if (c == '.' || (c >= '1' && c <= '9'))
 				{
-					if (print_itof(itof_getc_dummy, (const char **)&fmt, &f))
+					if (print_atof(NULL/*itof_getc_dummy*/, (const char **)&fmt, &f))
 					{
 						elems = (uint8_t)f;
 					}
@@ -310,12 +285,12 @@ void print_fmtva(print_putc_cb cb, char *buffer, const char *fmt, va_list *args)
 				{
 					switch (c)
 					{
+					case 'u':
+						i = ABS(i);
+						__FALL_THROUGH__
 					case 'd':
 					case 'i':
-						print_int(cb, buffer_ref, i);
-						break;
-					case 'u':
-						print_int(cb, buffer_ref, ABS(i));
+						print_int(cb, buffer_ref, i, 0);
 						break;
 #ifndef PRINT_DISABLE_FMT_HEX
 					case 'x':
@@ -401,76 +376,64 @@ void print_fmt(print_putc_cb cb, char *buffer, const char *fmt, ...)
 	va_end(args);
 }
 
-#define itof_peek(cb, buffer) ((!buffer) ? cb(true) : ((cb) ? rom_read_byte(*buffer) : **buffer))
-#define itof_get(cb, buffer) ((!buffer) ? cb(false) : ({ *buffer += 1; 0; }))
+#define atof_peek(cb, buffer) ((!buffer) ? cb(true) : rom_read_byte(*buffer)) /*((cb) ? rom_read_byte(*buffer) : **buffer))*/
+#define atof_get(cb, buffer) ((!buffer) ? cb(false) : ({ *buffer += 1; 0; }))
 
-uint8_t print_itof(print_read_input_cb cb, const char **buffer, float *value)
+uint8_t print_atof(print_read_input_cb cb, const char **buffer, float *value)
 {
 	uint32_t intval = 0;
 	uint8_t fpcount = 0;
-	uint8_t result = ITOF_NUMBER_UNDEF;
+	uint8_t result = ATOF_NUMBER_UNDEF;
 	float rhs = 0;
 
-	uint8_t c = (uint8_t)itof_peek(cb, buffer);
+	uint8_t c = (uint8_t)atof_peek(cb, buffer);
 
 	if (c == '-' || c == '+')
 	{
 		if (c == '-')
 		{
-			result |= ITOF_NUMBER_ISNEGATIVE;
+			result |= ATOF_NUMBER_ISNEGATIVE;
 		}
-		itof_get(cb, buffer);
-		c = (uint8_t)itof_peek(cb, buffer);
+		atof_get(cb, buffer);
+		c = (uint8_t)atof_peek(cb, buffer);
 	}
 
 	for (;;)
 	{
-		uint8_t digit = (uint8_t)c - 48;
-		if (digit <= 9)
+		switch (c)
 		{
-			intval = fast_int_mul10(intval) + digit;
-			if (fpcount)
+		case '.':
+			result |= ATOF_NUMBER_ISFLOAT;
+			break;
+		default:
+			c -= 48;
+			if (c <= 9)
 			{
-				fpcount++;
-			}
+				intval = fast_int_mul10(intval) + c;
+				if (result & ATOF_NUMBER_ISFLOAT)
+				{
+					fpcount++;
+				}
 
-			result |= ITOF_NUMBER_OK;
-		}
-		else if (c == '.' && !fpcount)
-		{
-			fpcount++;
-			result |= ITOF_NUMBER_ISFLOAT;
-		}
-		else
-		{
-			if (!(result & ITOF_NUMBER_OK))
+				result |= ATOF_NUMBER_OK;
+			}
+			else
 			{
-				return ITOF_NUMBER_UNDEF;
+				if ((result & ATOF_NUMBER_OK))
+				{
+					rhs = (float)intval;
+					while (fpcount--)
+					{
+						rhs *= 0.1f;
+					}
+
+					*value = (result & ATOF_NUMBER_ISNEGATIVE) ? -rhs : rhs;
+					return result;
+				}
 			}
 			break;
 		}
-
-		itof_get(cb, buffer);
-		c = (uint8_t)itof_peek(cb, buffer);
-	}
-
-	rhs = (float)intval;
-	if (fpcount)
-	{
-		fpcount--;
 	}
 
-	do
-	{
-		if (fpcount >= 1)
-		{
-			rhs *= 0.1f;
-			fpcount -= 1;
-		}
-
-	} while (fpcount != 0);
-
-	*value = (result & ITOF_NUMBER_ISNEGATIVE) ? -rhs : rhs;
-
-	return result;
+	return ATOF_NUMBER_UNDEF;
 }
diff --git a/uCNC/src/interface/print.h b/uCNC/src/interface/print.h
index 6bde11fa..d5365dbf 100644
--- a/uCNC/src/interface/print.h
+++ b/uCNC/src/interface/print.h
@@ -24,27 +24,27 @@ extern "C"
 {
 #endif
 
-#include <stdio.h>
+
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdarg.h>
 
-#define ITOF_NUMBER_UNDEF 0
-#define ITOF_NUMBER_OK 0x20
-#define ITOF_NUMBER_ISFLOAT 0x40
-#define ITOF_NUMBER_ISNEGATIVE 0x80
+#define ATOF_NUMBER_UNDEF 0
+#define ATOF_NUMBER_OK 0x20
+#define ATOF_NUMBER_ISFLOAT 0x40
+#define ATOF_NUMBER_ISNEGATIVE 0x80
 
 	// printing utils
 	typedef void (*print_putc_cb)(char);
-	void print_fmtva(print_putc_cb cb, char *buffer, const char *fmt, va_list *args);
+	// void print_fmtva(print_putc_cb cb, char *buffer, const char *fmt, va_list *args);
 	void print_fmt(print_putc_cb cb, char *buffer, const char *fmt, ...);
 	// scaning utilities
 	typedef unsigned char(*print_read_input_cb)(bool);
-	uint8_t print_itof(print_read_input_cb cb, const char **buffer, float *value);
+	uint8_t print_atof(print_read_input_cb cb, const char **buffer, float *value);
 
 // string helper functions
 #define str_sprintf(buffer, fmt, ...) print_fmt(NULL, buffer, fmt, __VA_ARGS__)
-#define str_itof(buffer, var) print_itof(NULL, buffer, var)
+#define str_itof(buffer, var) print_atof(NULL, buffer, var)
 
 #ifdef __cplusplus
 }
diff --git a/uCNC/src/interface/settings.c b/uCNC/src/interface/settings.c
index e631d51b..46a84173 100644
--- a/uCNC/src/interface/settings.c
+++ b/uCNC/src/interface/settings.c
@@ -811,7 +811,7 @@ bool settings_check_startup_gcode(uint16_t address)
 	return true;
 #else
 	grbl_protocol_putc(':');
-	grbl_protocol_print(MSG_OK);
+	grbl_protocol_error(0);
 	return false;
 #endif
 }
diff --git a/uCNC/src/interface/settings.h b/uCNC/src/interface/settings.h
index dfb6b9ff..6538c930 100644
--- a/uCNC/src/interface/settings.h
+++ b/uCNC/src/interface/settings.h
@@ -286,18 +286,18 @@ typedef uint16_t setting_offset_t;
 	{                                                                                    \
 		memset(var, 0, sizeof(char) * count);                                              \
 		settings_load(set##ID##_settings_address, (uint8_t *)var, sizeof(char) * count);   \
-		serial_putc('$');                                                                  \
-		serial_print_int(ID);                                                              \
-		serial_putc('=');                                                                  \
+		grbl_protocol_putc('$');                                                           \
+		grbl_protocol_printf("%ld", ID);                                                   \
+		grbl_protocol_putc('=');                                                           \
 		for (uint8_t i = 0; i < count; i++)                                                \
 		{                                                                                  \
 			char c = var[i];                                                                 \
 			if (c < 20 || c > 127)                                                           \
 			{                                                                                \
-				grbl_protocol_string(MSG_EOL);                                                 \
+				grbl_protocol_print(MSG_EOL);                                                  \
 				return EVENT_CONTINUE;                                                         \
 			}                                                                                \
-			serial_putc(c);                                                                  \
+			grbl_protocol_putc(c);                                                           \
 		}                                                                                  \
 		return EVENT_CONTINUE;                                                             \
 	}                                                                                    \
diff --git a/uCNC/src/modules/digimstep.c b/uCNC/src/modules/digimstep.c
index 408bcdb3..9ea56770 100644
--- a/uCNC/src/modules/digimstep.c
+++ b/uCNC/src/modules/digimstep.c
@@ -64,11 +64,11 @@ bool m351_exec(void *args)
 		itp_sync();
 		if (!ptr->cmd->words)
 		{
-			int32_t val = -1;
+			int8_t val = -1;
 			// if no additional args then print the
-			grbl_protocol_string("[MSTEPS:");
+			grbl_protocol_print("[MSTEPS:");
 			val = -1;
-			serial_putc('X');
+			grbl_protocol_putc('X');
 #if ASSERT_PIN(STEPPER0_MSTEP0)
 			val = io_get_output(STEPPER0_MSTEP0) ? 1 : 0;
 #endif
@@ -76,10 +76,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER0_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('Y');
+			grbl_protocol_putc('Y');
 #if ASSERT_PIN(STEPPER1_MSTEP0)
 			val = io_get_output(STEPPER1_MSTEP0) ? 1 : 0;
 #endif
@@ -87,10 +87,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER1_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('Z');
+			grbl_protocol_putc('Z');
 #if ASSERT_PIN(STEPPER2_MSTEP0)
 			val = io_get_output(STEPPER2_MSTEP0) ? 1 : 0;
 #endif
@@ -98,10 +98,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER2_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('A');
+			grbl_protocol_putc('A');
 #if ASSERT_PIN(STEPPER3_MSTEP0)
 			val = io_get_output(STEPPER3_MSTEP0) ? 1 : 0;
 #endif
@@ -109,10 +109,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER3_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('B');
+			grbl_protocol_putc('B');
 #if ASSERT_PIN(STEPPER4_MSTEP0)
 			val = io_get_output(STEPPER4_MSTEP0) ? 1 : 0;
 #endif
@@ -120,10 +120,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER4_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('C');
+			grbl_protocol_putc('C');
 #if ASSERT_PIN(STEPPER5_MSTEP0)
 			val = io_get_output(STEPPER5_MSTEP0) ? 1 : 0;
 #endif
@@ -131,10 +131,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER5_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('I');
+			grbl_protocol_putc('I');
 #if ASSERT_PIN(STEPPER6_MSTEP0)
 			val = io_get_output(STEPPER6_MSTEP0) ? 1 : 0;
 #endif
@@ -142,10 +142,10 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER6_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(',');
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(',');
 			val = -1;
-			serial_putc('J');
+			grbl_protocol_putc('J');
 #if ASSERT_PIN(STEPPER7_MSTEP0)
 			val = io_get_output(STEPPER7_MSTEP0) ? 1 : 0;
 #endif
@@ -153,9 +153,9 @@ bool m351_exec(void *args)
 			val = MAX(0, val);
 			val |= io_get_output(STEPPER7_MSTEP1) ? 2 : 0;
 #endif
-			serial_print_int(val);
-			serial_putc(']');
-			grbl_protocol_string(MSG_EOL);
+			grbl_protocol_printf("%d", val);
+			grbl_protocol_putc(']');
+			grbl_protocol_print(MSG_EOL);
 		}
 
 		if (CHECKFLAG(ptr->cmd->words, GCODE_WORD_X))
diff --git a/uCNC/src/modules/encoder.c b/uCNC/src/modules/encoder.c
index af8024ef..103c4f0c 100644
--- a/uCNC/src/modules/encoder.c
+++ b/uCNC/src/modules/encoder.c
@@ -181,12 +181,7 @@ int32_t encoder_get_position(uint8_t i)
 
 void encoder_print_values(void)
 {
-	for (uint8_t i = 0; i < ENCODERS; i++)
-	{
-		grbl_protocol_string("[EC:");
-		serial_print_int(encoder_get_position(i));
-		grbl_protocol_string(MSG_END);
-	}
+	grbl_protocol_printf("[EC:%"#ENCODERS"lld"MSG_END, encoders_pos);
 }
 
 void encoder_reset_position(uint8_t i, int32_t position)
diff --git a/uCNC/src/modules/file_system.c b/uCNC/src/modules/file_system.c
index 5c265ed5..69ff3930 100644
--- a/uCNC/src/modules/file_system.c
+++ b/uCNC/src/modules/file_system.c
@@ -291,24 +291,24 @@ static void fs_dir_list(void)
 	// if current working directory not initialized
 	if (!strlen(fs_cwd.full_name))
 	{
-		grbl_protocol_string("Available drives");
-		grbl_protocol_string(MSG_EOL);
+		grbl_protocol_print("Available drives");
+		grbl_protocol_print(MSG_EOL);
 		fs_t *drive = fs_default_drive;
 		while (drive)
 		{
-			grbl_protocol_string("<drive>\t");
-			serial_putc('/');
-			serial_putc(drive->drive);
-			grbl_protocol_string(MSG_EOL);
+			grbl_protocol_print("<drive>\t");
+			grbl_protocol_putc('/');
+			grbl_protocol_putc(drive->drive);
+			grbl_protocol_print(MSG_EOL);
 			drive = drive->next;
 		}
 		return;
 	}
 
 	// current dir
-	grbl_protocol_string("Index of /");
-	serial_print_str(fs_filename(&fs_cwd));
-	grbl_protocol_string(MSG_EOL);
+	grbl_protocol_print("Index of /");
+	grbl_protocol_printf("%s", fs_filename(&fs_cwd));
+	grbl_protocol_print(MSG_EOL);
 
 	fs_file_t *dir = fs_opendir(fs_cwd.full_name);
 
@@ -319,15 +319,15 @@ static void fs_dir_list(void)
 		{
 			if (finfo.is_dir)
 			{ /* It is a directory */
-				grbl_protocol_string("<dir>\t");
+				grbl_protocol_print("<dir>\t");
 			}
 			else
 			{ /* It is a file. */
-				grbl_protocol_string("     \t");
+				grbl_protocol_print("     \t");
 			}
 
-			serial_print_str(fs_filename(&finfo));
-			grbl_protocol_string(MSG_EOL);
+			grbl_protocol_printf("%s", fs_filename(&finfo));
+			grbl_protocol_print(MSG_EOL);
 		}
 
 		fs_close(dir);
@@ -341,24 +341,24 @@ void fs_cd(char *params)
 	{
 		if (dir->file_info.is_dir)
 		{
-			serial_print_str(fs_cwd.full_name);
-			serial_putc(' ');
-			serial_putc('>');
+			grbl_protocol_printf("%s", fs_cwd.full_name);
+			grbl_protocol_putc(' ');
+			grbl_protocol_putc('>');
 		}
 		else
 		{
-			serial_print_str(params);
+			grbl_protocol_printf("%s", params);
 			grbl_protocol_feedback(" is not a dir!");
 		}
 		fs_close(dir);
 	}
 	else if (strlen(fs_cwd.full_name))
 	{
-		serial_print_str(params);
+		grbl_protocol_printf("%s", params);
 		grbl_protocol_feedback("Dir not found!");
 	}
 
-	grbl_protocol_string(MSG_EOL);
+	grbl_protocol_print(MSG_EOL);
 }
 
 void fs_file_print(char *params)
@@ -375,7 +375,7 @@ void fs_file_print(char *params)
 				grbl_protocol_feedback("File read error!");
 				break;
 			}
-			serial_putc(c);
+			grbl_protocol_putc(c);
 		}
 
 		fs_close(fp);
@@ -387,7 +387,7 @@ void fs_file_print(char *params)
 		grbl_protocol_feedback("File not found!");
 	}
 
-	grbl_protocol_string(MSG_EOL);
+	grbl_protocol_print(MSG_EOL);
 }
 
 void fs_file_run(char *params)
@@ -411,10 +411,7 @@ void fs_file_run(char *params)
 	if (fp)
 	{
 		startline = MAX(1, startline);
-		grbl_protocol_string(MSG_START);
-		grbl_protocol_string("Running file from line - ");
-		serial_print_int(startline);
-		grbl_protocol_string(MSG_END);
+		grbl_protocol_printf(MSG_START"Running file from line - %llu" MSG_END, startline);
 #ifdef DECL_SERIAL_STREAM
 #ifdef ENABLE_MAIN_LOOP_MODULES
 		// prefill buffer
diff --git a/uCNC/src/modules/file_system.h b/uCNC/src/modules/file_system.h
index af5aaa4c..e2b55ca5 100644
--- a/uCNC/src/modules/file_system.h
+++ b/uCNC/src/modules/file_system.h
@@ -26,7 +26,7 @@ extern "C"
 
 #include "../cnc.h"
 #include <stddef.h>
-#include <stdio.h>
+
 #include "system_menu.h"
 
 #ifndef FS_PATH_NAME_MAX_LEN
diff --git a/uCNC/src/modules/modbus.c b/uCNC/src/modules/modbus.c
index de3ece29..e1fe2af5 100644
--- a/uCNC/src/modules/modbus.c
+++ b/uCNC/src/modules/modbus.c
@@ -64,11 +64,11 @@ void send_request(modbus_request_t request, uint8_t len, softuart_port_t *port)
 	request.crc = crc16(data, len);
 
 #ifdef ENABLE_MODBUS_VERBOSE
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("MODBUS-OUT");
+	grbl_protocol_print(MSG_START);
+	grbl_protocol_print("MODBUS-OUT");
 	serial_print_bytes((uint8_t *)&request, len);
 	serial_print_bytes((uint8_t *)&(request.crc), 2);
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_END);
 #endif
 
 	while (len--)
@@ -100,10 +100,12 @@ bool read_response(modbus_response_t *response, uint8_t len, softuart_port_t *po
 	} while ((c >= 0) && (count < len));
 
 #ifdef ENABLE_MODBUS_VERBOSE
-	grbl_protocol_string(MSG_START);
-	grbl_protocol_string("MODBUS-IN");
-	serial_print_bytes((uint8_t *)response, count);
-	grbl_protocol_string(MSG_END);
+	grbl_protocol_print(MSG_START "MODBUS-IN");
+	for (uint8_t i = 0; i < count; i++)
+	{
+		grbl_protocol_printf("%x", response[i]);
+	}
+	grbl_protocol_print(MSG_END);
 #endif
 	// minimum message length
 	if (count < 6)
diff --git a/uCNC/src/modules/pid.h b/uCNC/src/modules/pid.h
index f188cac7..ba0108ad 100644
--- a/uCNC/src/modules/pid.h
+++ b/uCNC/src/modules/pid.h
@@ -28,7 +28,7 @@ extern "C"
 #include <stdint.h>
 #include <stdbool.h>
 #include <float.h>
-#include <stdio.h>
+
 
 #define HZ_TO_MS(hz) (1000 / (hz))