cpu_support.s

日本著名的的嵌入式实时操作系统T-Kernel的源码及用户手册。

 *		+---------------+
 *		| SPSR_xxx	| Save when the interrupt occurs
 *		| R12_xxx	|
 *		| R14_xxx	| <- Return address
 *		+---------------+
 */
	.text
	.balign	4
	.globl	Csym(_tk_ret_int)
Csym(_tk_ret_int):
	ldr	ip, [sp]		// ip = SPSR

	and	lr, ip, #PSR_M(31)
	cmp	lr, #PSR_SVC
	beq	l_retint_svc		// Is it 'tk_ret_int' from SWI?

	stmfd	sp!, {r2, r3}		// Save 'r2' for work (Save 'r3' for acquiring location)
	add	r2, sp, #4

	orr	ip, ip, #PSR_DI
	bic	ip, ip, #PSR_T
	cmp	lr, #PSR_FIQ
	msr	cpsr_c, ip		// Move to interrupted mode/Interrupt disable

	ldmfd	sp!, {ip, lr}		// Copy from 'isp' to 'ssp'/Free 'isp'
	str	ip, [r2, #0*4]		// R14_svc
	str	lr, [r2, #1*4]		// SPSR_xxx
	ldmfd	sp!, {ip, lr}
	strne	ip, [r2, #2*4]		// R12_xxx (except for FIQ)
	str	lr, [r2, #3*4]		// R14_xxx (Return address)

	msr	cpsr_c, #PSR_SVC|PSR_DI	// Move to SVC mode/Interrupt disable

	ldmfd	sp!, {r2}		// r2 restore
	b	l_retint1

  l_retint_svc:
	add	sp, sp, #3*4		// Trash parts saved by 'swi SWI_RETINT'

	msr	cpsr_c, #PSR_SVC|PSR_DI	// Interrupt disable

  l_retint1:
	ldr	ip, =TASKINDP		// Is it a nesting interrupt?
	ldr	ip, [ip]
	cmp	ip, #0
	bne	l_nodispatch

	ldr	ip, =Csym(dispatch_disabled)	// Is it during dispatch disable?
	ldr	ip, [ip]
	cmp	ip, #0
	bne	l_nodispatch

	ldr	ip, [sp, #4]		// SPSR
	tst	ip, #PSR_I|PSR_F	// Is it an exception during interrupt disable?
	bne	l_nodispatch

	ldr	ip, =Csym(ctxtsk)	// Is dispatch required?
	ldr	lr, =Csym(schedtsk)
	ldr	ip, [ip]
	ldr	lr, [lr]
	cmp	ip, lr
	bne	_ret_int_dispatch	// To dispatch processing

  l_nodispatch:
	ldmfd	sp!, {lr}		// lr restore 
	EXC_RETURN


/* ------------------------------------------------------------------------ */

/*
 * Unsupported system call
 */
	.text
	.balign	4
	.globl	Csym(no_support)
Csym(no_support):
	ldr	r0, =E_RSFN
	bx	lr

/*
 * System call entry table
 */
	.text
	.balign	4
_svctbl:
	.int	Csym(no_support)
#define	_tk_ret_int	no_support
#include <sys/svc/tksvctbl.h>
#undef	_tk_ret_int

/*
 * System call entry
 *	Do not need to save the temporary register.
 *	The compiler saves the permanent register.
 *		+---------------+
 *	ssp ->	| SPSR		|
 *		| ip		| Function code
 *		| lr		| Return address
 *		+---------------+
 */
	.text
	.balign	4
	.globl	Csym(call_entry)
Csym(call_entry):
	/* SVC mode/During interrupt disable CPSR.I=1 F=? */
	ldr	ip, [sp]
	and	ip, ip, #PSR_DI
	orr	ip, ip, #PSR_SVC
	msr	cpsr_c, ip		// Return interrupt disable state to caller's state

	stmfd	sp!, {r10, fp}		// Save register for work
	add	fp, sp, #2*4

	ldr	ip, =TASKMODE		// Task mode flag update
	ldr	r10, [ip]
	stmfd	sp!, {r10}		// taskmode save 
	mov	lr, r10, lsl #16
	str	lr, [ip]

#if USE_DBGSPT
	ldr	ip, =hook_enter_jmp	// Hook processing
	ldr	pc, [ip]
  ret_hook_enter:
#endif

	ldr	lr, [fp, #4]		// lr = Function code
	cmp	lr, #0			//	< 0: System call
	bge	l_esvc_function		//	>= 0: Extended SVC

	/* T-Kernel system call */
	ldr	ip, =TASKINDP		// If it is a call from the task independent part,
	ldr	ip, [ip]		// protected level check is not necessary.
	cmp	ip, #0
	bhi	l_nochklevel

	ldr	ip, =Csym(svc_call_limit)
	ldr	ip, [ip]		// Limit of protected level
	and	r10, r10, #TMF_CPL(3)	// r10 = taskmode when calling
	cmp	r10, ip
	bhi	l_oacv_err
  l_nochklevel:

	mov	r10, lr, asr #16	// r10 = Function number
	ldr	ip, =N_TFN + 0xffff8000
	cmp	r10, ip
	bgt	l_illegal_svc

	mov	lr, lr, lsr #8
	and	lr, lr, #0xff		// lr = Number of arguments
	cmp	lr, #5
	bne	l_nocopy
	ldr	ip, [r4]		// Copy fifth argument
	stmfd	sp!, {ip}
  l_nocopy:

	ldr	ip, =_svctbl - (0xffff8000 << 2)
	mov	lr, pc
	ldr	pc, [ip, r10, lsl #2]	// T-Kernel system call

  l_retsvc:
#if USE_DBGSPT
	ldr	ip, =hook_leave_jmp	// Hook processing
	ldr	pc, [ip]
  ret_hook_leave:
#endif

	sub	sp, fp, #3*4
	ldmfd	sp!, {r1, r10, fp}	// Restore register for work

	ldr	ip, =TASKMODE		// Task mode restore
	str	r1, [ip]

	ands	r1, r1, #TMF_CPL(3)	// If it is not a call from protected level 0
	beq	l_nodct			// Do not process DCT

	msr	cpsr_c, #PSR_SVC|PSR_DI	// Interrupt disable

	ldr	ip, =TASKINDP		// If it is a call from the task independent part
	ldr	ip, [ip]		// Do not process DCT
	cmp	ip, #0
	bne	l_nodct

	ldr	ip, =Csym(ctxtsk)	// Is there a DCT request?
	ldr	ip, [ip]
	ldr	ip, [ip, #TCB_reqdct]
	cmp	ip, #1
	bne	l_nodct

	ldr	ip, =Csym(dispatch_disabled)
	ldr	ip, [ip]		// During dispatch disable
	cmp	ip, #0			// Do not process DCT
	bne	l_nodct

	b	Csym(dct_startup)	// To DCT processing

  l_nodct:
	EXC_RETURN


  l_esvc_function:
	/* Extended SVC */
	mov	r1, lr			// r1 = Function code
	bl	Csym(svc_ientry)	// svc_ientry(pk_para, fncd)

	b	l_retsvc


  l_illegal_svc:
	ldr	r0, =E_RSFN
	b	l_retsvc

  l_oacv_err:
	ldr	r0, =E_OACV
	b	l_retsvc


#if USE_DBGSPT

/*
 * System call/Extended SVC hook routine call
 *	VP enter( FN fncd, TD_CALINF *calinf, ... )
 *	void leave( FN fncd, INT ret, VP exinf )
 *
 *	typedef struct td_calinf {
 *		VP	ssp;	System stack pointer
 *		VP	r11;	Flame pointer when calling
 *	} TD_CALINF;
 */
	.text
	.balign	4
hook_enter:
	stmfd	sp!, {r0-r3, r8-r9}	// Save argument and register for work
	mov	r8, sp			// r8 = Keep stack position

	ldr	ip, [fp, #-4]		// Flame pointer when calling
	stmfd	sp!, {fp, ip}		// Create TD_CALINF
	mov	r9, sp			// r9 = &TD_CALINF

	ldr	lr, [fp, #4]		// lr = Function code
	cmp	lr, #0			//	< 0: System call
	bge	l_hooksvc		//	>= 0: Extended SVC

	mov	lr, lr, lsr #8
	and	lr, lr, #0xff		// Number of arguments

	cmp	lr, #5
	ldrge	ip, [r4]
	stmgefd	sp!, {ip}		// Fifth argument
	cmp	lr, #4
	stmgefd	sp!, {r3}		// Fourth argument
	cmp	lr, #3
	stmgefd	sp!, {r2}		// Third argument
	mov	r3, r1			// Second argument
  l_hooksvc:
	mov	r2, r0			// First argument
	mov	r1, r9			// calinf
	ldr	r0, [fp, #4]		// fncd
	ldr	ip, =Csym(hook_enterfn)
	ldr	ip, [ip]
	mov	lr, pc
	bx	ip			// exinf = enter(fncd, ...)
	mov	r9, r0			// Temporarily save 'exinf'

	mov	sp, r8			// Return stack position
	ldmfd	sp!, {r0-r3, r8}	// Restore argument and register for work
	swp	r9, r9, [sp]		// Restore 'r9' and save 'exinf' in it
	b	ret_hook_enter

hook_leave:
	mov	r1, r0			// r1 = ret

	mov	lr, #0
	ldr	r0, [fp, #4]		// r0 = Function code
	cmp	r0, #0			//	< 0: System call
	bge	l_hooksvc2		//	>= 0: Extended SVC

	mov	lr, r0, lsr #8
	and	lr, lr, #0xff		// Number of arguments
	subs	lr, lr, #4
	movlt	lr, #0
  l_hooksvc2:

	add	lr, lr, #3		// Whether 'hook_enter' is executed
	sub	ip, sp, fp		// Check by stack usage
	sub	sp, fp, #4*4		// Location in which 'sp = exinf' is saved
	cmp	lr, ip, lsr #2		// If 'hook_enter' is executed,
	ldrne	r2, [sp]		// Get 'exinf' from stack
	ldreq	r2, =0			// If 'exinf' is not saved, 0

	str	r9, [sp]		// r9 save
	mov	r9, r1			// Save 'ret' in 'r9'

	ldr	ip, =Csym(hook_leavefn)
	ldr	ip, [ip]
	mov	lr, pc
	bx	ip			// call leave(fncd, ret, exinf)

	mov	r0, r9			// r0 = ret restore
	ldmfd	sp!, {r9}		// r9 restore
	b	ret_hook_leave

/*
 * Set/Free system call/extended SVC hook routine 
 */
	.text
	.balign	4
	.globl	Csym(hook_svc)
Csym(hook_svc):
	ldr	r0, =hook_enter_jmp
	ldr	r1, =hook_leave_jmp
	ldr	r2, =hook_enter
	ldr	r3, =hook_leave
	str	r2, [r0]
	str	r3, [r1]
	bx	lr

	.globl	Csym(unhook_svc)
Csym(unhook_svc):
	ldr	r0, =hook_enter_jmp
	ldr	r1, =hook_leave_jmp
	ldr	r2, =ret_hook_enter
	ldr	r3, =ret_hook_leave
	str	r2, [r0]
	str	r3, [r1]
	bx	lr

			.data
			.balign	4
  hook_enter_jmp:	.long	ret_hook_enter
  hook_leave_jmp:	.long	ret_hook_leave

#endif /* USE_DBGSPT */

/* ------------------------------------------------------------------------ */

#if USE_DBGSPT
/*
 * Debugger support function service call entry table
 */
	.text
	.balign	4
_tdsvctbl:
	.int	Csym(no_support)
#include <sys/svc/tdsvctbl.h>

/*
 * Debugger support function service call entry
 */
	.text
	.balign	4
	.globl	Csym(call_dbgspt)
Csym(call_dbgspt):
	/* SVC mode/During interrupt disable CPSR.I=1 F=? */
	ldr	ip, [sp]
	and	ip, ip, #PSR_I|PSR_F
	orr	ip, ip, #PSR_SVC
	msr	cpsr_c, ip		// Return interrupt disable state to caller's state

	stmfd	sp!, {r10, fp}		// Save register for work
	add	fp, sp, #2*4

	ldr	ip, =TASKMODE
	ldr	lr, =Csym(svc_call_limit)
	ldr	ip, [ip]
	ldr	lr, [lr]		// Call protected level limit
	and	ip, ip, #TMF_CPL(3)	// Protected level when calling
	cmp	ip, lr
	bhi	b_oacv_err

	ldr	lr, [fp, #4]		// lr = Function code
	mov	r10, lr, asr #16
	ldr	ip, =N_TDFN + 0xffff8000
	cmp	r10, ip
	bgt	b_illegal_svc

	ldr	ip, =_tdsvctbl - (0xffff8000 << 2)
	mov	lr, pc
	ldr	pc, [ip, r10, lsl #2]	// T-Kernel/DS service call

  b_retsvc:
	ldmfd	sp!, {r10, fp}		// Restore register for work
	EXC_RETURN


  b_illegal_svc:
	ldr	r0, =E_RSFN
	b	b_retsvc

  b_oacv_err:
	ldr	r0, =E_OACV
	b	b_retsvc

#endif /* USE_DBGSPT */

/* ------------------------------------------------------------------------ */
/*
 * High level programming language routine for timer handler
 */
	.text
	.balign	4
	.globl	Csym(timer_handler_startup)
Csym(timer_handler_startup):
	/* IRQ mode/During interrupt disable CPSR.I=1 F=? */
	msr	cpsr_c, #PSR_SVC|PSR_DI	// Move to SVC mode/Interrupt disable

	stmfd	sp!, {r0-r2, r4-r6, lr}	// Save register
	ldr	r0, =base(IRCA(0))	// Enable nesting interrupt
	ldr	r6, [r0, #offs(AILM)]
	ldr	r3, [r0, #offs(AICRMN)]
	str	r3, [r0, #offs(AILM)]
	str	r0, [r0, #offs(AIRQF)]

	ldr	r4, =TASKINDP		// Enter task independent part
	ldr	r5, [r4]
	add	r0, r5, #1
	str	r0, [r4]

	bl	Csym(timer_handler)	// call timer_handler()
	/* Return by interrupt disable CPSR.I=1 F=1 */

	str	r5, [r4]		// Leave task independent part

	ldr	ip, =base(AILM)
	str	r6, [ip, #offs(AILM)]	// Restore interrupt disable level
	ldmfd	sp!, {r0-r2, r4-r6, lr}	// Restore register

	TK_RET_INT_FIQ PSR_IRQ		// tk_ret_int()

/* ------------------------------------------------------------------------ */
/*
 * Delayed context trap (DCT)
 *	Task exception handler startup
 *	When called, the interrupt stack is configured as shown below.
 *		+---------------+
 *	ssp ->	| SPSR_svc	|
 *		| R12     = ip	|
 *		| R14_svc = lr	| Return address (pc)
 *		+---------------+
 */
	.text
	.balign	4
	.globl	Csym(dct_startup)
Csym(dct_startup):
	/* SVC mode/During interrupt disable CPSR.I=1 F=1 */
	stmfd	sp!, {r0-r3, lr}	// Register save

	add	r0, sp, #5*4
	bl	Csym(setup_texhdr)	// call setup_texhdr(ssp)
	/* Return in interrupt enable state */

	ldmfd	sp!, {r0-r3, lr}	// Register restore
	EXC_RETURN

/* ------------------------------------------------------------------------ */