11 files changed, 41 insertions, 10 deletions

diff --git a/arch/arm/mach-omap1/pm.h b/arch/arm/mach-omap1/pm.h
index 56a647986ae9..cd926dcb5e7f 100644
--- a/arch/arm/mach-omap1/pm.h
+++ b/arch/arm/mach-omap1/pm.h
@@ -123,9 +123,9 @@ extern void allow_idle_sleep(void);
 extern void omap1_pm_idle(void);
 extern void omap1_pm_suspend(void);
 
-extern void omap7xx_cpu_suspend(unsigned short, unsigned short);
-extern void omap1510_cpu_suspend(unsigned short, unsigned short);
-extern void omap1610_cpu_suspend(unsigned short, unsigned short);
+extern void omap7xx_cpu_suspend(unsigned long, unsigned long);
+extern void omap1510_cpu_suspend(unsigned long, unsigned long);
+extern void omap1610_cpu_suspend(unsigned long, unsigned long);
 extern void omap7xx_idle_loop_suspend(void);
 extern void omap1510_idle_loop_suspend(void);
 extern void omap1610_idle_loop_suspend(void);
diff --git a/arch/arm/mach-omap1/sleep.S b/arch/arm/mach-omap1/sleep.S
index ef771ce8b030..c875bdc902c5 100644
--- a/arch/arm/mach-omap1/sleep.S
+++ b/arch/arm/mach-omap1/sleep.S
@@ -58,6 +58,7 @@
  */
 
 #if defined(CONFIG_ARCH_OMAP730) || defined(CONFIG_ARCH_OMAP850)
+        .align  3
 ENTRY(omap7xx_cpu_suspend)
 
         @ save registers on stack
@@ -137,6 +138,7 @@ ENTRY(omap7xx_cpu_suspend_sz)
 #endif /* CONFIG_ARCH_OMAP730 || CONFIG_ARCH_OMAP850 */
 
 #ifdef CONFIG_ARCH_OMAP15XX
+        .align  3
 ENTRY(omap1510_cpu_suspend)
 
         @ save registers on stack
@@ -211,6 +213,7 @@ ENTRY(omap1510_cpu_suspend_sz)
 #endif /* CONFIG_ARCH_OMAP15XX */
 
 #if defined(CONFIG_ARCH_OMAP16XX)
+        .align  3
 ENTRY(omap1610_cpu_suspend)
 
         @ save registers on stack
diff --git a/arch/arm/mach-omap1/sram.S b/arch/arm/mach-omap1/sram.S
index 7724e520d07c..692587d07ea5 100644
--- a/arch/arm/mach-omap1/sram.S
+++ b/arch/arm/mach-omap1/sram.S
@@ -18,6 +18,7 @@
 /*
  * Reprograms ULPD and CKCTL.
  */
+        .align  3
 ENTRY(omap1_sram_reprogram_clock)
         stmfd   sp!, {r0 - r12, lr}             @ save registers on stack
 
diff --git a/arch/arm/mach-omap2/pm.h b/arch/arm/mach-omap2/pm.h
index 1c1b0ab5b978..39580e6060e8 100644
--- a/arch/arm/mach-omap2/pm.h
+++ b/arch/arm/mach-omap2/pm.h
@@ -92,7 +92,7 @@ extern void omap24xx_idle_loop_suspend(void);
 extern void omap24xx_cpu_suspend(u32 dll_ctrl, void __iomem *sdrc_dlla_ctrl,
                                         void __iomem *sdrc_power);
 extern void omap34xx_cpu_suspend(u32 *addr, int save_state);
-extern void save_secure_ram_context(u32 *addr);
+extern int save_secure_ram_context(u32 *addr);
 extern void omap3_save_scratchpad_contents(void);
 
 extern unsigned int omap24xx_idle_loop_suspend_sz;
diff --git a/arch/arm/mach-omap2/sleep24xx.S b/arch/arm/mach-omap2/sleep24xx.S
index c7780cc8d919..b5071a47ec39 100644
--- a/arch/arm/mach-omap2/sleep24xx.S
+++ b/arch/arm/mach-omap2/sleep24xx.S
@@ -47,6 +47,7 @@
  * Note: This code get's copied to internal SRAM at boot. When the OMAP
  *       wakes up it continues execution at the point it went to sleep.
  */
+        .align  3
 ENTRY(omap24xx_idle_loop_suspend)
         stmfd   sp!, {r0, lr}           @ save registers on stack
         mov     r0, #0                  @ clear for mcr setup
@@ -82,6 +83,7 @@ ENTRY(omap24xx_idle_loop_suspend_sz)
  * The DLL load value is not kept in RETENTION or OFF.  It needs to be restored
  * at wake
  */
+        .align  3
 ENTRY(omap24xx_cpu_suspend)
         stmfd   sp!, {r0 - r12, lr}     @ save registers on stack
         mov     r3, #0x0                @ clear for mcr call
diff --git a/arch/arm/mach-omap2/sleep34xx.S b/arch/arm/mach-omap2/sleep34xx.S
index 98d8232808b8..951a0be66cf7 100644
--- a/arch/arm/mach-omap2/sleep34xx.S
+++ b/arch/arm/mach-omap2/sleep34xx.S
@@ -118,6 +118,7 @@ ENTRY(enable_omap3630_toggle_l2_on_restore)
 
         .text
 /* Function to call rom code to save secure ram context */
+        .align  3
 ENTRY(save_secure_ram_context)
         stmfd   sp!, {r1-r12, lr}       @ save registers on stack
         adr     r3, api_params          @ r3 points to parameters
@@ -169,6 +170,7 @@ ENTRY(save_secure_ram_context_sz)
  *   depending on the low power mode (non-OFF vs OFF modes),
  *   cf. 'Resume path for xxx mode' comments.
  */
+        .align  3
 ENTRY(omap34xx_cpu_suspend)
         stmfd   sp!, {r0-r12, lr}       @ save registers on stack
 
diff --git a/arch/arm/mach-omap2/sram242x.S b/arch/arm/mach-omap2/sram242x.S
index 055310cc77de..ff9b9dbcb30e 100644
--- a/arch/arm/mach-omap2/sram242x.S
+++ b/arch/arm/mach-omap2/sram242x.S
@@ -39,6 +39,7 @@
 
         .text
 
+        .align  3
 ENTRY(omap242x_sram_ddr_init)
         stmfd   sp!, {r0 - r12, lr}     @ save registers on stack
 
@@ -143,6 +144,7 @@ ENTRY(omap242x_sram_ddr_init_sz)
  * r0 = [PRCM_FULL | PRCM_HALF] r1 = SDRC_DLLA_CTRL value r2 = [DDR | SDR]
  * PRCM_FULL = 2, PRCM_HALF = 1, DDR = 1, SDR = 0
  */
+        .align  3
 ENTRY(omap242x_sram_reprogram_sdrc)
         stmfd   sp!, {r0 - r10, lr}     @ save registers on stack
         mov     r3, #0x0                @ clear for mrc call
@@ -238,6 +240,7 @@ ENTRY(omap242x_sram_reprogram_sdrc_sz)
 /*
  * Set dividers and pll. Also recalculate DLL value for DDR and unlock mode.
  */
+        .align  3
 ENTRY(omap242x_sram_set_prcm)
         stmfd   sp!, {r0-r12, lr}       @ regs to stack
         adr     r4, pbegin              @ addr of preload start
diff --git a/arch/arm/mach-omap2/sram243x.S b/arch/arm/mach-omap2/sram243x.S
index f9007580aea3..76730209fa0e 100644
--- a/arch/arm/mach-omap2/sram243x.S
+++ b/arch/arm/mach-omap2/sram243x.S
@@ -39,6 +39,7 @@
 
         .text
 
+        .align  3
 ENTRY(omap243x_sram_ddr_init)
         stmfd   sp!, {r0 - r12, lr}     @ save registers on stack
 
@@ -143,6 +144,7 @@ ENTRY(omap243x_sram_ddr_init_sz)
  * r0 = [PRCM_FULL | PRCM_HALF] r1 = SDRC_DLLA_CTRL value r2 = [DDR | SDR]
  * PRCM_FULL = 2, PRCM_HALF = 1, DDR = 1, SDR = 0
  */
+        .align  3
 ENTRY(omap243x_sram_reprogram_sdrc)
         stmfd   sp!, {r0 - r10, lr}     @ save registers on stack
         mov     r3, #0x0                @ clear for mrc call
@@ -238,6 +240,7 @@ ENTRY(omap243x_sram_reprogram_sdrc_sz)
 /*
  * Set dividers and pll. Also recalculate DLL value for DDR and unlock mode.
  */
+        .align  3
 ENTRY(omap243x_sram_set_prcm)
         stmfd   sp!, {r0-r12, lr}       @ regs to stack
         adr     r4, pbegin              @ addr of preload start
diff --git a/arch/arm/mach-omap2/sram34xx.S b/arch/arm/mach-omap2/sram34xx.S
index 7f893a29d500..25011ca2145d 100644
--- a/arch/arm/mach-omap2/sram34xx.S
+++ b/arch/arm/mach-omap2/sram34xx.S
@@ -111,6 +111,7 @@
  * since it will cause the ARM MMU to attempt to walk the page tables.
  * These crashes may be intermittent.
  */
+        .align  3
 ENTRY(omap3_sram_configure_core_dpll)
         stmfd   sp!, {r1-r12, lr}       @ store regs to stack
 
diff --git a/arch/arm/plat-omap/include/plat/sram.h b/arch/arm/plat-omap/include/plat/sram.h
index 9967d5e855c7..f500fc34d065 100644
--- a/arch/arm/plat-omap/include/plat/sram.h
+++ b/arch/arm/plat-omap/include/plat/sram.h
@@ -12,7 +12,19 @@
 #define __ARCH_ARM_OMAP_SRAM_H
 
 #ifndef __ASSEMBLY__
-extern void * omap_sram_push(void * start, unsigned long size);
+#include <asm/fncpy.h>
+
+extern void *omap_sram_push_address(unsigned long size);
+
+/* Macro to push a function to the internal SRAM, using the fncpy API */
+#define omap_sram_push(funcp, size) ({                          \
+        typeof(&(funcp)) _res = NULL;                           \
+        void *_sram_address = omap_sram_push_address(size);     \
+        if (_sram_address)                                      \
+                _res = fncpy(_sram_address, &(funcp), size);    \
+        _res;                                                   \
+})
+
 extern void omap_sram_reprogram_clock(u32 dpllctl, u32 ckctl);
 
 extern void omap2_sram_ddr_init(u32 *slow_dll_ctrl, u32 fast_dll_ctrl,
diff --git a/arch/arm/plat-omap/sram.c b/arch/arm/plat-omap/sram.c
index e26e50487d60..68fcc7dc56e7 100644
--- a/arch/arm/plat-omap/sram.c
+++ b/arch/arm/plat-omap/sram.c
@@ -242,7 +242,14 @@ static void __init omap_map_sram(void)
                omap_sram_size - SRAM_BOOTLOADER_SZ);
 }
 
-void * omap_sram_push(void * start, unsigned long size)
+/*
+ * Memory allocator for SRAM: calculates the new ceiling address
+ * for pushing a function using the fncpy API.
+ *
+ * Note that fncpy requires the returned address to be aligned
+ * to an 8-byte boundary.
+ */
+void *omap_sram_push_address(unsigned long size)
 {
         if (size > (omap_sram_ceil - (omap_sram_base + SRAM_BOOTLOADER_SZ))) {
                 printk(KERN_ERR "Not enough space in SRAM\n");
@@ -250,10 +257,7 @@ void * omap_sram_push(void * start, unsigned long size)
         }
 
         omap_sram_ceil -= size;
-        omap_sram_ceil = ROUND_DOWN(omap_sram_ceil, sizeof(void *));
-        memcpy((void *)omap_sram_ceil, start, size);
-        flush_icache_range((unsigned long)omap_sram_ceil,
-                (unsigned long)(omap_sram_ceil + size));
+        omap_sram_ceil = ROUND_DOWN(omap_sram_ceil, FNCPY_ALIGN);
 
         return (void *)omap_sram_ceil;
 }