Thumb-2: Implement the unified boot code

This patch adds the ARM/Thumb-2 unified support for the
arch/arm/boot/* files.
Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>

arch/arm/boot/compressed/head.S

@@ -8,6 +8,8 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
+#include <asm/unified.h>
+
 #include <linux/linkage.h>
 
 /*
@@ -134,7 +136,8 @@ start:
 		tst	r2, #3			@ not user?
 		bne	not_angel
 		mov	r0, #0x17		@ angel_SWIreason_EnterSVC
-		swi	0x123456		@ angel_SWI_ARM
+ ARM(		swi	0x123456	)	@ angel_SWI_ARM
+ THUMB(		svc	0xab		)	@ angel_SWI_THUMB
 not_angel:
 		mrs	r2, cpsr		@ turn off interrupts to
 		orr	r2, r2, #0xc0		@ prevent angel from running
@@ -155,7 +158,9 @@ not_angel:
 
 		.text
 		adr	r0, LC0
-		ldmia	r0, {r1, r2, r3, r4, r5, r6, ip, sp}
+ ARM(		ldmia	r0, {r1, r2, r3, r4, r5, r6, ip, sp}	)
+ THUMB(		ldmia	r0, {r1, r2, r3, r4, r5, r6, ip}	)
+ THUMB(		ldr	sp, [r0, #28]				)
 		subs	r0, r0, r1		@ calculate the delta offset
 
 						@ if delta is zero, we are
@@ -263,12 +268,19 @@ not_relocated:	mov	r0, #0
 		adr	r2, reloc_start
 		ldr	r3, LC1
 		add	r3, r2, r3
-1:		ldmia	r2!, {r9 - r14}		@ copy relocation code
-		stmia	r1!, {r9 - r14}
-		ldmia	r2!, {r9 - r14}
-		stmia	r1!, {r9 - r14}
+ THUMB(		mov	sp, r8			) @ save the atags pointer
+1:
+ ARM(		ldmia	r2!, {r9 - r14}		) @ copy relocation code
+ ARM(		stmia	r1!, {r9 - r14}		)
+ ARM(		ldmia	r2!, {r9 - r14}		)
+ ARM(		stmia	r1!, {r9 - r14}		)
+ THUMB(		ldmia	r2!, {r8 - r12, r14}	) @ copy relocation code
+ THUMB(		stmia	r1!, {r8 - r12, r14}	)
+ THUMB(		ldmia	r2!, {r8 - r12, r14}	)
+ THUMB(		stmia	r1!, {r8 - r12, r14}	)
 		cmp	r2, r3
 		blo	1b
+ THUMB(		mov	r8, sp			) @ restore the atags pointer
 		add	sp, r1, #128		@ relocate the stack
 
 		bl	cache_clean_flush
@@ -285,6 +297,7 @@ wont_overwrite:	mov	r0, r4
 		bl	decompress_kernel
 		b	call_kernel
 
+		.align	2
 		.type	LC0, #object
 LC0:		.word	LC0			@ r1
 		.word	__bss_start		@ r2
@@ -399,8 +412,10 @@ __setup_mmu:	sub	r3, r4, #16384		@ Page directory size
 		orr	r1, r1, #3 << 10
 		add	r2, r3, #16384
 1:		cmp	r1, r9			@ if virt > start of RAM
+		it	hs
 		orrhs	r1, r1, #0x0c		@ set cacheable, bufferable
 		cmp	r1, r10			@ if virt > end of RAM
+		it	hs
 		bichs	r1, r1, #0x0c		@ clear cacheable, bufferable
 		str	r1, [r0], #4		@ 1:1 mapping
 		add	r1, r1, #1048576
@@ -441,14 +456,17 @@ __armv7_mmu_cache_on:
 		mov	r12, lr
 		mrc	p15, 0, r11, c0, c1, 4	@ read ID_MMFR0
 		tst	r11, #0xf		@ VMSA
+		it	ne
 		blne	__setup_mmu
 		mov	r0, #0
 		mcr	p15, 0, r0, c7, c10, 4	@ drain write buffer
 		tst	r11, #0xf		@ VMSA
+		it	ne
 		mcrne	p15, 0, r0, c8, c7, 0	@ flush I,D TLBs
 		mrc	p15, 0, r0, c1, c0, 0	@ read control reg
 		orr	r0, r0, #0x5000		@ I-cache enable, RR cache replacement
 		orr	r0, r0, #0x003c		@ write buffer
+		itttt	ne
 		orrne	r0, r0, #1		@ MMU enabled
 		movne	r1, #-1
 		mcrne	p15, 0, r3, c2, c0, 0	@ load page table pointer
@@ -503,14 +521,18 @@ reloc_start:	add	r9, r5, r0
 		sub	r9, r9, #128		@ do not copy the stack
 		debug_reloc_start
 		mov	r1, r4
+ THUMB(		mov	sp, r8			)	@ save the atags pointer
 1:
 		.rept	4
-		ldmia	r5!, {r0, r2, r3, r10 - r14}	@ relocate kernel
-		stmia	r1!, {r0, r2, r3, r10 - r14}
+ ARM(		ldmia	r5!, {r0, r2, r3, r10 - r14}	)	@ relocate kernel
+ ARM(		stmia	r1!, {r0, r2, r3, r10 - r14}	)
+ THUMB(		ldmia	r5!, {r0, r2, r3, r8, r10 - r12, r14})	@ relocate kernel
+ THUMB(		stmia	r1!, {r0, r2, r3, r8, r10 - r12, r14})
 		.endr
 
 		cmp	r5, r9
 		blo	1b
+ THUMB(		mov	r8, sp			)	@ restore the atags pointer
 		add	sp, r1, #128		@ relocate the stack
 		debug_reloc_end
 
@@ -545,6 +567,7 @@ call_cache_fn:	adr	r12, proc_types
 		ldr	r2, [r12, #4]		@ get mask
 		eor	r1, r1, r6		@ (real ^ match)
 		tst	r1, r2			@       & mask
+		it	eq
 		addeq	pc, r12, r3		@ call cache function
 		add	r12, r12, #4*5
 		b	1b
@@ -563,104 +586,105 @@ call_cache_fn:	adr	r12, proc_types
  * methods.  Writeback caches _must_ have the flush method
  * defined.
  */
+		.align	2
 		.type	proc_types,#object
 proc_types:
 		.word	0x41560600		@ ARM6/610
 		.word	0xffffffe0
-		b	__arm6_mmu_cache_off	@ works, but slow
-		b	__arm6_mmu_cache_off
-		mov	pc, lr
+		W(b)	__arm6_mmu_cache_off	@ works, but slow
+		W(b)	__arm6_mmu_cache_off
+		W(mov)	pc, lr
 @		b	__arm6_mmu_cache_on		@ untested
 @		b	__arm6_mmu_cache_off
 @		b	__armv3_mmu_cache_flush
 
 		.word	0x00000000		@ old ARM ID
 		.word	0x0000f000
-		mov	pc, lr
-		mov	pc, lr
-		mov	pc, lr
+		W(mov)	pc, lr
+		W(mov)	pc, lr
+		W(mov)	pc, lr
 
 		.word	0x41007000		@ ARM7/710
 		.word	0xfff8fe00
-		b	__arm7_mmu_cache_off
-		b	__arm7_mmu_cache_off
-		mov	pc, lr
+		W(b)	__arm7_mmu_cache_off
+		W(b)	__arm7_mmu_cache_off
+		W(mov)	pc, lr
 
 		.word	0x41807200		@ ARM720T (writethrough)
 		.word	0xffffff00
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		mov	pc, lr
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(mov)	pc, lr
 
 		.word	0x41007400		@ ARM74x
 		.word	0xff00ff00
-		b	__armv3_mpu_cache_on
-		b	__armv3_mpu_cache_off
-		b	__armv3_mpu_cache_flush
+		W(b)	__armv3_mpu_cache_on
+		W(b)	__armv3_mpu_cache_off
+		W(b)	__armv3_mpu_cache_flush
 		
 		.word	0x41009400		@ ARM94x
 		.word	0xff00ff00
-		b	__armv4_mpu_cache_on
-		b	__armv4_mpu_cache_off
-		b	__armv4_mpu_cache_flush
+		W(b)	__armv4_mpu_cache_on
+		W(b)	__armv4_mpu_cache_off
+		W(b)	__armv4_mpu_cache_flush
 
 		.word	0x00007000		@ ARM7 IDs
 		.word	0x0000f000
-		mov	pc, lr
-		mov	pc, lr
-		mov	pc, lr
+		W(mov)	pc, lr
+		W(mov)	pc, lr
+		W(mov)	pc, lr
 
 		@ Everything from here on will be the new ID system.
 
 		.word	0x4401a100		@ sa110 / sa1100
 		.word	0xffffffe0
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		b	__armv4_mmu_cache_flush
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(b)	__armv4_mmu_cache_flush
 
 		.word	0x6901b110		@ sa1110
 		.word	0xfffffff0
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		b	__armv4_mmu_cache_flush
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(b)	__armv4_mmu_cache_flush
 
 		@ These match on the architecture ID
 
 		.word	0x00020000		@ ARMv4T
 		.word	0x000f0000
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		b	__armv4_mmu_cache_flush
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(b)	__armv4_mmu_cache_flush
 
 		.word	0x00050000		@ ARMv5TE
 		.word	0x000f0000
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		b	__armv4_mmu_cache_flush
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(b)	__armv4_mmu_cache_flush
 
 		.word	0x00060000		@ ARMv5TEJ
 		.word	0x000f0000
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		b	__armv4_mmu_cache_flush
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(b)	__armv4_mmu_cache_flush
 
 		.word	0x0007b000		@ ARMv6
 		.word	0x000ff000
-		b	__armv4_mmu_cache_on
-		b	__armv4_mmu_cache_off
-		b	__armv6_mmu_cache_flush
+		W(b)	__armv4_mmu_cache_on
+		W(b)	__armv4_mmu_cache_off
+		W(b)	__armv6_mmu_cache_flush
 
 		.word	0x000f0000		@ new CPU Id
 		.word	0x000f0000
-		b	__armv7_mmu_cache_on
-		b	__armv7_mmu_cache_off
-		b	__armv7_mmu_cache_flush
+		W(b)	__armv7_mmu_cache_on
+		W(b)	__armv7_mmu_cache_off
+		W(b)	__armv7_mmu_cache_flush
 
 		.word	0			@ unrecognised type
 		.word	0
-		mov	pc, lr
-		mov	pc, lr
-		mov	pc, lr
+		W(mov)	pc, lr
+		W(mov)	pc, lr
+		W(mov)	pc, lr
 
 		.size	proc_types, . - proc_types
 
@@ -756,6 +780,7 @@ __armv4_mpu_cache_flush:
 		bcs	1b			@ segments 7 to 0
 
 		teq	r2, #0
+		it	ne
 		mcrne	p15, 0, ip, c7, c5, 0	@ invalidate I cache
 		mcr	p15, 0, ip, c7, c10, 4	@ drain WB
 		mov	pc, lr
@@ -777,7 +802,8 @@ __armv7_mmu_cache_flush:
 		mcr	p15, 0, r10, c7, c14, 0	@ clean+invalidate D
 		b	iflush
 hierarchical:
-		stmfd	sp!, {r0-r5, r7, r9-r11}
+ ARM(		stmfd	sp!, {r0-r5, r7, r9-r11})
+ THUMB(		stmfd	sp!, {r0-r7, r9-r11}	)
 		mrc	p15, 1, r0, c0, c0, 1	@ read clidr
 		ands	r3, r0, #0x7000000	@ extract loc from clidr
 		mov	r3, r3, lsr #23		@ left align loc bit field
@@ -796,14 +822,19 @@ loop1:
 		add	r2, r2, #4		@ add 4 (line length offset)
 		ldr	r4, =0x3ff
 		ands	r4, r4, r1, lsr #3	@ find maximum number on the way size
-		.word	0xe16f5f14		@ clz r5, r4 - find bit position of way size increment
+ ARM(		.word	0xe16f5f14		) @ clz r5, r4
+ THUMB(		clz	r5, r4			) @ find bit position of way size increment
 		ldr	r7, =0x7fff
 		ands	r7, r7, r1, lsr #13	@ extract max number of the index size
 loop2:
 		mov	r9, r4			@ create working copy of max way size
 loop3:
-		orr	r11, r10, r9, lsl r5	@ factor way and cache number into r11
-		orr	r11, r11, r7, lsl r2	@ factor index number into r11
+ ARM(		orr	r11, r10, r9, lsl r5	) @ factor way and cache number into r11
+ ARM(		orr	r11, r11, r7, lsl r2	) @ factor index number into r11
+ THUMB(		lsl	r6, r9, r5		)
+ THUMB(		orr	r11, r10, r6		) @ factor way and cache number into r11
+ THUMB(		lsl	r6, r7, r2		)
+ THUMB(		orr	r11, r11, r6		) @ factor index number into r11
 		mcr	p15, 0, r11, c7, c14, 2	@ clean & invalidate by set/way
 		subs	r9, r9, #1		@ decrement the way
 		bge	loop3
@@ -816,7 +847,8 @@ skip:
 finished:
 		mov	r10, #0			@ swith back to cache level 0
 		mcr	p15, 2, r10, c0, c0, 0	@ select current cache level in cssr
-		ldmfd	sp!, {r0-r5, r7, r9-r11}
+ ARM(		ldmfd	sp!, {r0-r5, r7, r9-r11})
+ THUMB(		ldmfd	sp!, {r0-r7, r9-r11}	)
 iflush:
 		mcr	p15, 0, r10, c7, c5, 0	@ invalidate I+BTB
 		mcr	p15, 0, r10, c7, c10, 4	@ drain WB
@@ -833,6 +865,7 @@ __armv4_mmu_cache_flush:
 		mov	r2, #1024
 		mov	r2, r2, lsl r1		@ base dcache size *2
 		tst	r3, #1 << 14		@ test M bit
+		it	ne
 		addne	r2, r2, r2, lsr #1	@ +1/2 size if M == 1
 		mov	r3, r3, lsr #12
 		and	r3, r3, #3
@@ -841,7 +874,10 @@ __armv4_mmu_cache_flush:
 no_cache_id:
 		bic	r1, pc, #63		@ align to longest cache line
 		add	r2, r1, r2
-1:		ldr	r3, [r1], r11		@ s/w flush D cache
+1:
+ ARM(		ldr	r3, [r1], r11		) @ s/w flush D cache
+ THUMB(		ldr	r3, [r1]		) @ s/w flush D cache
+ THUMB(		add	r1, r1, r11		)
 		teq	r1, r2
 		bne	1b
 
@@ -861,6 +897,7 @@ __armv3_mpu_cache_flush:
  * memory, which again must be relocatable.
  */
 #ifdef DEBUG
+		.align	2
 		.type	phexbuf,#object
 phexbuf:	.space	12
 		.size	phexbuf, . - phexbuf