diff --git a/ports/ARM7-LPC214x/GCC/lpc214x_serial.c b/ports/ARM7-LPC214x/GCC/lpc214x_serial.c
index 296c725ee..523b93257 100644
--- a/ports/ARM7-LPC214x/GCC/lpc214x_serial.c
+++ b/ports/ARM7-LPC214x/GCC/lpc214x_serial.c
@@ -102,30 +102,39 @@ void UART1Irq(void) {
   ServeInterrupt(U1Base, &COM2);
 }
 
-/*
- * Invoked by the high driver when one or more bytes are inserted in the
- * output queue.
- */
-static void OutNotify1(void) {
-  UART *u = U0Base;
-
 #ifdef FIFO_PRELOAD
+static void preload(UART *u, FullDuplexDriver *com) {
+
   if (u->UART_LSR & LSR_THRE) {
     int i = FIFO_PRELOAD;
     do {
-      t_msg b = chOQGetI(&COM1.sd_oqueue);
+      t_msg b = chOQGetI(&com->sd_oqueue);
       if (b < Q_OK) {
-        chEvtSendI(&COM1.sd_oevent);
+        chEvtSendI(&com->sd_oevent);
         return;
       }
       u->UART_THR = b;
     } while (--i);
   }
+  u->UART_IER |= IER_THRE;
+}
+#endif
+
+/*
+ * Invoked by the high driver when one or more bytes are inserted in the
+ * output queue.
+ */
+static void OutNotify1(void) {
+#ifdef FIFO_PRELOAD
+
+  preload(U0Base, &COM1);
 #else
+  UART *u = U0Base;
+
   if (u->UART_LSR & LSR_THRE)
     u->UART_THR = chOQGetI(&COM1.sd_oqueue);
-#endif
   u->UART_IER |= IER_THRE;
+#endif
 }
 
 /*
@@ -133,25 +142,16 @@ static void OutNotify1(void) {
  * output queue.
  */
 static void OutNotify2(void) {
+#ifdef FIFO_PRELOAD
+
+  preload(U1Base, &COM2);
+#else
   UART *u = U1Base;
 
-#ifdef FIFO_PRELOAD
-  if (u->UART_LSR & LSR_THRE) {
-    int i = FIFO_PRELOAD;
-    do {
-      t_msg b = chOQGetI(&COM2.sd_oqueue);
-      if (b < Q_OK) {
-        chEvtSendI(&COM2.sd_oevent);
-        return;
-      }
-      u->UART_THR = b;
-    } while (--i);
-  }
-#else
   if (u->UART_LSR & LSR_THRE)
     u->UART_THR = chOQGetI(&COM2.sd_oqueue);
-#endif
   u->UART_IER |= IER_THRE;
+#endif
 }
 
 /*
diff --git a/readme.txt b/readme.txt
index 50182db84..4ca08701f 100644
--- a/readme.txt
+++ b/readme.txt
@@ -51,7 +51,8 @@ AVR-AT90CANx-GCC    - Port on AVR AT90CAN128, not complete yet.
   order to minimize the number of interrupts generated, it is possible to
   disable the feature and return to the old code which is a bit smaller, see
   the configuration parameters in ./ARM7-LPC214x/GCC/lpc214x_serial.h.
-- Some more work done on the AVR port, not tested yet.
+- Some more work done on the AVR port, it is almost complete but not tested
+  yet because my JTAG probe broke...
 
 *** 0.3.4 ***
 - Fixed a problem in chVTSetI().