diff --git a/dda_queue.c b/dda_queue.c
index 9dae2c5..3054905 100644
--- a/dda_queue.c
+++ b/dda_queue.c
@@ -35,14 +35,13 @@ uint8_t	mb_tail = 0;
 /// The size does not need to be a power of 2 anymore!
 DDA BSS movebuffer[MOVEBUFFER_SIZE];
 
+/// Find the next DDA index after 'x', where 0 <= x < MOVEBUFFER_SIZE
+#define MB_NEXT(x) ((x) < MOVEBUFFER_SIZE - 1 ? (x) + 1 : 0)
+
 /// check if the queue is completely full
 uint8_t queue_full() {
 	MEMORY_BARRIER();
-	if (mb_tail > mb_head) {
-    return (mb_tail - mb_head - 1 == 0);
-	} else {
-    return (mb_tail + MOVEBUFFER_SIZE - mb_head - 1 == 0);
-	}
+  return MB_NEXT(mb_head) == mb_tail;
 }
 
 /// check if the queue is completely empty
@@ -107,8 +106,7 @@ void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) {
 	while (queue_full())
 		delay_us(100);
 
-	uint8_t h = mb_head + 1;
-	h &= (MOVEBUFFER_SIZE - 1);
+  uint8_t h = MB_NEXT(mb_head);
 
 	DDA* new_movebuffer = &(movebuffer[h]);
 
@@ -148,7 +146,7 @@ void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) {
 
 /// go to the next move.
 /// be aware that this is sometimes called from interrupt context, sometimes not.
-/// Note that if it is called from outside an interrupt it must not/can not by
+/// Note that if it is called from outside an interrupt it must not/can not
 /// be interrupted such that it can be re-entered from within an interrupt.
 /// The timer interrupt MUST be disabled on entry. This is ensured because
 /// the timer was disabled at the start of the ISR or else because the current
@@ -157,8 +155,7 @@ void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) {
 void next_move() {
 	while ((queue_empty() == 0) && (movebuffer[mb_tail].live == 0)) {
 		// next item
-		uint8_t t = mb_tail + 1;
-		t &= (MOVEBUFFER_SIZE - 1);
+    uint8_t t = MB_NEXT(mb_tail);
 		DDA* current_movebuffer = &movebuffer[t];
     // Tail must be set before calling timer_set(), as timer_set() reenables
     // the timer interrupt, potentially exposing mb_tail to the timer