uart_writestr("DRAM init... ");
        struct gramCtx ctx;
-       gram_init(&ctx, (void*)0x10000000, (void*)0x00009000, (void*)0x00008000);
+       struct gramProfile profile = {
+               .mode_registers = {
+                       0x320, 0x6, 0x200, 0x0
+               },
+               .rdly_p0 = 2,
+               .rdly_p1 = 2,
+       };
+       gram_init(&ctx, &profile, (void*)0x10000000, (void*)0x00009000, (void*)0x00008000);
        uart_writestr("done\n");
 
        uart_writestr("DRAM test... \n");
        while (1);
 
        return 0;
-}
\ No newline at end of file
+}
 
        size_t i;
        int delay, miss = 0;
 
-       struct gramProfile profile = {0,0};
+       struct gramProfile profile = {
+               .mode_registers = {
+                       0x320, 0x6, 0x200, 0x0
+               },
+               .rdly_p0 = 2,
+               .rdly_p1 = 2,
+       };
 
        if (argc < 3) {
                fprintf(stderr, "Usage: %s port baudrate\n", argv[0]);
        ctx.user_data = &serial_port;
 
        printf("gram init... ");
-       gram_init(&ctx, (void*)0x10000000, (void*)0x00009000, (void*)0x00008000);
-       gram_load_calibration(&ctx, &profile);
+       gram_init(&ctx, &profile, (void*)0x10000000, (void*)0x00009000, (void*)0x00008000);
        printf("done\n");
 
        srand(time(NULL));
 
 
 int main(void) {
        struct gramCtx ctx;
-       int err = gram_init(&ctx, 0x10000000, 0x00006000, 0x00005000);
+       struct gramProfile profile {
+               .mode_registers = {
+                       0x320, 0x6, 0x200, 0
+               },
+               .rdly_p0 = 0,
+               .rdly_p1 = 0
+       };
+       int err = gram_init(&ctx, &profile, 0x10000000, 0x00006000, 0x00005000);
 
        return 0;
 }
 
        GRAM_ERR_MEMTEST,
 };
 
-enum GramWidth {
-       GRAM_8B,
-       GRAM_32B,
-};
-
 struct gramCoreRegs;
 struct gramPHYRegs;
 struct gramCtx {
        uint32_t mode_registers[4];
 };
 
-extern __attribute__((visibility ("default"))) int gram_init(struct gramCtx *ctx, void *ddr_base, void *core_base, void *phy_base);
-extern __attribute__((visibility ("default"))) int gram_memtest(struct gramCtx *ctx, size_t length, enum GramWidth width);
-extern __attribute__((visibility ("default"))) int gram_generate_calibration(struct gramCtx *ctx, struct gramProfile *profile);
-extern __attribute__((visibility ("default"))) void gram_load_calibration(struct gramCtx *ctx, struct gramProfile *profile);
+extern __attribute__((visibility ("default"))) int gram_init(struct gramCtx *ctx, const struct gramProfile *profile, void *ddr_base, void *core_base, void *phy_base);
+extern __attribute__((visibility ("default"))) int gram_generate_calibration(const struct gramCtx *ctx, struct gramProfile *profile);
+extern __attribute__((visibility ("default"))) void gram_load_calibration(const struct gramCtx *ctx, const struct gramProfile *profile);
 
-extern __attribute__((visibility ("default"))) void gram_reset_burstdet(struct gramCtx *ctx);
-extern __attribute__((visibility ("default"))) bool gram_read_burstdet(struct gramCtx *ctx, int phase);
+extern __attribute__((visibility ("default"))) void gram_reset_burstdet(const struct gramCtx *ctx);
+extern __attribute__((visibility ("default"))) bool gram_read_burstdet(const struct gramCtx *ctx, int phase);
 
 #ifdef GRAM_RW_FUNC
 extern uint32_t gram_read(struct gramCtx *ctx, void *addr);
 
 #include "dfii.h"
 #include "helpers.h"
 
-static void set_rdly(struct gramCtx *ctx, unsigned int phase, unsigned int rdly) {
+static void set_rdly(const struct gramCtx *ctx, unsigned int phase, unsigned int rdly) {
 #ifdef GRAM_RW_FUNC
        if (phase == 0) {
                gram_write(ctx, &(ctx->phy->rdly_p0), rdly);
 #endif
 }
 
-void gram_reset_burstdet(struct gramCtx *ctx) {
+static inline uint32_t lsfr(uint32_t in) {
+       return (in >> 1) ^ (uint32_t)(0 - (in & 1u) & 0xd0000001);
+}
+
+static bool memtest(uint32_t *start, uint32_t *stop, int delay) {
+       const uint32_t seed = 0x6C616D62;
+       uint32_t rand = seed;
+       volatile uint32_t *ptr;
+       int i;
+
+       for (ptr = start; ptr < stop; ptr++) {
+               *ptr = rand;
+               rand = lsfr(rand);
+       }
+
+       for (i = 0; i < delay; i++) {
+               __asm__("nop");
+       }
+
+       rand = seed;
+       for (ptr = start; ptr < stop; ptr++) {
+               if (*ptr != rand) {
+                       return false;
+               }
+               rand = lsfr(rand);
+       }
+
+       return true;
+}
+
+void gram_reset_burstdet(const struct gramCtx *ctx) {
 #ifdef GRAM_RW_FUNC
        gram_write(ctx, &(ctx->phy->burstdet), 0);
 #else
 #endif
 }
 
-bool gram_read_burstdet(struct gramCtx *ctx, int phase) {
+bool gram_read_burstdet(const struct gramCtx *ctx, int phase) {
 #ifdef GRAM_RW_FUNC
        return gram_read(ctx, &(ctx->phy->burstdet)) & (1 << phase);
 #else
 #endif
 }
 
-int gram_generate_calibration(struct gramCtx *ctx, struct gramProfile *profile) {
-       uint32_t refval[8];
-       size_t i, j, k;
-       int score;
+int gram_generate_calibration(const struct gramCtx *ctx, struct gramProfile *profile) {
+       unsigned char rdly_p0, rdly_p1;
+       unsigned char min_rdly_p0, min_rdly_p1;
+       unsigned char max_rdly_p0, max_rdly_p1;
 
        dfii_setsw(ctx, true);
 
-       for (i = 0; i < 8; i++) {
-               for (j = 0; j < 8; j++) {
-                       /* Generating test pattern */
-                       for (k = 0; k < 8; k++) {
-                               refval[k] = (0xABCD1234*i*j) & 0xFFFFFFFF;
-                       }
+       // Find minimal rdly
+       for (rdly_p0 = 0; rdly_p0 < 8; rdly_p0++) {
+               for (rdly_p1 = 0; rdly_p1 < 8; rdly_p1++) {
 
-                       /* Writing to RAM */
+               }
+       }
 
-                       /* Reading from RAM */
-                       score = 0;
-                       for (k = 0; k < 8; k++) {
+       // Find maximal rdly
+       for (rdly_p0 = 0; rdly_p0 < 8; rdly_p0++) {
+               for (rdly_p1 = 0; rdly_p1 < 8; rdly_p1++) {
 
-                       }
                }
        }
 
        dfii_setsw(ctx, false);
 
+       // Store average rdly value
+       profile->rdly_p0 = (min_rdly_p0+max_rdly_p0)/2;
+       profile->rdly_p1 = (min_rdly_p1+max_rdly_p1)/2;
+
        return 0;
 }
 
-void gram_load_calibration(struct gramCtx *ctx, struct gramProfile *profile) {
+void gram_load_calibration(const struct gramCtx *ctx, const struct gramProfile *profile) {
        dfii_setsw(ctx, true);
        set_rdly(ctx, 0, profile->rdly_p0);
        set_rdly(ctx, 1, profile->rdly_p1);
 
 #include "dfii.h"
 #include "helpers.h"
 
-static void dfii_setcontrol(struct gramCtx *ctx, uint8_t val) {
+static void dfii_setcontrol(const struct gramCtx *ctx, uint8_t val) {
 #ifdef GRAM_RW_FUNC
        gram_write(ctx, &(ctx->core->control), val);
 #else
 #endif
 }
 
-void dfii_setsw(struct gramCtx *ctx, bool software_control) {
+void dfii_setsw(const struct gramCtx *ctx, bool software_control) {
        if (software_control) {
                dfii_setcontrol(ctx, DFII_CONTROL_CKE|DFII_CONTROL_ODT);
        } else {
        }
 }
 
-void dfii_set_p0_address(struct gramCtx *ctx, uint32_t val) {
+void dfii_set_p0_address(const struct gramCtx *ctx, uint32_t val) {
 #ifdef GRAM_RW_FUNC
        gram_write(ctx, &(ctx->core->phases[0].address), val);
 #else
 #endif
 }
 
-void dfii_set_p0_baddress(struct gramCtx *ctx, uint32_t val) {
+void dfii_set_p0_baddress(const struct gramCtx *ctx, uint32_t val) {
 #ifdef GRAM_RW_FUNC
        gram_write(ctx, &(ctx->core->phases[0].baddress), val);
 #else
 #endif
 }
 
-void dfii_p0_command(struct gramCtx *ctx, uint32_t cmd) {
+void dfii_p0_command(const struct gramCtx *ctx, uint32_t cmd) {
 #ifdef GRAM_RW_FUNC
        gram_write(ctx, &(ctx->core->phases[0].command), cmd);
        gram_write(ctx, &(ctx->core->phases[0].command_issue), 1);
 }
 
 /* Set MRx register */
-static void dfii_set_mr(struct gramCtx *ctx, uint8_t mr, uint16_t val) {
+static void dfii_set_mr(const struct gramCtx *ctx, uint8_t mr, uint16_t val) {
        dfii_set_p0_address(ctx, val);
        dfii_set_p0_baddress(ctx, mr);
        dfii_p0_command(ctx, DFII_COMMAND_RAS|DFII_COMMAND_CAS|DFII_COMMAND_WE|DFII_COMMAND_CS);
 }
 
-/* TODO: those values are hardcoded for ECPIX-5's RAM */
-/* Should add the capacity to generate MRx from RAM spec */
-void dfii_initseq(struct gramCtx *ctx) {
+#define MR0_DLL_RESET (1 << 8)
+void dfii_initseq(const struct gramCtx *ctx, const struct gramProfile *profile) {
        /* Release reset */
        dfii_set_p0_address(ctx, 0x0);
        dfii_set_p0_baddress(ctx, 0);
        cdelay(10000);
 
        /* Load Mode Register 2, CWL=5 */
-       dfii_set_mr(ctx, 2, 0x200);
+       dfii_set_mr(ctx, 2, profile->mode_registers[2]);
 
        /* Load Mode Register 3 */
-       dfii_set_mr(ctx, 3, 0x0);
+       dfii_set_mr(ctx, 3, profile->mode_registers[3]);
 
        /* Load Mode Register 1 */
-       dfii_set_mr(ctx, 1, 0x6);
+       dfii_set_mr(ctx, 1, profile->mode_registers[1]);
 
        /* Load Mode Register 0, CL=6, BL=8 */
-       dfii_set_mr(ctx, 0, 0x320);
-       cdelay(100);
-       dfii_set_mr(ctx, 0, 0x220);
+       dfii_set_mr(ctx, 0, profile->mode_registers[0]);
+    if (profile->mode_registers[0] & MR0_DLL_RESET) {
+          cdelay(100);
+          dfii_set_mr(ctx, 0, profile->mode_registers[0] & ~MR0_DLL_RESET);
+    }
        cdelay(600);
 
        /* ZQ Calibration */
 
 #define DFII_COMMAND_RAS (1 << 3)
 #define DFII_COMMAND_WRDATA (1 << 4)
 
-void dfii_setsw(struct gramCtx *ctx, bool software_control);
-void dfii_initseq(struct gramCtx *ctx);
-void dfii_set_p0_address(struct gramCtx *ctx, uint32_t val);
-void dfii_set_p0_baddress(struct gramCtx *ctx, uint32_t val);
-void dfii_p0_command(struct gramCtx *ctx, uint32_t cmd);
+void dfii_setsw(const struct gramCtx *ctx, bool software_control);
+void dfii_initseq(const struct gramCtx *ctx, const struct gramProfile *profile);
+void dfii_set_p0_address(const struct gramCtx *ctx, uint32_t val);
+void dfii_set_p0_baddress(const struct gramCtx *ctx, uint32_t val);
+void dfii_p0_command(const struct gramCtx *ctx, uint32_t cmd);
 
 #endif /* DFII_H */
 
 #include <gram.h>
 #include "dfii.h"
 
-int gram_init(struct gramCtx *ctx, void *ddr_base, void *core_base, void *phy_base) {
+int gram_init(struct gramCtx *ctx, const struct gramProfile *profile, void *ddr_base, void *core_base, void *phy_base) {
        ctx->ddr_base = ddr_base;
        ctx->core = core_base;
        ctx->phy = phy_base;
 
        dfii_setsw(ctx, true);
-       dfii_initseq(ctx);
+       dfii_initseq(ctx, profile);
        dfii_setsw(ctx, false);
 }