summaryrefslogtreecommitdiff
path: root/arch/arm26/kernel/entry.S
blob: a231dd88d0e13fb514eeb92b53edf106840d10d6 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
/* arch/arm26/kernel/entry.S
 * 
 * Assembled from chunks of code in arch/arm
 *
 * Copyright (C) 2003 Ian Molton
 * Based on the work of RMK.
 *
 */

#include <linux/linkage.h>

#include <asm/assembler.h>
#include <asm/asm_offsets.h>
#include <asm/errno.h>
#include <asm/hardware.h>
#include <asm/sysirq.h>
#include <asm/thread_info.h>
#include <asm/page.h>
#include <asm/ptrace.h>

	.macro	zero_fp
#ifndef CONFIG_NO_FRAME_POINTER
	mov	fp, #0
#endif
	.endm

	.text

@ Bad Abort numbers
@ -----------------
@
#define BAD_PREFETCH	0
#define BAD_DATA	1
#define BAD_ADDREXCPTN	2
#define BAD_IRQ		3
#define BAD_UNDEFINSTR	4

@ OS version number used in SWIs
@  RISC OS is 0
@  RISC iX is 8
@
#define OS_NUMBER	9
#define ARMSWI_OFFSET	0x000f0000

@
@ Stack format (ensured by USER_* and SVC_*)
@ PSR and PC are comined on arm26
@

#define S_OFF		8

#define S_OLD_R0	64
#define S_PC		60
#define S_LR		56
#define S_SP		52
#define S_IP		48
#define S_FP		44
#define S_R10		40
#define S_R9		36
#define S_R8		32
#define S_R7		28
#define S_R6		24
#define S_R5		20
#define S_R4		16
#define S_R3		12
#define S_R2		8
#define S_R1		4
#define S_R0		0

	.macro	save_user_regs
	str	r0, [sp, #-4]!   @ Store SVC r0
	str	lr, [sp, #-4]!   @ Store user mode PC
	sub	sp, sp, #15*4
	stmia	sp, {r0 - lr}^   @ Store the other user-mode regs
	mov	r0, r0
	.endm

	.macro	slow_restore_user_regs
	ldmia	sp, {r0 - lr}^   @ restore the user regs not including PC
	mov	r0, r0
	ldr	lr, [sp, #15*4]  @ get user PC
	add	sp, sp, #15*4+8  @ free stack
	movs	pc, lr           @ return
	.endm

	.macro	fast_restore_user_regs
	add	sp, sp, #S_OFF
	ldmib	sp, {r1 - lr}^
	mov	r0, r0
	ldr	lr, [sp, #15*4]
	add	sp, sp, #15*4+8
	movs	pc, lr
	.endm

	.macro	save_svc_regs
	str     sp, [sp, #-16]!
	str     lr, [sp, #8]
	str     lr, [sp, #4]
	stmfd   sp!, {r0 - r12}
	mov     r0, #-1
	str     r0, [sp, #S_OLD_R0]
	zero_fp
	.endm

	.macro	save_svc_regs_irq
	str     sp, [sp, #-16]!
	str     lr, [sp, #4]
	ldr     lr, .LCirq
	ldr     lr, [lr]
	str     lr, [sp, #8]
	stmfd   sp!, {r0 - r12}
	mov     r0, #-1
	str     r0, [sp, #S_OLD_R0]
	zero_fp
	.endm

	.macro	restore_svc_regs
                ldmfd   sp, {r0 - pc}^
	.endm

	.macro	mask_pc, rd, rm
	bic	\rd, \rm, #PCMASK
	.endm

	.macro  disable_irqs, temp
	mov     \temp, pc
	orr     \temp, \temp, #PSR_I_BIT
	teqp    \temp, #0
	.endm

	.macro	enable_irqs, temp
	mov     \temp, pc
	and     \temp, \temp, #~PSR_I_BIT
	teqp	\temp, #0
	.endm

	.macro	initialise_traps_extra
	.endm

	.macro	get_thread_info, rd
	mov	\rd, sp, lsr #13
	mov	\rd, \rd, lsl #13
	.endm

/*
 * These are the registers used in the syscall handler, and allow us to
 * have in theory up to 7 arguments to a function - r0 to r6.
 *
 * Note that tbl == why is intentional.
 *
 * We must set at least "tsk" and "why" when calling ret_with_reschedule.
 */
scno	.req	r7		@ syscall number
tbl	.req	r8		@ syscall table pointer
why	.req	r8		@ Linux syscall (!= 0)
tsk	.req	r9		@ current thread_info

/*
 * Get the system call number.
 */
	.macro	get_scno
	mask_pc	lr, lr
	ldr	scno, [lr, #-4]		@ get SWI instruction
	.endm
/*
 *  -----------------------------------------------------------------------
 */

/* 
 * We rely on the fact that R0 is at the bottom of the stack (due to
 * slow/fast restore user regs).
 */
#if S_R0 != 0
#error "Please fix"
#endif

/*
 * This is the fast syscall return path.  We do as little as
 * possible here, and this includes saving r0 back into the SVC
 * stack.
 */
ret_fast_syscall:
	disable_irqs r1				@ disable interrupts
	ldr	r1, [tsk, #TI_FLAGS]
	tst	r1, #_TIF_WORK_MASK
	bne	fast_work_pending
	fast_restore_user_regs

/*
 * Ok, we need to do extra processing, enter the slow path.
 */
fast_work_pending:
	str	r0, [sp, #S_R0+S_OFF]!		@ returned r0
work_pending:
	tst	r1, #_TIF_NEED_RESCHED
	bne	work_resched
	tst	r1, #_TIF_NOTIFY_RESUME | _TIF_SIGPENDING
	beq	no_work_pending
	mov	r0, sp				@ 'regs'
	mov	r2, why				@ 'syscall'
	bl	do_notify_resume
	disable_irqs r1				@ disable interrupts
	b	no_work_pending

work_resched:
	bl	schedule
/*
 * "slow" syscall return path.  "why" tells us if this was a real syscall.
 */
ENTRY(ret_to_user)
ret_slow_syscall:
	disable_irqs r1				@ disable interrupts
	ldr	r1, [tsk, #TI_FLAGS]
	tst	r1, #_TIF_WORK_MASK
	bne	work_pending
no_work_pending:
	slow_restore_user_regs

/*
 * This is how we return from a fork.
 */
ENTRY(ret_from_fork)
	bl	schedule_tail
	get_thread_info tsk
	ldr	r1, [tsk, #TI_FLAGS]		@ check for syscall tracing
	mov	why, #1
	tst	r1, #_TIF_SYSCALL_TRACE		@ are we tracing syscalls?
	beq	ret_slow_syscall
	mov	r1, sp
	mov	r0, #1				@ trace exit [IP = 1]
	bl	syscall_trace
	b	ret_slow_syscall
	
// FIXME - is this strictly necessary?
#include "calls.S"

/*=============================================================================
 * SWI handler
 *-----------------------------------------------------------------------------
 */

	.align	5
ENTRY(vector_swi)
	save_user_regs
	zero_fp
	get_scno

#ifdef CONFIG_ALIGNMENT_TRAP
	ldr	ip, __cr_alignment
	ldr	ip, [ip]
	mcr	p15, 0, ip, c1, c0		@ update control register
#endif
	enable_irqs ip

	str	r4, [sp, #-S_OFF]!		@ push fifth arg

	get_thread_info tsk
	ldr	ip, [tsk, #TI_FLAGS]		@ check for syscall tracing
	bic	scno, scno, #0xff000000		@ mask off SWI op-code
	eor	scno, scno, #OS_NUMBER << 20	@ check OS number
	adr	tbl, sys_call_table		@ load syscall table pointer
	tst	ip, #_TIF_SYSCALL_TRACE		@ are we tracing syscalls?
	bne	__sys_trace

	adral	lr, ret_fast_syscall            @ set return address
        orral	lr, lr, #PSR_I_BIT | MODE_SVC26 @ Force SVC mode on return
	cmp	scno, #NR_syscalls		@ check upper syscall limit
	ldrcc	pc, [tbl, scno, lsl #2]		@ call sys_* routine

	add	r1, sp, #S_OFF
2:	mov	why, #0				@ no longer a real syscall
	cmp	scno, #ARMSWI_OFFSET
	eor	r0, scno, #OS_NUMBER << 20	@ put OS number back
	bcs	arm_syscall	
	b	sys_ni_syscall			@ not private func

	/*
	 * This is the really slow path.  We're going to be doing
	 * context switches, and waiting for our parent to respond.
	 */
__sys_trace:
	add	r1, sp, #S_OFF
	mov	r0, #0				@ trace entry [IP = 0]
	bl	syscall_trace

	adral   lr, __sys_trace_return          @ set return address
        orral   lr, lr, #PSR_I_BIT | MODE_SVC26 @ Force SVC mode on return
	add	r1, sp, #S_R0 + S_OFF		@ pointer to regs
	cmp	scno, #NR_syscalls		@ check upper syscall limit
	ldmccia	r1, {r0 - r3}			@ have to reload r0 - r3
	ldrcc	pc, [tbl, scno, lsl #2]		@ call sys_* routine
	b	2b

__sys_trace_return:
	str	r0, [sp, #S_R0 + S_OFF]!	@ save returned r0
	mov	r1, sp
	mov	r0, #1				@ trace exit [IP = 1]
	bl	syscall_trace
	b	ret_slow_syscall

	.align	5
#ifdef CONFIG_ALIGNMENT_TRAP
	.type	__cr_alignment, #object
__cr_alignment:
	.word	cr_alignment
#endif

	.type	sys_call_table, #object
ENTRY(sys_call_table)
#include "calls.S"

/*============================================================================
 * Special system call wrappers
 */
@ r0 = syscall number
@ r5 = syscall table
		.type	sys_syscall, #function
sys_syscall:
		eor	scno, r0, #OS_NUMBER << 20
		cmp	scno, #NR_syscalls	@ check range
		stmleia	sp, {r5, r6}		@ shuffle args
		movle	r0, r1
		movle	r1, r2
		movle	r2, r3
		movle	r3, r4
		ldrle	pc, [tbl, scno, lsl #2]
		b	sys_ni_syscall

sys_fork_wrapper:
		add	r0, sp, #S_OFF
		b	sys_fork

sys_vfork_wrapper:
		add	r0, sp, #S_OFF
		b	sys_vfork

sys_execve_wrapper:
		add	r3, sp, #S_OFF
		b	sys_execve

sys_clone_wapper:
		add	r2, sp, #S_OFF
		b	sys_clone

sys_sigsuspend_wrapper:
		add	r3, sp, #S_OFF
		b	sys_sigsuspend

sys_rt_sigsuspend_wrapper:
		add	r2, sp, #S_OFF
		b	sys_rt_sigsuspend

sys_sigreturn_wrapper:
		add	r0, sp, #S_OFF
		b	sys_sigreturn

sys_rt_sigreturn_wrapper:
		add	r0, sp, #S_OFF
		b	sys_rt_sigreturn

sys_sigaltstack_wrapper:
		ldr	r2, [sp, #S_OFF + S_SP]
		b	do_sigaltstack

/*
 * Note: off_4k (r5) is always units of 4K.  If we can't do the requested
 * offset, we return EINVAL.  FIXME - this lost some stuff from arm32 to
 * ifdefs. check it out.
 */
sys_mmap2:
		tst	r5, #((1 << (PAGE_SHIFT - 12)) - 1)
		moveq	r5, r5, lsr #PAGE_SHIFT - 12
		streq	r5, [sp, #4]
		beq	do_mmap2
		mov	r0, #-EINVAL
		RETINSTR(mov,pc, lr)

/*
 *  Design issues:
 *   - We have several modes that each vector can be called from,
 *     each with its own set of registers.  On entry to any vector,
 *     we *must* save the registers used in *that* mode.
 *
 *   - This code must be as fast as possible.
 *
 *  There are a few restrictions on the vectors:
 *   - the SWI vector cannot be called from *any* non-user mode
 *
 *   - the FP emulator is *never* called from *any* non-user mode undefined
 *     instruction.
 *
 */

		.text

		.macro handle_irq
1:		mov     r4, #IOC_BASE
		ldrb    r6, [r4, #0x24]            @ get high priority first
		adr     r5, irq_prio_h
		teq     r6, #0
		ldreqb  r6, [r4, #0x14]            @ get low priority
		adreq   r5, irq_prio_l

                teq     r6, #0                     @ If an IRQ happened...
                ldrneb  r0, [r5, r6]               @ get IRQ number
                movne   r1, sp                     @ get struct pt_regs
                adrne   lr, 1b                     @ Set return address to 1b
                orrne   lr, lr, #PSR_I_BIT | MODE_SVC26  @ (and force SVC mode)
                bne     asm_do_IRQ                 @ process IRQ (if asserted)
		.endm


/*
 * Interrupt table (incorporates priority)
 */
		.macro	irq_prio_table
irq_prio_l:	.byte	 0, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3
		.byte	 4, 0, 1, 0, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3
		.byte	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
		.byte	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
		.byte	 6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3
		.byte	 6, 6, 6, 6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3
		.byte	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
		.byte	 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
		.byte	 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7
irq_prio_h:	.byte	 0, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	12, 8, 9, 8,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	14,14,14,14,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	15,15,15,15,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.byte	13,13,13,13,10,10,10,10,11,11,11,11,10,10,10,10
		.endm

#if 1
/*
 * Uncomment these if you wish to get more debugging into about data aborts.
 * FIXME - I bet we can find a way to encode these and keep performance.
 */
#define FAULT_CODE_LDRSTRPOST	0x80
#define FAULT_CODE_LDRSTRPRE	0x40
#define FAULT_CODE_LDRSTRREG	0x20
#define FAULT_CODE_LDMSTM	0x10
#define FAULT_CODE_LDCSTC	0x08
#endif
#define FAULT_CODE_PREFETCH	0x04
#define FAULT_CODE_WRITE	0x02
#define FAULT_CODE_FORCECOW	0x01

/*=============================================================================
 * Undefined FIQs
 *-----------------------------------------------------------------------------
 */
_unexp_fiq:	ldr     sp, .LCfiq
		mov	r12, #IOC_BASE
		strb	r12, [r12, #0x38]	@ Disable FIQ register
		teqp	pc, #PSR_I_BIT | PSR_F_BIT | MODE_SVC26
		mov	r0, r0
		stmfd	sp!, {r0 - r3, ip, lr}
		adr	r0, Lfiqmsg
		bl	printk
		ldmfd	sp!, {r0 - r3, ip, lr}
		teqp	pc, #PSR_I_BIT | PSR_F_BIT | MODE_FIQ26
		mov	r0, r0
		movs	pc, lr

Lfiqmsg:	.ascii	"*** Unexpected FIQ\n\0"
		.align

.LCfiq:		.word	__temp_fiq
.LCirq:		.word	__temp_irq

/*=============================================================================
 * Undefined instruction handler
 *-----------------------------------------------------------------------------
 * Handles floating point instructions
 */
vector_undefinstr:
		tst	lr, #MODE_SVC26          @ did we come from a non-user mode?
		bne	__und_svc                @ yes - deal with it.
/* Otherwise, fall through for the user-space (common) case. */
		save_user_regs
		zero_fp                                 @ zero frame pointer
		teqp	pc, #PSR_I_BIT | MODE_SVC26     @ disable IRQs
.Lbug_undef:
		ldr	r4, .LC2
                ldr     pc, [r4]         @ Call FP module entry point
/* FIXME - should we trap for a null pointer here? */

/* The SVC mode case */
__und_svc:	save_svc_regs                           @ Non-user mode
                mask_pc r0, lr
                and     r2, lr, #3
                sub     r0, r0, #4
                mov     r1, sp
                bl      do_undefinstr
                restore_svc_regs

/* We get here if the FP emulator doesnt handle the undef instr.
 * If the insn WAS handled, the emulator jumps to ret_from_exception by itself/
 */
		.globl	fpundefinstr 
fpundefinstr:
		mov	r0, lr
		mov	r1, sp
		teqp	pc, #MODE_SVC26
		bl	do_undefinstr
		b	ret_from_exception		@ Normal FP exit

#if defined CONFIG_FPE_NWFPE || defined CONFIG_FPE_FASTFPE
		/* The FPE is always present */
		.equ	fpe_not_present, 0
#else
/* We get here if an undefined instruction happens and the floating
 * point emulator is not present.  If the offending instruction was
 * a WFS, we just perform a normal return as if we had emulated the
 * operation.  This is a hack to allow some basic userland binaries
 * to run so that the emulator module proper can be loaded. --philb
 * FIXME - probably a broken useless hack...
 */
fpe_not_present:
		adr	r10, wfs_mask_data
		ldmia	r10, {r4, r5, r6, r7, r8}
		ldr	r10, [sp, #S_PC]		@ Load PC
		sub	r10, r10, #4
		mask_pc	r10, r10
		ldrt	r10, [r10]			@ get instruction
		and	r5, r10, r5
		teq	r5, r4				@ Is it WFS?
		beq	ret_from_exception
		and	r5, r10, r8
		teq	r5, r6				@ Is it LDF/STF on sp or fp?
		teqne	r5, r7
		bne	fpundefinstr
		tst	r10, #0x00200000		@ Does it have WB
		beq	ret_from_exception
		and	r4, r10, #255			@ get offset
		and	r6, r10, #0x000f0000
		tst	r10, #0x00800000		@ +/-
		ldr	r5, [sp, r6, lsr #14]		@ Load reg
		rsbeq	r4, r4, #0
		add	r5, r5, r4, lsl #2
		str	r5, [sp, r6, lsr #14]		@ Save reg
		b	ret_from_exception

wfs_mask_data:	.word	0x0e200110			@ WFS/RFS
		.word	0x0fef0fff
		.word	0x0d0d0100			@ LDF [sp]/STF [sp]
		.word	0x0d0b0100			@ LDF [fp]/STF [fp]
		.word	0x0f0f0f00
#endif

.LC2:		.word	fp_enter

/*=============================================================================
 * Prefetch abort handler
 *-----------------------------------------------------------------------------
 */
#define DEBUG_UNDEF
/* remember: lr = USR pc */
vector_prefetch:
		sub	lr, lr, #4
		tst	lr, #MODE_SVC26
		bne	__pabt_invalid
		save_user_regs
		teqp	pc, #MODE_SVC26         @ Enable IRQs...
		mask_pc	r0, lr			@ Address of abort
		mov	r1, sp			@ Tasks registers
		bl	do_PrefetchAbort
		teq	r0, #0			@ If non-zero, we believe this abort..
		bne	ret_from_exception
#ifdef DEBUG_UNDEF
		adr	r0, t
		bl	printk
#endif
		ldr	lr, [sp,#S_PC]		@ FIXME program to test this on.  I think its
		b	.Lbug_undef		@ broken at the moment though!)

__pabt_invalid:	save_svc_regs
		mov	r0, sp			@ Prefetch aborts are definitely *not*
		mov	r1, #BAD_PREFETCH	@ allowed in non-user modes.  We cant
		and	r2, lr, #3		@ recover from this problem.
		b	bad_mode

#ifdef DEBUG_UNDEF
t:		.ascii "*** undef ***\r\n\0"
		.align
#endif

/*=============================================================================
 * Address exception handler
 *-----------------------------------------------------------------------------
 * These aren't too critical.
 * (they're not supposed to happen).
 * In order to debug the reason for address exceptions in non-user modes,
 * we have to obtain all the registers so that we can see what's going on.
 */

vector_addrexcptn:
		sub	lr, lr, #8
		tst	lr, #3
		bne	Laddrexcptn_not_user
		save_user_regs
		teq	pc, #MODE_SVC26
		mask_pc	r0, lr			@ Point to instruction
		mov	r1, sp			@ Point to registers
		mov	r2, #0x400
		mov	lr, pc
		bl	do_excpt
		b	ret_from_exception

Laddrexcptn_not_user:
		save_svc_regs
		and	r2, lr, #3
		teq	r2, #3
		bne	Laddrexcptn_illegal_mode
		teqp	pc, #MODE_SVC26
		mask_pc	r0, lr
		mov	r1, sp
		orr	r2, r2, #0x400
		bl	do_excpt
		ldmia	sp, {r0 - lr}		@ I cant remember the reason I changed this...
		add	sp, sp, #15*4
		movs	pc, lr

Laddrexcptn_illegal_mode:
		mov	r0, sp
		str	lr, [sp, #-4]!
		orr	r1, r2, #PSR_I_BIT | PSR_F_BIT
		teqp	r1, #0			@ change into mode (wont be user mode)
		mov	r0, r0
		mov	r1, r8			@ Any register from r8 - r14 can be banked
		mov	r2, r9
		mov	r3, r10
		mov	r4, r11
		mov	r5, r12
		mov	r6, r13
		mov	r7, r14
		teqp	pc, #PSR_F_BIT | MODE_SVC26 @ back to svc
		mov	r0, r0
		stmfd	sp!, {r1-r7}
		ldmia	r0, {r0-r7}
		stmfd	sp!, {r0-r7}
		mov	r0, sp
		mov	r1, #BAD_ADDREXCPTN
		b	bad_mode

/*=============================================================================
 * Interrupt (IRQ) handler
 *-----------------------------------------------------------------------------
 * Note: if the IRQ was taken whilst in user mode, then *no* kernel routine
 * is running, so do not have to save svc lr.
 *
 * Entered in IRQ mode.
 */

vector_IRQ:	ldr     sp, .LCirq         @ Setup some temporary stack
                sub     lr, lr, #4
                str     lr, [sp]           @ push return address

		tst     lr, #3
		bne	__irq_non_usr

__irq_usr:	teqp	pc, #PSR_I_BIT | MODE_SVC26     @ Enter SVC mode
		mov	r0, r0

		ldr	lr, .LCirq
		ldr	lr, [lr]           @ Restore lr for jump back to USR

		save_user_regs

		handle_irq

		mov	why, #0
		get_thread_info tsk
		b	ret_to_user

@ Place the IRQ priority table here so that the handle_irq macros above
@ and below here can access it.

		irq_prio_table

__irq_non_usr:	teqp	pc, #PSR_I_BIT | MODE_SVC26     @ Enter SVC mode
		mov	r0, r0

		save_svc_regs_irq

                and	r2, lr, #3
		teq	r2, #3
		bne	__irq_invalid                @ IRQ not from SVC mode

		handle_irq

		restore_svc_regs

__irq_invalid:	mov	r0, sp
		mov	r1, #BAD_IRQ
		b	bad_mode

/*=============================================================================
 * Data abort handler code
 *-----------------------------------------------------------------------------
 *
 * This handles both exceptions from user and SVC modes, computes the address
 *  range of the problem, and does any correction that is required.  It then
 *  calls the kernel data abort routine.
 *
 * This is where I wish that the ARM would tell you which address aborted.
 */

vector_data:	sub	lr, lr, #8		@ Correct lr
		tst	lr, #3
		bne	Ldata_not_user
		save_user_regs
		teqp	pc, #MODE_SVC26
		mask_pc	r0, lr
		bl	Ldata_do
		b	ret_from_exception

Ldata_not_user:
		save_svc_regs
		and	r2, lr, #3
		teq	r2, #3
		bne	Ldata_illegal_mode
		tst	lr, #PSR_I_BIT
		teqeqp	pc, #MODE_SVC26
		mask_pc	r0, lr
		bl	Ldata_do
		restore_svc_regs

Ldata_illegal_mode:
		mov	r0, sp
		mov	r1, #BAD_DATA
		b	bad_mode

Ldata_do:	mov	r3, sp
		ldr	r4, [r0]		@ Get instruction
		mov	r2, #0
		tst	r4, #1 << 20		@ Check to see if it is a write instruction
		orreq	r2, r2, #FAULT_CODE_WRITE @ Indicate write instruction
		mov	r1, r4, lsr #22		@ Now branch to the relevent processing routine
		and	r1, r1, #15 << 2
		add	pc, pc, r1
		movs	pc, lr
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_ldrstr_post	@ ldr	rd, [rn], #m
		b	Ldata_ldrstr_numindex	@ ldr	rd, [rn, #m]	@ RegVal
		b	Ldata_ldrstr_post	@ ldr	rd, [rn], rm
		b	Ldata_ldrstr_regindex	@ ldr	rd, [rn, rm]
		b	Ldata_ldmstm		@ ldm*a	rn, <rlist>
		b	Ldata_ldmstm		@ ldm*b	rn, <rlist>
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_ldrstr_post	@ ldc	rd, [rn], #m	@ Same as ldr	rd, [rn], #m
		b	Ldata_ldcstc_pre	@ ldc	rd, [rn, #m]
		b	Ldata_unknown
Ldata_unknown:	@ Part of jumptable
		mov	r0, r1
		mov	r1, r4
		mov	r2, r3
		b	baddataabort

Ldata_ldrstr_post:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		biceq	r0, r0, #PCMASK
		mov	r1, r0
#ifdef FAULT_CODE_LDRSTRPOST
		orr	r2, r2, #FAULT_CODE_LDRSTRPOST
#endif
		b	do_DataAbort

Ldata_ldrstr_numindex:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		mov	r1, r4, lsl #20
		biceq	r0, r0, #PCMASK
		tst	r4, #1 << 23
		addne	r0, r0, r1, lsr #20
		subeq	r0, r0, r1, lsr #20
		mov	r1, r0
#ifdef FAULT_CODE_LDRSTRPRE
		orr	r2, r2, #FAULT_CODE_LDRSTRPRE
#endif
		b	do_DataAbort

Ldata_ldrstr_regindex:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		and	r7, r4, #15
		biceq	r0, r0, #PCMASK
		teq	r7, #15			@ Check for PC
		ldr	r7, [r3, r7, lsl #2]	@ Get Rm
		and	r8, r4, #0x60		@ Get shift types
		biceq	r7, r7, #PCMASK
		mov	r9, r4, lsr #7		@ Get shift amount
		and	r9, r9, #31
		teq	r8, #0
		moveq	r7, r7, lsl r9
		teq	r8, #0x20		@ LSR shift
		moveq	r7, r7, lsr r9
		teq	r8, #0x40		@ ASR shift
		moveq	r7, r7, asr r9
		teq	r8, #0x60		@ ROR shift
		moveq	r7, r7, ror r9
		tst	r4, #1 << 23
		addne	r0, r0, r7
		subeq	r0, r0, r7		@ Apply correction
		mov	r1, r0
#ifdef FAULT_CODE_LDRSTRREG
		orr	r2, r2, #FAULT_CODE_LDRSTRREG
#endif
		b	do_DataAbort

Ldata_ldmstm:
		mov	r7, #0x11
		orr	r7, r7, r7, lsl #8
		and	r0, r4, r7
		and	r1, r4, r7, lsl #1
		add	r0, r0, r1, lsr #1
		and	r1, r4, r7, lsl #2
		add	r0, r0, r1, lsr #2
		and	r1, r4, r7, lsl #3
		add	r0, r0, r1, lsr #3
		add	r0, r0, r0, lsr #8
		add	r0, r0, r0, lsr #4
		and	r7, r0, #15		@ r7 = no. of registers to transfer.
		mov	r5, r4, lsr #14		@ Get Rn
		and	r5, r5, #15 << 2
		ldr	r0, [r3, r5]		@ Get reg
		eor	r6, r4, r4, lsl #2
		tst	r6, #1 << 23		@ Check inc/dec ^ writeback
		rsbeq	r7, r7, #0
		add	r7, r0, r7, lsl #2	@ Do correction (signed)
		subne	r1, r7, #1
		subeq	r1, r0, #1
		moveq	r0, r7
		tst	r4, #1 << 21		@ Check writeback
		strne	r7, [r3, r5]
		eor	r6, r4, r4, lsl #1
		tst	r6, #1 << 24		@ Check Pre/Post ^ inc/dec
		addeq	r0, r0, #4
		addeq	r1, r1, #4
		teq	r5, #15*4		@ CHECK FOR PC
		biceq	r1, r1, #PCMASK
		biceq	r0, r0, #PCMASK
#ifdef FAULT_CODE_LDMSTM
		orr	r2, r2, #FAULT_CODE_LDMSTM
#endif
		b	do_DataAbort

Ldata_ldcstc_pre:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		mov	r1, r4, lsl #24		@ Get offset
		biceq	r0, r0, #PCMASK
		tst	r4, #1 << 23
		addne	r0, r0, r1, lsr #24
		subeq	r0, r0, r1, lsr #24
		mov	r1, r0
#ifdef FAULT_CODE_LDCSTC
		orr	r2, r2, #FAULT_CODE_LDCSTC
#endif
		b	do_DataAbort


/*
 * This is the return code to user mode for abort handlers
 */
ENTRY(ret_from_exception)
		get_thread_info tsk
		mov	why, #0
		b	ret_to_user

		.data
ENTRY(fp_enter)
		.word	fpe_not_present
		.text
/*
 * Register switch for older 26-bit only ARMs
 */
ENTRY(__switch_to)
		add	r0, r0, #TI_CPU_SAVE
		stmia	r0, {r4 - sl, fp, sp, lr}
		add	r1, r1, #TI_CPU_SAVE
		ldmia	r1, {r4 - sl, fp, sp, pc}^

/*
 *=============================================================================
 *		Low-level interface code
 *-----------------------------------------------------------------------------
 *		Trap initialisation
 *-----------------------------------------------------------------------------
 *
 * Note - FIQ code has changed.  The default is a couple of words in 0x1c, 0x20
 * that call _unexp_fiq.  Nowever, we now copy the FIQ routine to 0x1c (removes
 * some excess cycles).
 *
 * What we need to put into 0-0x1c are branches to branch to the kernel.
 */

		.section ".init.text",#alloc,#execinstr

.Ljump_addresses:
		swi	SYS_ERROR0
		.word	vector_undefinstr	- 12
		.word	vector_swi		- 16
		.word	vector_prefetch		- 20
		.word	vector_data		- 24
		.word	vector_addrexcptn	- 28
		.word	vector_IRQ		- 32
		.word	_unexp_fiq		- 36
		b	. + 8
/*
 * initialise the trap system
 */
ENTRY(__trap_init)
		stmfd	sp!, {r4 - r7, lr}
		adr	r1, .Ljump_addresses
		ldmia	r1, {r1 - r7, ip, lr}
		orr	r2, lr, r2, lsr #2
		orr	r3, lr, r3, lsr #2
		orr	r4, lr, r4, lsr #2
		orr	r5, lr, r5, lsr #2
		orr	r6, lr, r6, lsr #2
		orr	r7, lr, r7, lsr #2
		orr	ip, lr, ip, lsr #2
		mov	r0, #0
		stmia	r0, {r1 - r7, ip}
		ldmfd	sp!, {r4 - r7, pc}^

		.bss
__temp_irq:	.space	4				@ saved lr_irq
__temp_fiq:	.space	128