cmp     memctrl, #AT91_MEMCTRL_DDRSDR
        bne     sdr_sr_enable
 
+       /* LPDDR1 --> force DDR2 mode during self-refresh */
+       ldr     tmp1, [sdramc, #AT91_DDRSDRC_MDR]
+       str     tmp1, .saved_sam9_mdr
+       bic     tmp1, tmp1, #~AT91_DDRSDRC_MD
+       cmp     tmp1, #AT91_DDRSDRC_MD_LOW_POWER_DDR
+       ldreq   tmp1, [sdramc, #AT91_DDRSDRC_MDR]
+       biceq   tmp1, tmp1, #AT91_DDRSDRC_MD
+       orreq   tmp1, tmp1, #AT91_DDRSDRC_MD_DDR2
+       streq   tmp1, [sdramc, #AT91_DDRSDRC_MDR]
+
        /* prepare for DDRAM self-refresh mode */
        ldr     tmp1, [sdramc, #AT91_DDRSDRC_LPR]
        str     tmp1, .saved_sam9_lpr
 
        /* figure out if we use the second ram controller */
        cmp     ramc1, #0
-       ldrne   tmp2, [ramc1, #AT91_DDRSDRC_LPR]
-       strne   tmp2, .saved_sam9_lpr1
-       bicne   tmp2, #AT91_DDRSDRC_LPCB
-       orrne   tmp2, #AT91_DDRSDRC_LPCB_SELF_REFRESH
+       beq     ddr_no_2nd_ctrl
+
+       ldr     tmp2, [ramc1, #AT91_DDRSDRC_MDR]
+       str     tmp2, .saved_sam9_mdr1
+       bic     tmp2, tmp2, #~AT91_DDRSDRC_MD
+       cmp     tmp2, #AT91_DDRSDRC_MD_LOW_POWER_DDR
+       ldreq   tmp2, [ramc1, #AT91_DDRSDRC_MDR]
+       biceq   tmp2, tmp2, #AT91_DDRSDRC_MD
+       orreq   tmp2, tmp2, #AT91_DDRSDRC_MD_DDR2
+       streq   tmp2, [ramc1, #AT91_DDRSDRC_MDR]
+
+       ldr     tmp2, [ramc1, #AT91_DDRSDRC_LPR]
+       str     tmp2, .saved_sam9_lpr1
+       bic     tmp2, #AT91_DDRSDRC_LPCB
+       orr     tmp2, #AT91_DDRSDRC_LPCB_SELF_REFRESH
 
        /* Enable DDRAM self-refresh mode */
+       str     tmp2, [ramc1, #AT91_DDRSDRC_LPR]
+ddr_no_2nd_ctrl:
        str     tmp1, [sdramc, #AT91_DDRSDRC_LPR]
-       strne   tmp2, [ramc1, #AT91_DDRSDRC_LPR]
 
        b       sdr_sr_done
 
         */
        cmp     memctrl, #AT91_MEMCTRL_DDRSDR
        bne     sdr_en_restore
+       /* Restore MDR in case of LPDDR1 */
+       ldr     tmp1, .saved_sam9_mdr
+       str     tmp1, [sdramc, #AT91_DDRSDRC_MDR]
        /* Restore LPR on AT91 with DDRAM */
        ldr     tmp1, .saved_sam9_lpr
        str     tmp1, [sdramc, #AT91_DDRSDRC_LPR]
 
        /* if we use the second ram controller */
        cmp     ramc1, #0
+       ldrne   tmp2, .saved_sam9_mdr1
+       strne   tmp2, [ramc1, #AT91_DDRSDRC_MDR]
        ldrne   tmp2, .saved_sam9_lpr1
        strne   tmp2, [ramc1, #AT91_DDRSDRC_LPR]
 
 .saved_sam9_lpr1:
        .word 0
 
+.saved_sam9_mdr:
+       .word 0
+
+.saved_sam9_mdr1:
+       .word 0
+
 ENTRY(at91_slow_clock_sz)
        .word .-at91_slow_clock