ARM: omap[34]: Thumb-2 compatibility fixes

index 6ae937a..4ee6aec 100644
@@ -45,5 +45,5 @@ hold:	ldr	r12,=0x103
 	 * should now contain the SVC stack for this core
 	 */
 	b	secondary_startup
-END(omap_secondary_startup)
+ENDPROC(omap_secondary_startup)
 

index 1980dc3..e69d37d 100644
@@ -29,7 +29,7 @@ ENTRY(omap_smc1)
 	dsb
 	smc	#0
 	ldmfd   sp!, {r2-r12, pc}
-END(omap_smc1)
+ENDPROC(omap_smc1)
 
 ENTRY(omap_modify_auxcoreboot0)
 	stmfd   sp!, {r1-r12, lr}

@@ -37,7 +37,7 @@ ENTRY(omap_modify_auxcoreboot0)
 	dsb
 	smc	#0
 	ldmfd   sp!, {r1-r12, pc}
-END(omap_modify_auxcoreboot0)
+ENDPROC(omap_modify_auxcoreboot0)
 
 ENTRY(omap_auxcoreboot_addr)
 	stmfd   sp!, {r2-r12, lr}

@@ -45,7 +45,7 @@ ENTRY(omap_auxcoreboot_addr)
 	dsb
 	smc	#0
 	ldmfd   sp!, {r2-r12, pc}
-END(omap_auxcoreboot_addr)
+ENDPROC(omap_auxcoreboot_addr)
 
 ENTRY(omap_read_auxcoreboot0)
 	stmfd   sp!, {r2-r12, lr}

@@ -54,4 +54,4 @@ ENTRY(omap_read_auxcoreboot0)
 	smc	#0
 	mov	r0, r0, lsr #9
 	ldmfd   sp!, {r2-r12, pc}
-END(omap_read_auxcoreboot0)
+ENDPROC(omap_read_auxcoreboot0)

index 98d8232..a05c348 100644
@@ -133,7 +133,7 @@ ENTRY(save_secure_ram_context)
 	mov	r6, #0xff
 	mcr	p15, 0, r0, c7, c10, 4	@ data write barrier
 	mcr	p15, 0, r0, c7, c10, 5	@ data memory barrier
-	.word	0xE1600071		@ call SMI monitor (smi #1)
+	smc	#1			@ call SMI monitor (smi #1)
 	nop
 	nop
 	nop

@@ -408,7 +408,7 @@ skipl2dis:
 	adr	r3, l2_inv_api_params	@ r3 points to dummy parameters
 	mcr	p15, 0, r0, c7, c10, 4	@ data write barrier
 	mcr	p15, 0, r0, c7, c10, 5	@ data memory barrier
-	.word	0xE1600071		@ call SMI monitor (smi #1)
+	smc	#1			@ call SMI monitor (smi #1)
 	/* Write to Aux control register to set some bits */
 	mov	r0, #42			@ set service ID for PPA
 	mov	r12, r0			@ copy secure Service ID in r12

@@ -419,7 +419,7 @@ skipl2dis:
 	ldr	r3, [r4, #0xBC]		@ r3 points to parameters
 	mcr	p15, 0, r0, c7, c10, 4	@ data write barrier
 	mcr	p15, 0, r0, c7, c10, 5	@ data memory barrier
-	.word	0xE1600071		@ call SMI monitor (smi #1)
+	smc	#1			@ call SMI monitor (smi #1)
 
 #ifdef CONFIG_OMAP3_L2_AUX_SECURE_SAVE_RESTORE
 	/* Restore L2 aux control register */

@@ -434,7 +434,7 @@ skipl2dis:
 	adds	r3, r3, #8		@ r3 points to parameters
 	mcr	p15, 0, r0, c7, c10, 4	@ data write barrier
 	mcr	p15, 0, r0, c7, c10, 5	@ data memory barrier
-	.word	0xE1600071		@ call SMI monitor (smi #1)
+	smc	#1			@ call SMI monitor (smi #1)
 #endif
 	b	logic_l1_restore
 

@@ -443,18 +443,18 @@ l2_inv_api_params:
 l2_inv_gp:
 	/* Execute smi to invalidate L2 cache */
 	mov r12, #0x1			@ set up to invalidate L2
-	.word 0xE1600070		@ Call SMI monitor (smieq)
+	smc	#0			@ Call SMI monitor (smieq)
 	/* Write to Aux control register to set some bits */
 	ldr	r4, scratchpad_base
 	ldr	r3, [r4,#0xBC]
 	ldr	r0, [r3,#4]
 	mov	r12, #0x3
-	.word	0xE1600070		@ Call SMI monitor (smieq)
+	smc	#0			@ Call SMI monitor (smieq)
 	ldr	r4, scratchpad_base
 	ldr	r3, [r4,#0xBC]
 	ldr	r0, [r3,#12]
 	mov	r12, #0x2
-	.word	0xE1600070		@ Call SMI monitor (smieq)
+	smc	#0			@ Call SMI monitor (smieq)
 logic_l1_restore:
 	ldr	r1, l2dis_3630
 	cmp	r1, #0x1		@ Test if L2 re-enable needed on 3630

index 7f893a2..829d235 100644
@@ -116,24 +116,34 @@ ENTRY(omap3_sram_configure_core_dpll)
 
 					@ pull the extra args off the stack
 					@  and store them in SRAM
+
+@ PC-relative stores lead to undefined behaviour in Thumb-2: use a r7 as a
+@ base instead.
+@ Be careful not to clobber r7 when maintaing this file.
+ THUMB(	adr	r7, omap3_sram_configure_core_dpll			)
+	.macro strtext Rt:req, label:req
+ ARM(	str	\Rt, \label						)
+ THUMB(	str	\Rt, [r7, \label - omap3_sram_configure_core_dpll]	)
+	.endm
+
 	ldr	r4, [sp, #52]
-	str     r4, omap_sdrc_rfr_ctrl_0_val
+	strtext	r4, omap_sdrc_rfr_ctrl_0_val
 	ldr	r4, [sp, #56]
-	str     r4, omap_sdrc_actim_ctrl_a_0_val
+	strtext	r4, omap_sdrc_actim_ctrl_a_0_val
 	ldr	r4, [sp, #60]
-	str     r4, omap_sdrc_actim_ctrl_b_0_val
+	strtext	r4, omap_sdrc_actim_ctrl_b_0_val
 	ldr	r4, [sp, #64]
-	str     r4, omap_sdrc_mr_0_val
+	strtext	r4, omap_sdrc_mr_0_val
 	ldr	r4, [sp, #68]
-	str     r4, omap_sdrc_rfr_ctrl_1_val
+	strtext	r4, omap_sdrc_rfr_ctrl_1_val
 	cmp	r4, #0			@ if SDRC_RFR_CTRL_1 is 0,
 	beq	skip_cs1_params		@  do not use cs1 params
 	ldr	r4, [sp, #72]
-	str     r4, omap_sdrc_actim_ctrl_a_1_val
+	strtext	r4, omap_sdrc_actim_ctrl_a_1_val
 	ldr	r4, [sp, #76]
-	str     r4, omap_sdrc_actim_ctrl_b_1_val
+	strtext	r4, omap_sdrc_actim_ctrl_b_1_val
 	ldr	r4, [sp, #80]
-	str     r4, omap_sdrc_mr_1_val
+	strtext	r4, omap_sdrc_mr_1_val
 skip_cs1_params:
 	mrc	p15, 0, r8, c1, c0, 0	@ read ctrl register
 	bic	r10, r8, #0x800		@ clear Z-bit, disable branch prediction

@@ -271,6 +281,7 @@ skip_cs1_prog:
 	ldr	r12, [r11]		@ posted-write barrier for SDRC
 	bx	lr
 
+	.align
 omap3_sdrc_power:
 	.word OMAP34XX_SDRC_REGADDR(SDRC_POWER)
 omap3_cm_clksel1_pll:

@@ -319,6 +330,7 @@ omap3_sdrc_dlla_ctrl:
 	.word OMAP34XX_SDRC_REGADDR(SDRC_DLLA_CTRL)
 core_m2_mask_val:
 	.word 0x07FFFFFF
+ENDPROC(omap3_sram_configure_core_dpll)
 
 ENTRY(omap3_sram_configure_core_dpll_sz)
 	.word	. - omap3_sram_configure_core_dpll

index a05c348..a204c78 100644
@@ -80,8 +80,10 @@
 /* Function call to get the restore pointer for resume from OFF */
 ENTRY(get_restore_pointer)
 	stmfd	sp!, {lr}	@ save registers on stack
-	adr	r0, restore
+	adr	r0, BSYM(restore)
 	ldmfd	sp!, {pc}	@ restore regs and return
+ENDPROC(get_restore_pointer)
+	.align
 ENTRY(get_restore_pointer_sz)
 	.word	. - get_restore_pointer
 

@@ -89,8 +91,10 @@ ENTRY(get_restore_pointer_sz)
 /* Function call to get the restore pointer for 3630 resume from OFF */
 ENTRY(get_omap3630_restore_pointer)
 	stmfd	sp!, {lr}	@ save registers on stack
-	adr	r0, restore_3630
+	adr	r0, BSYM(restore_3630)
 	ldmfd	sp!, {pc}	@ restore regs and return
+ENDPROC(get_omap3630_restore_pointer)
+	.align
 ENTRY(get_omap3630_restore_pointer_sz)
 	.word	. - get_omap3630_restore_pointer
 

@@ -98,8 +102,10 @@ ENTRY(get_omap3630_restore_pointer_sz)
 /* Function call to get the restore pointer for ES3 to resume from OFF */
 ENTRY(get_es3_restore_pointer)
 	stmfd	sp!, {lr}	@ save registers on stack
-	adr	r0, restore_es3
+	adr	r0, BSYM(restore_es3)
 	ldmfd	sp!, {pc}	@ restore regs and return
+ENDPROC(get_es3_restore_pointer)
+	.align
 ENTRY(get_es3_restore_pointer_sz)
 	.word	. - get_es3_restore_pointer
 

@@ -113,8 +119,11 @@ ENTRY(enable_omap3630_toggle_l2_on_restore)
 	stmfd	sp!, {lr}	@ save registers on stack
 	/* Setup so that we will disable and enable l2 */
 	mov	r1, #0x1
-	str	r1, l2dis_3630
+ ARM(	adrl	r2, l2dis_3630	)	@ may be out of range for adr in ARM
+ THUMB(	adr	r2, l2dis_3630	)	@ Thumb has more range, but not adrl
+	str	r1, [r2]
 	ldmfd	sp!, {pc}	@ restore regs and return
+ENDPROC(enable_omap3630_toggle_l2_on_restore)
 
 	.text
 /* Function to call rom code to save secure ram context */

@@ -139,12 +148,14 @@ ENTRY(save_secure_ram_context)
 	nop
 	nop
 	ldmfd	sp!, {r1-r12, pc}
+	.align
 sram_phy_addr_mask:
 	.word	SRAM_BASE_P
 high_mask:
 	.word	0xffff
 api_params:
 	.word	0x4, 0x0, 0x0, 0x1, 0x1
+ENDPROC(save_secure_ram_context)
 ENTRY(save_secure_ram_context_sz)
 	.word	. - save_secure_ram_context
 

@@ -279,8 +290,7 @@ clean_l2:
 	 *  - 'might' have to copy address, load and jump to it
 	 */
 	ldr	r1, kernel_flush
-	mov	lr, pc
-	bx	r1
+	blx	r1
 
 omap3_do_wfi:
 	ldr	r4, sdrc_power		@ read the SDRC_POWER register

@@ -346,8 +356,8 @@ restore_es3:
 	and	r4, r4, #0x3
 	cmp	r4, #0x0	@ Check if previous power state of CORE is OFF
 	bne	restore
-	adr	r0, es3_sdrc_fix
-	ldr	r1, sram_base
+	adr	r0, es3_sdrc_fix	@ Not using BSYM clears the Thumb bit.
+	ldr	r1, sram_base	@ Must be 8-byte aligned to preserve alignment.
 	ldr	r2, es3_sdrc_fix_sz
 	mov	r2, r2, ror #2
 copy_to_sram:

@@ -356,6 +366,7 @@ copy_to_sram:
 	subs	r2, r2, #0x1	@ num_words--
 	bne	copy_to_sram
 	ldr	r1, sram_base
+ THUMB(	orr	r1, r1, #BSYM(es3_sdrc_fix) & 1 )
 	blx	r1
 	b	restore
 

@@ -438,6 +449,7 @@ skipl2dis:
 #endif
 	b	logic_l1_restore
 
+	.align
 l2_inv_api_params:
 	.word	0x1, 0x00
 l2_inv_gp:

@@ -607,6 +619,7 @@ usettbr0:
 
 /* This function implements the erratum ID i443 WA, applies to 34xx >= ES3.0 */
 	.text
+	.align	3
 ENTRY(es3_sdrc_fix)
 	ldr	r4, sdrc_syscfg		@ get config addr
 	ldr	r5, [r4]		@ get value

@@ -634,6 +647,7 @@ ENTRY(es3_sdrc_fix)
 	str	r5, [r4]		@ kick off refreshes
 	bx	lr
 
+	.align
 sdrc_syscfg:
 	.word	SDRC_SYSCONFIG_P
 sdrc_mr_0:

@@ -648,6 +662,7 @@ sdrc_emr2_1:
 	.word	SDRC_EMR2_1_P
 sdrc_manual_1:
 	.word	SDRC_MANUAL_1_P
+ENDPROC(es3_sdrc_fix)
 ENTRY(es3_sdrc_fix_sz)
 	.word	. - es3_sdrc_fix
 

@@ -682,6 +697,10 @@ wait_sdrc_ready:
 	bic	r5, r5, #0x40
 	str	r5, [r4]
 
+@ PC-relative stores lead to undefined behaviour in Thumb-2: use a r7 as a
+@ base instead.
+@ Be careful not to clobber r7 when maintaing this code.
+
 is_dll_in_lock_mode:
 	/* Is dll in lock mode? */
 	ldr	r4, sdrc_dlla_ctrl

@@ -689,10 +708,11 @@ is_dll_in_lock_mode:
 	tst	r5, #0x4
 	bxne	lr			@ Return if locked
 	/* wait till dll locks */
+	adr	r7, kick_counter
 wait_dll_lock_timed:
 	ldr	r4, wait_dll_lock_counter
 	add	r4, r4, #1
-	str	r4, wait_dll_lock_counter
+	str	r4, [r7, #wait_dll_lock_counter - kick_counter]
 	ldr	r4, sdrc_dlla_status
 	/* Wait 20uS for lock */
 	mov	r6, #8

@@ -718,9 +738,10 @@ kick_dll:
 	dsb
 	ldr	r4, kick_counter
 	add	r4, r4, #1
-	str	r4, kick_counter
+	str	r4, [r7]		@ kick_counter
 	b	wait_dll_lock_timed
 
+	.align
 cm_idlest1_core:
 	.word	CM_IDLEST1_CORE_V
 cm_idlest_ckgen:

@@ -763,6 +784,7 @@ kick_counter:
 	.word	0
 wait_dll_lock_counter:
 	.word	0
+ENDPROC(omap34xx_cpu_suspend)
 
 ENTRY(omap34xx_cpu_suspend_sz)
 	.word	. - omap34xx_cpu_suspend

index a204c78..ee1edb1 100644
@@ -32,6 +32,14 @@
 #include "sdrc.h"
 #include "control.h"
 
+#undef ARM
+#undef THUMB
+#undef BSYM
+#define ARM(x...) x
+#define THUMB(x...)
+#define BSYM(x) (x)
+	.arm
+
 /*
  * Registers access definitions
  */

@@ -289,8 +297,20 @@ clean_l2:
 	 *  - should be faster and will change with kernel
 	 *  - 'might' have to copy address, load and jump to it
 	 */
+#ifdef CONFIG_THUMB2_KERNEL
+	/* kernel is non-interworking : must do this from Thumb */
+	adr	r1, . + 1
+	bx	r1
+	.thumb
+#endif
 	ldr	r1, kernel_flush
 	blx	r1
+#ifdef CONFIG_THUMB2_KERNEL
+	.align
+	bx	pc
+	nop
+	.arm
+#endif
 
 omap3_do_wfi:
 	ldr	r4, sdrc_power		@ read the SDRC_POWER register

index 829d235..64faab8 100644
@@ -34,6 +34,14 @@
 #include "sdrc.h"
 #include "cm2xxx_3xxx.h"
 
+#undef ARM
+#undef THUMB
+#undef BSYM
+#define ARM(x...) x
+#define THUMB(x...)
+#define BSYM(x) (x)
+	.arm
+
 	.text
 
 /* r1 parameters */

index a204c78..6ae8a92 100644
@@ -32,6 +32,14 @@
 #include "sdrc.h"
 #include "control.h"
 
+#undef ARM
+#undef THUMB
+#undef BSYM
+#define ARM(x...) x
+#define THUMB(x...)
+#define BSYM(x) (x)
+	.arm
+
 /*
  * Registers access definitions
  */

@@ -289,8 +297,20 @@ clean_l2:
 	 *  - should be faster and will change with kernel
 	 *  - 'might' have to copy address, load and jump to it
 	 */
-	ldr	r1, kernel_flush
+#ifdef CONFIG_THUMB2_KERNEL
+	/* kernel is non-interworking : must do this from Thumb */
+	adr	r1, 1f + 1
+	bx	r1
+	.thumb
+#endif
+1:	ldr	r1, kernel_flush
 	blx	r1
+#ifdef CONFIG_THUMB2_KERNEL
+	.align
+	bx	pc
+	nop
+	.arm
+#endif
 
 omap3_do_wfi:
 	ldr	r4, sdrc_power		@ read the SDRC_POWER register

index 829d235..64faab8 100644
@@ -34,6 +34,14 @@
 #include "sdrc.h"
 #include "cm2xxx_3xxx.h"
 
+#undef ARM
+#undef THUMB
+#undef BSYM
+#define ARM(x...) x
+#define THUMB(x...)
+#define BSYM(x) (x)
+	.arm
+
 	.text
 
 /* r1 parameters */