power: reset: at91-poweroff: use one poweroff function for at91-poweroff

Use only one poweroff function and adapt it to work for both scenarios
(with LPDDR or not). The assignement of pm_power_off was moved at the
end of probe after all initializations are OK. This patch adapt the idea
from commit 4e018c1e9b05 ("power: reset: at91-poweroff: use only one
poweroff function").

Signed-off-by: Claudiu Beznea <claudiu.beznea@microchip.com>
Signed-off-by: Sebastian Reichel <sebastian.reichel@collabora.com>

diff --git a/drivers/power/reset/at91-poweroff.c b/drivers/power/reset/at91-poweroff.c
index fb2fc8f741a1e098d8fd6d1e88fe7608473a0adf..82533f4c72fcc1e7e90159100c12eb884e63e7a0 100644 (file)
--- a/drivers/power/reset/at91-poweroff.c
+++ b/drivers/power/reset/at91-poweroff.c
@@ -75,11 +75,6 @@ static void __init at91_wakeup_status(struct platform_device *pdev)
 }
 
 static void at91_poweroff(void)
-{
-       writel(AT91_SHDW_KEY | AT91_SHDW_SHDW, at91_shdwc_base + AT91_SHDW_CR);
-}
-
-static void at91_lpddr_poweroff(void)
 {
        asm volatile(
                /* Align to cache lines */
@@ -89,9 +84,11 @@ static void at91_lpddr_poweroff(void)
                "       ldr     r6, [%2, #" __stringify(AT91_SHDW_CR) "]\n\t"
 
                /* Power down SDRAM0 */
+               "       tst     %0, #0\n\t"
+               "       beq     1f\n\t"
                "       str     %1, [%0, #" __stringify(AT91_DDRSDRC_LPR) "]\n\t"
                /* Shutdown CPU */
-               "       str     %3, [%2, #" __stringify(AT91_SHDW_CR) "]\n\t"
+               "1:     str     %3, [%2, #" __stringify(AT91_SHDW_CR) "]\n\t"
 
                "       b       .\n\t"
                :
@@ -177,34 +174,42 @@ static int __init at91_poweroff_probe(struct platform_device *pdev)
        if (pdev->dev.of_node)
                at91_poweroff_dt_set_wakeup_mode(pdev);
 
-       pm_power_off = at91_poweroff;
-
        np = of_find_compatible_node(NULL, NULL, "atmel,sama5d3-ddramc");
-       if (!np)
-               return 0;
+       if (np) {
+               mpddrc_base = of_iomap(np, 0);
+               of_node_put(np);
 
-       mpddrc_base = of_iomap(np, 0);
-       of_node_put(np);
+               if (!mpddrc_base) {
+                       ret = -ENOMEM;
+                       goto clk_disable;
+               }
 
-       if (!mpddrc_base)
-               return 0;
+               ddr_type = readl(mpddrc_base + AT91_DDRSDRC_MDR) &
+                                AT91_DDRSDRC_MD;
+               if (ddr_type != AT91_DDRSDRC_MD_LPDDR2 &&
+                   ddr_type != AT91_DDRSDRC_MD_LPDDR3) {
+                       iounmap(mpddrc_base);
+                       mpddrc_base = NULL;
+               }
+       }
 
-       ddr_type = readl(mpddrc_base + AT91_DDRSDRC_MDR) & AT91_DDRSDRC_MD;
-       if ((ddr_type == AT91_DDRSDRC_MD_LPDDR2) ||
-           (ddr_type == AT91_DDRSDRC_MD_LPDDR3))
-               pm_power_off = at91_lpddr_poweroff;
-       else
-               iounmap(mpddrc_base);
+       pm_power_off = at91_poweroff;
 
        return 0;
+
+clk_disable:
+       clk_disable_unprepare(sclk);
+       return ret;
 }
 
 static int __exit at91_poweroff_remove(struct platform_device *pdev)
 {
-       if (pm_power_off == at91_poweroff ||
-           pm_power_off == at91_lpddr_poweroff)
+       if (pm_power_off == at91_poweroff)
                pm_power_off = NULL;
 
+       if (mpddrc_base)
+               iounmap(mpddrc_base);
+
        clk_disable_unprepare(sclk);
 
        return 0;