diff --git a/main.py b/main.py
index 3d43617..d872d33 100644
--- a/main.py
+++ b/main.py
@@ -9,18 +9,19 @@ def menu():
     print("2: Write reg hex value by name")
     print("3: Dump registers")
     print("4: Poll for packets")
+    print("5: Run Read+Write test")
     print("0: Quit")
 
 if __name__ == "__main__":
     spi = spidev.SpiDev()
     spi.open(0, 0)  # Bus 0, CE0 (Pin 24)
-    spi.max_speed_hz = 500000  # Safe start speed
+    spi.max_speed_hz = 8000000  # Safe start speed
     spi.mode = 0b00  # SPI mode 0
     stop = False
 
     print("Sending SRES (reset)...")
     spi.xfer2([SRES])
-    sleep(0.1)
+    sleep(0.5)
 
     print("Sending SNOP (no-op)...")
     status = spi.xfer2([SNOP])[0]
@@ -30,10 +31,12 @@ if __name__ == "__main__":
     version = read_register(spi, VERSION)
     print(f"CC2500 VERSION register: 0x{version:02X}")  
 
+    test_read_write_reg(spi)
+
     init_regs(spi)
     sleep(0.1)
 
-    test_read_write_reg(spi)
+
 
     reg_name = ""
     reg_hex_val = ""
@@ -59,10 +62,12 @@ if __name__ == "__main__":
             elif cmd == 3:
                 dump_regs(spi)
             elif cmd == 4:
-                while True:
-                    receive_packet(spi)
-                    sleep(0.2)
+                receive_packet(spi)
+            elif cmd == 5:
+                res = test_read_write_reg(spi, True)
+                print("Test result : "+str(res))
             else:
                 print("Invalid command")
     finally:
+        print("Closing SPI...")
         spi.close()
diff --git a/receive.py b/receive.py
index ddc732d..ea3e789 100644
--- a/receive.py
+++ b/receive.py
@@ -3,8 +3,8 @@ import time
 
 def burst_read(spi, addr, length):
     """Read multiple bytes"""
-    READ_SINGLE = get_addr["READ_SINGLE"]
-    READ_BURST = get_addr["READ_BURST"]
+    READ_SINGLE = get_addr("READ_SINGLE")
+    READ_BURST = get_addr("READ_BURST")
     return spi.xfer2([addr | READ_SINGLE | READ_BURST] + [0x00] * length)
 
 
@@ -18,26 +18,28 @@ def read_fifo(spi):
 
 
 def receive_packet(spi):
-    # Flush RX FIFO
     SFRX = get_addr("SFRX")
-    strobe(spi, SFRX)  # SFRX
-
-    SRX = get_addr( "SRX")
+    RXBYTES = get_addr("RXBYTES")
+    SRX = get_addr("SRX")
+    
+    # Flush RX FIFO
+    strobe(spi, SFRX)
+    time.sleep(0.5)  # 1 ms delay to allow flush
     # Go into RX mode
-    strobe(spi, SRX)  # SRX
+    strobe(spi, SRX) 
 
     # Wait for data (use GDO0 in real app)
-    time.sleep(0.5)
+    sleep(0.5)
 
     # Check RXBYTES
-    RXBYTES = get_addr( "RXBYTES")
-    rx_bytes = read_register(spi, RXBYTES) & 0x7F
-    if rx_bytes == 0:
-        print("No data received.")
-        return None
+    
+    timeout = time.time() + 2  # 2-second timeout
+    while time.time() < timeout:
+        rx_bytes = read_register(spi, RXBYTES) & 0x7F
+        if rx_bytes > 0:
+            data = read_fifo(spi)
+            print(f"Received: {data}")
+            return data
+        time.sleep(0.01)
 
-    # Read data
-    # RXFIFO = get_addr("RXFIFO")
-    data = read_fifo(spi)  # 0x3F = RX FIFO
-    print(f"Received: {data}")
-    return data
+print("Timeout: no data received.")
diff --git a/util.py b/util.py
index f4e5ea6..20754ed 100644
--- a/util.py
+++ b/util.py
@@ -45,17 +45,18 @@ def init_regs(spi):
         addr = get_addr(reg_name)
         write_reg(spi, addr, value)
 
-def test_read_write_reg(spi):
+def test_read_write_reg(spi, dbg=False):
     FIFOTHR = get_addr("FIFOTHR")
     initial_val = read_register(spi, FIFOTHR)
     test_value = initial_val + 1
     write_reg(spi, FIFOTHR, test_value)
     check = read_register(spi, FIFOTHR)
     write_reg(spi, FIFOTHR, initial_val)
-    if check != test_value:
+    if(dbg):
         print("initial value ", initial_val)
         print("test value ", test_value)
         print("check ",check)
+    elif (not dbg and (check != test_value)):
         raise Exception("Test Read+Write failed")
     return check == test_value