diff --git a/dda_queue.c b/dda_queue.c
index e5a0cc5..b6a77e7 100644
--- a/dda_queue.c
+++ b/dda_queue.c
@@ -21,6 +21,10 @@ uint8_t queue_empty() {
 	return ((mb_tail == mb_head) && (movebuffer[mb_tail].live == 0))?255:0;
 }
 
+// -------------------------------------------------------
+// This is the one function called by the timer interrupt.
+// It calls a few other functions, though.
+// -------------------------------------------------------
 void queue_step() {
 	disableTimerInterrupt();
 
@@ -71,8 +75,9 @@ void enqueue(TARGET *t) {
 	mb_head = h;
 
 	#ifdef	XONXOFF
-	// if queue is full, stop transmission
-	if (queue_full())
+	// If the queue has only two slots remaining, stop transmission. More
+	// characters might come in until the stop takes effect.
+	if (((mb_tail - mb_head - 1) & (MOVEBUFFER_SIZE - 1)) < (MOVEBUFFER_SIZE - 2))
 		xoff();
 	#endif
 
@@ -105,8 +110,7 @@ void enqueue_temp_wait() {
 	mb_head = h;
 
 	#ifdef	XONXOFF
-	// if queue is full, stop transmission
-	if (queue_full())
+	if (((mb_tail - mb_head - 1) & (MOVEBUFFER_SIZE - 1)) < (MOVEBUFFER_SIZE - 2))
 		xoff();
 	#endif
 
diff --git a/serial.c b/serial.c
index d094ee1..4141dfb 100644
--- a/serial.c
+++ b/serial.c
@@ -48,8 +48,7 @@ volatile uint8_t txbuf[BUFSIZE];
 #ifdef	XONXOFF
 #define		FLOWFLAG_SEND_XON		1
 #define		FLOWFLAG_SEND_XOFF	2
-#define		FLOWFLAG_SENT_XON		4
-#define		FLOWFLAG_SENT_XOFF	8
+#define		FLOWFLAG_STATE_XON	4
 // initially, send an XON
 volatile uint8_t flowflags = FLOWFLAG_SEND_XON;
 #endif
@@ -88,11 +87,11 @@ ISR(USART_UDRE_vect)
 	#ifdef	XONXOFF
 	if (flowflags & FLOWFLAG_SEND_XON) {
 		UDR0 = ASCII_XON;
-		flowflags = (flowflags & ~FLOWFLAG_SEND_XON) | FLOWFLAG_SENT_XON;
+		flowflags = FLOWFLAG_STATE_XON;
 	}
 	else if (flowflags & FLOWFLAG_SEND_XOFF) {
 		UDR0 = ASCII_XOFF;
-		flowflags = (flowflags & ~FLOWFLAG_SEND_XOFF) | FLOWFLAG_SENT_XOFF;
+		flowflags = 0;
 	}
 	else
 	#endif
@@ -193,15 +192,26 @@ void serial_writestr_P(PGM_P data)
 
 #ifdef	XONXOFF
 void xon() {
-	if (flowflags & FLOWFLAG_SENT_XOFF)
+	// disable TX interrupt
+	UCSR0B &= ~MASK(UDRIE0);
+
+	if ((flowflags & FLOWFLAG_STATE_XON) == 0)
 		flowflags = FLOWFLAG_SEND_XON;
+	else
+		flowflags = FLOWFLAG_STATE_XON;	// purge a possible FLOWFLAG_SEND_XOFF
+
 	// enable TX interrupt so we can send this character
 	UCSR0B |= MASK(UDRIE0);
 }
 
 void xoff() {
-	flowflags = FLOWFLAG_SEND_XOFF;
-	// enable TX interrupt so we can send this character
+	UCSR0B &= ~MASK(UDRIE0);
+
+	if (flowflags & FLOWFLAG_STATE_XON)
+		flowflags = FLOWFLAG_SEND_XOFF | FLOWFLAG_STATE_XON;
+	else
+		flowflags = 0;
+
 	UCSR0B |= MASK(UDRIE0);
 }
 #endif