diff --git a/dda_queue.c b/dda_queue.c
index b6a77e7..fc81c6b 100644
--- a/dda_queue.c
+++ b/dda_queue.c
@@ -91,9 +91,6 @@ void enqueue_temp_wait() {
 		delay(WAITING_DELAY);
 
 	uint8_t h = mb_head + 1;
-// 	h++;
-// 	if (h == MOVEBUFFER_SIZE)
-// 		h = 0;
 	h &= (MOVEBUFFER_SIZE - 1);
 
 	// wait for temp flag
@@ -122,9 +119,6 @@ void next_move() {
 	if (queue_empty() == 0) {
 		// next item
 		uint8_t t = mb_tail + 1;
-// 		t++;
-// 		if (t == MOVEBUFFER_SIZE)
-// 			t = 0;
 		t &= (MOVEBUFFER_SIZE - 1);
 		dda_start(&movebuffer[t]);
 		mb_tail = t;
diff --git a/gcode.c b/gcode.c
index b3063d7..bce6e46 100644
--- a/gcode.c
+++ b/gcode.c
@@ -627,7 +627,19 @@ void process_gcode_command(GCODE_COMMAND *gcmd) {
 				break;
 			// M113- extruder PWM
 			// M114- report XYZEF to host
-
+			case 114:
+				serial_writestr_P(PSTR("X:"));
+				serwrite_int32(current_position.X);
+				serial_writestr_P(PSTR(",Y:"));
+				serwrite_int32(current_position.Y);
+				serial_writestr_P(PSTR(",Z:"));
+				serwrite_int32(current_position.Z);
+				serial_writestr_P(PSTR(",E:"));
+				serwrite_int32(current_position.E);
+				serial_writestr_P(PSTR(",F:"));
+				serwrite_int32(current_position.F);
+				serial_writechar('\n');
+			 	break;
 			// M130- heater P factor
 			case 130:
 				if (gcmd->seen_S)
@@ -664,7 +676,6 @@ void process_gcode_command(GCODE_COMMAND *gcmd) {
 				debug_flags |= DEBUG_ECHO;
 				serial_writestr_P(PSTR("Echo on\n"));
 				break;
-			#endif
 
 			// DEBUG: return current position
 			case 250:
@@ -724,6 +735,7 @@ void process_gcode_command(GCODE_COMMAND *gcmd) {
 				serial_writechar('\n');
 				(*(volatile uint8_t *)(gcmd->S)) = gcmd->P;
 				break;
+			#endif /* DEBUG */
 			// unknown mcode: spit an error
 			default:
 				serial_writestr_P(PSTR("E: Bad M-code "));
diff --git a/serial.c b/serial.c
index 732fb2f..1e3c566 100644
--- a/serial.c
+++ b/serial.c
@@ -58,12 +58,9 @@ void serial_init()
 {
 #if BAUD > 38401
 	UCSR0A = MASK(U2X0);
-#else
-	UCSR0A = 0;
-#endif
-#if BAUD > 38401
 	UBRR0 = (((F_CPU / 8) / BAUD) - 0.5);
 #else
+	UCSR0A = 0;
 	UBRR0 = (((F_CPU / 16) / BAUD) - 0.5);
 #endif
 
@@ -124,11 +121,6 @@ uint8_t serial_popchar()
 	Write
 */
 
-// uint8_t serial_txchars()
-// {
-// 	return buf_canwrite(tx);
-// }
-
 void serial_writechar(uint8_t data)
 {
 	// check if interrupts are enabled
@@ -159,7 +151,6 @@ void serial_writestr(uint8_t *data)
 {
 	uint8_t i = 0, r;
 	// yes, this is *supposed* to be assignment rather than comparison, so we break when r is assigned zero
-// 	for (uint8_t r; (r = data[i]); i++)
 	while ((r = data[i++]))
 		serial_writechar(r);
 }
@@ -187,7 +178,7 @@ void serial_writestr_P(PGM_P data)
 {
 	uint8_t r, i = 0;
 	// yes, this is *supposed* to be assignment rather than comparison, so we break when r is assigned zero
-	for ( ; (r = pgm_read_byte(&data[i])); i++)
+	while ((r = pgm_read_byte(&data[i++])))
 		serial_writechar(r);
 }
 
diff --git a/timer.c b/timer.c
index 3b18343..3b71b7d 100644
--- a/timer.c
+++ b/timer.c
@@ -30,7 +30,7 @@ void setupTimerInterrupt()
 	setTimer(F_CPU / 100);
 }
 
-// the following are all from reprap project 5D firmware
+// the following are all from reprap project 5D firmware with some modification to reduce redundancy
 
 uint8_t getTimerResolution(const uint32_t delay)
 {
@@ -54,10 +54,7 @@ uint8_t getTimerResolution(const uint32_t delay)
 		return 4;
 	// our slowest speed at our lowest resolution ((2^16-1) * 64 usecs = 4194240 usecs (4.19 seconds max))
 	// range: 7.812Khz max - 0.119hz min
-// 	else if (delay <= 67107840L)
-// 		return 5;
 	//its really slow... hopefully we can just get by with super slow.
-// 	else
 	return 5;
 }
 
@@ -106,11 +103,6 @@ void setTimer(uint32_t delay)
 	// we also then calculate the timer ceiling required. (ie what the counter counts to)
 	// the result is the timer counts up to the appropriate time and then fires an interrupt.
 
-	// Actual ticks are 0.0625 us, so multiply delay by 16
-
-	// convert to ticks
-// 	delay = delay US;
-
 	setTimerCeiling(getTimerCeiling(delay));
 	setTimerResolution(getTimerResolution(delay));
 }
@@ -156,8 +148,8 @@ void delayMicrosecondsInterruptible(uint16_t us)
 
   // busy wait
   __asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles
-"brne 1b" :
-  "=w" (us) :
-  "0" (us) // 2 cycles
+		"brne 1b" :
+  	"=w" (us) :
+  	"0" (us) // 2 cycles
     );
 }