diff --git a/litex/soc/software/bios/main.c b/litex/soc/software/bios/main.c
index 4476aa372..87b4219df 100644
--- a/litex/soc/software/bios/main.c
+++ b/litex/soc/software/bios/main.c
@@ -175,48 +175,85 @@ __attribute__((__used__)) int main(int i, char **c)
 
         sdr_ok = 1;
 
-#ifdef CSR_HYPERRAM_BASE
-    /* HyperRAM Configuration */
+#ifdef CSR_HYPERRAM_BASE /* FIXME: Move to libbase/hyperram.h/c? */
+    /* Helper Functions */
 
-    uint16_t config_reg_0;
-    hyperram_config_write(7 << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
-	hyperram_reg_control_write(
-		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
-		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		2 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
-	);
-	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
-	config_reg_0 = hyperram_reg_rdata_read();
-	printf("Configuration Register 0 prev : %08lx\n", config_reg_0);
-	config_reg_0 &= ~(0b1    << 3); /* Enable Variable Latency */
-	config_reg_0 &= ~(0b1111 << 4);  /* Clear Initial Latency */
-    //config_reg_0 |=  (0b0010 << 4);  /* Initial Latency of 7 */
-	//config_reg_0 |=  (0b0001 << 4);  /* Initial Latency of 6 */
-	//config_reg_0 |=  (0b0000 << 4);  /* Initial Latency of 5 */
-	//config_reg_0 |=  (0b1111 << 4);  /* Initial Latency of 4 */
-	config_reg_0 |=  (0b1110 << 4);  /* Initial Latency of 3 */
-	printf("Configuration Register 0 update : %08lx\n", config_reg_0);
-	hyperram_reg_wdata_write(config_reg_0);
-	hyperram_reg_control_write(
-		1 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
-		0 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		2 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
-	);
-	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_WRITE_DONE_OFFSET)) == 0);
-	//hyperram_config_write(7 << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
-	//hyperram_config_write(6 << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
-	//hyperram_config_write(5 << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
-	//hyperram_config_write(4 << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
-	hyperram_config_write(3 << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
-	hyperram_reg_control_write(
-		0 << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
-		1 << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
-		2 << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
-	);
-	while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
-	config_reg_0 = hyperram_reg_rdata_read();
-	printf("Configuration Register 0 new : %08lx\n", config_reg_0);
+    printf("HyperRAM init...\n");
+    void hyperram_write_reg(uint16_t reg_addr, uint16_t data) {
+        /* Write data to the register */
+        hyperram_reg_wdata_write(data);
+        hyperram_reg_control_write(
+            1        << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
+            0        << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
+            reg_addr << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
+        );
+        /* Wait for write to complete */
+        while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_WRITE_DONE_OFFSET)) == 0);
+    }
 
+    uint16_t hyperram_read_reg(uint16_t reg_addr) {
+        /* Read data from the register */
+        hyperram_reg_control_write(
+            0        << CSR_HYPERRAM_REG_CONTROL_WRITE_OFFSET |
+            1        << CSR_HYPERRAM_REG_CONTROL_READ_OFFSET  |
+            reg_addr << CSR_HYPERRAM_REG_CONTROL_ADDR_OFFSET
+        );
+        /* Wait for read to complete */
+        while ((hyperram_reg_status_read() & (1 << CSR_HYPERRAM_REG_STATUS_READ_DONE_OFFSET)) == 0);
+        return hyperram_reg_rdata_read();
+    }
+
+    /* Configuration and Utility Functions */
+
+    uint16_t hyperram_get_core_latency_setting(uint32_t clk_freq) {
+        /* Raw clock latency settings for the HyperRAM core */
+        if (clk_freq <=  85000000) return 3; /* 3 Clock Latency */
+        if (clk_freq <= 104000000) return 4; /* 4 Clock Latency */
+        if (clk_freq <= 133000000) return 5; /* 5 Clock Latency */
+        if (clk_freq <= 166000000) return 6; /* 6 Clock Latency */
+        if (clk_freq <= 250000000) return 7; /* 7 Clock Latency */
+        return 7; /* Default to highest latency for safety */
+    }
+
+    uint16_t hyperram_get_chip_latency_setting(uint32_t clk_freq) {
+        /* LUT/Translated settings for the HyperRAM chip */
+        if (clk_freq <=  85000000) return 0b1110; /* 3 Clock Latency */
+        if (clk_freq <= 104000000) return 0b1111; /* 4 Clock Latency */
+        if (clk_freq <= 133000000) return 0b0000; /* 5 Clock Latency */
+        if (clk_freq <= 166000000) return 0b0001; /* 6 Clock Latency */
+        if (clk_freq <= 250000000) return 0b0010; /* 7 Clock Latency */
+        return 0b0010; /* Default to highest latency for safety */
+    }
+
+    void hyperram_configure_latency(void) {
+        uint16_t config_reg_0 = 0x8f2f;
+        uint16_t core_latency_setting;
+        uint16_t chip_latency_setting;
+
+        /* Compute Latency settings */
+        core_latency_setting = hyperram_get_core_latency_setting(CONFIG_CLOCK_FREQUENCY/4);
+        chip_latency_setting = hyperram_get_chip_latency_setting(CONFIG_CLOCK_FREQUENCY/4);
+
+        /* Write Latency to HyperRAM Core */
+        printf("HyperRAM Core Latency: %d CK (X1).\n", core_latency_setting);
+        hyperram_config_write(core_latency_setting << CSR_HYPERRAM_CONFIG_LATENCY_OFFSET);
+
+        /* Enable Variable Latency on HyperRAM Chip */
+        config_reg_0 &= ~(0b1 << 3); /* Enable Variable Latency */
+
+        /* Update Latency on HyperRAM Chip */
+        config_reg_0 &= ~(0b1111 << 4);
+        config_reg_0 |= chip_latency_setting << 4;
+
+        /* Write Configuration Register 0 to HyperRAM Chip */
+        hyperram_write_reg(2, config_reg_0);
+
+        /* Read current configuration */
+        config_reg_0 = hyperram_read_reg(2);
+        printf("HyperRAM Configuration Register 0: %08x\n", config_reg_0);
+    }
+    hyperram_configure_latency();
+    printf("\n");
 #endif
 
 #if defined(CSR_ETHMAC_BASE) || defined(MAIN_RAM_BASE) || defined(CSR_SPIFLASH_CORE_BASE)