diff --git a/tb/sw/src/halfLvdsMacSvr.c b/tb/sw/src/halfLvdsMacSvr.c
index 921ae1a..60394d0 100644
--- a/tb/sw/src/halfLvdsMacSvr.c
+++ b/tb/sw/src/halfLvdsMacSvr.c
@@ -77,32 +77,34 @@ uint32_t HLM_poll(uint32_t mode)
 		{
 			status = *HLM_status;
 
-			if (((tx_index * 4) < tx_pkt_len) && (!(status & HLM_TX0_FIFO_FULL)))
+			for (i = 0; i < HLM_TX_FIFO_LEN - HLM_TX0_FIFO_LEFT(status); i++)
 			{
-				*tx0_txFifo = tx_data_ptr[tx_index];
-				tx_index++;
-			}
-
-			status = *HLM_status;
-
-			if (((tx_index * 4) < tx_pkt_len) && (!(status & HLM_TX0_FIFO_FULL)))
-			{
-				*tx0_txFifo = tx_data_ptr[tx_index];
-				tx_index++;
-			}
-
-			if ((rx_start) && (status & HLM_RX1_VALID))
-			{
-				rx_data_ptr[rx_index] = *rx1_rxFifo;
-				if (rx_index == 0)
-					rx_pkt_len = (rx_data_ptr[rx_index] >> 20) + 4;
-
-				rx_index++;
-				if (rx_pkt_len <= (rx_index * 4))
+				if ((tx_index * 4) < tx_pkt_len)
 				{
-					rx_start = 0;
-					rx_index = 0;
+					*tx0_txFifo = tx_data_ptr[tx_index];
+					tx_index++;
+				}
+				else
 					break;
+			}
+
+			for (i = 0; i < HLM_RX1_FIFO_LEFT(status); i++)
+			{
+				//if ((rx_start) && (status & HLM_RX1_VALID))
+				if (rx_start)
+				{
+					rx_data_ptr[rx_index] = *rx1_rxFifo;
+					if (rx_index == 0)
+						rx_pkt_len = (rx_data_ptr[rx_index] >> 20) + 4;
+
+					rx_index++;
+					
+					if (rx_pkt_len <= (rx_index * 4))
+					{
+						rx_start = 0;
+						rx_index = 0;
+						break;
+					}
 				}
 			}
 
diff --git a/tb/sw/tl2cdr/tl2cdr.h b/tb/sw/tl2cdr/tl2cdr.h
index 53764d3..93e4258 100644
--- a/tb/sw/tl2cdr/tl2cdr.h
+++ b/tb/sw/tl2cdr/tl2cdr.h
@@ -47,6 +47,10 @@
 #define HLM_RX0_VALID           (1U<<6U)
 #define HLM_RX1_VALID           (1U<<7U)
 
+#define HLM_RX0_FIFO_LEFT(s)    (((s) >>        8) & 0x1f)
+#define HLM_RX1_FIFO_LEFT(s)    (((s) >> (8 +  5)) & 0x1f)
+#define HLM_TX0_FIFO_LEFT(s)    (((s) >> (8 + 10)) & 0x1f)
+#define HLM_TX1_FIFO_LEFT(s)    (((s) >> (8 + 15)) & 0x1f)
 
 #ifndef __ASSEMBLER__
 extern volatile uint32_t * const tx0_softReset;