diff --git a/litex/soc/cores/hyperbus.py b/litex/soc/cores/hyperbus.py
index 528deade2..45286fc6a 100644
--- a/litex/soc/cores/hyperbus.py
+++ b/litex/soc/cores/hyperbus.py
@@ -176,7 +176,8 @@ class HyperRAM(LiteXModule):
 
         # Latency count starts from the middle of the command (thus the -4). In fixed latency mode
         # (default), latency is 2 x Latency count. We have 4 x sys_clk per RAM clock:
-        latency_cycles = (latency * 2 * 4) - 4
+        latency_cycles_0 = latency * 4
+        latency_cycles_1 = latency * 4 - 4
 
         # Bus Latch --------------------------------------------------------------------------------
         bus_adr   = Signal(32)
@@ -217,7 +218,11 @@ class HyperRAM(LiteXModule):
                     NextValue(sr, Cat(Signal(40), self.reg_write_data[8:])),
                     NextState("REG-WRITE-0")
                 ).Else(
-                    NextState("WAIT-LATENCY")
+                    If(rwds.i,
+                        NextState("WAIT-LATENCY-0")
+                    ).Else(
+                        NextState("WAIT-LATENCY-1")
+                    )
                 )
             )
         )
@@ -246,11 +251,19 @@ class HyperRAM(LiteXModule):
                 NextState("IDLE")
             )
         )
-        fsm.act("WAIT-LATENCY",
+        fsm.act("WAIT-LATENCY-0",
             # Set CSn.
             cs.eq(1),
             # Wait for Latency cycles...
-            If(cycles == (latency_cycles - 1),
+            If(cycles == (latency_cycles_0 - 1),
+                NextState("WAIT-LATENCY-1")
+            )
+        )
+        fsm.act("WAIT-LATENCY-1",
+            # Set CSn.
+            cs.eq(1),
+            # Wait for Latency cycles...
+            If(cycles == (latency_cycles_1 - 1),
                 # Latch Bus.
                 bus_latch.eq(1),
                 # Early Write Ack (to allow bursting).
diff --git a/litex/soc/software/bios/main.c b/litex/soc/software/bios/main.c
index 0293f6acd..f0db259bd 100644
--- a/litex/soc/software/bios/main.c
+++ b/litex/soc/software/bios/main.c
@@ -181,7 +181,7 @@ __attribute__((__used__)) int main(int i, char **c)
 	hyperram_reg_control_write(
 		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
 		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		0 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
+		0 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
 	);
 	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
 	printf("Identification Register 0 : %08lx\n", hyperram_reg_rdata_read());
@@ -189,7 +189,7 @@ __attribute__((__used__)) int main(int i, char **c)
 	hyperram_reg_control_write(
 		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
 		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		1 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
+		1 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
 	);
 	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
 	printf("Identification Register 1 : %08lx\n", hyperram_reg_rdata_read());
@@ -197,7 +197,7 @@ __attribute__((__used__)) int main(int i, char **c)
 	hyperram_reg_control_write(
 		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
 		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		2 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
+		2 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
 	);
 	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
 	printf("Configuration Register 0  : %08lx\n", hyperram_reg_rdata_read());
@@ -206,7 +206,7 @@ __attribute__((__used__)) int main(int i, char **c)
 	hyperram_reg_control_write(
 		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
 		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		3 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
+		3 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
 	);
 	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
 	printf("Configuration Register 1  : %08lx\n", hyperram_reg_rdata_read());
@@ -217,30 +217,18 @@ __attribute__((__used__)) int main(int i, char **c)
 	hyperram_reg_control_write(
 		1 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
 		0 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		2 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
+		2 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
 	);
 	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_WRITE_DONE_OFFSET)) == 0);
 
-	printf("reg_control: %x\n", hyperram_reg_control_read());
-	printf("reg_status:  %x\n", hyperram_reg_status_read());
-	printf("reg_debug:   %x\n", hyperram_reg_debug_read());
-
 	hyperram_reg_control_write(
 		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
 		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		2 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
+		2 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
 	);
 	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
 	printf("Configuration Register 0 after update : %08lx\n", hyperram_reg_rdata_read());
 
-	hyperram_reg_control_write(
-		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
-		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		3 << CSR_HYPERRAM_REG_CONTROL_REG_OFFSET
-	);
-	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
-	printf("Configuration Register 1  : %08lx\n", hyperram_reg_rdata_read());
-
 
 #if defined(CSR_ETHMAC_BASE) || defined(MAIN_RAM_BASE) || defined(CSR_SPIFLASH_CORE_BASE)
     printf("--========== \e[1mInitialization\e[0m ============--\n");