diff --git a/tb/sw/src/slvSvr.S b/tb/sw/src/slvSvr.S
index 510e9c5..dbda3ce 100644
--- a/tb/sw/src/slvSvr.S
+++ b/tb/sw/src/slvSvr.S
@@ -180,14 +180,17 @@ isr_tx1_deq_hdl:
 
 isr_rx0_enq_hdl:
 	lw		x17, RX0_ReadRxFifo(REG_TL2CDR_BASE)
-	bnez 	x21, 1f
+	bnez 	x21, 1f   //if x21 != 0 then goto 1f
 	srli	x16, x17, 20
-	li		x18, 8
-	add		x16, x16, x18
+	add		x16, x16, REG_4
+	mv		x20, x16
+	add		x16, x16, REG_4
 	sw		x16, TX1_WriteTxLen(REG_TL2CDR_BASE)
 1:
+	bge     x21, x20, 1f
 	add		x21, x21, REG_4
 	sw		x17, TX1_WriteTxFifo(REG_TL2CDR_BASE)
+1:
 	mret
 
 isr_rx1_enq_hdl: