cpu_support.s

uT Kernel os source code for AT91

/*
 *----------------------------------------------------------------------
 *    micro T-Kernel
 *
 *    Copyright (C) 2006-2007 by Ken Sakamura. All rights reserved.
 *    micro T-Kernel is distributed under the micro T-License.
 *----------------------------------------------------------------------
 *
 *    Version:   1.00.00
 *    Released by T-Engine Forum(http://www.t-engine.org) at 2007/03/26.
 *
 *----------------------------------------------------------------------
 */

/*
 *	cpu_support.S (AT91)
 *	Device-Dependent CPU Operation
 */

#define	_in_asm_source_

#include <machine.h>
#include <tk/errno.h>
#include <tk/sysdef.h>
#include <tk/asm.h>
#include <sys/sysinfo.h>

#include "config.h"
#include "utk_config.h"
#if USE_TRAP | (USE_DBGSPT & USE_HOOK_TRACE)
#include "isysconf.h"
#endif
#include "tkdev_conf.h"
#include "offset.h"

/* ------------------------------------------------------------------------ */
/*
 * Dispatcher
 *	dispatch_to_schedtsk:
 *		Throw away the current contexts and forcibly dispatch
 *		to 'schedtsk.'
 *		Called directly by jump (bx) but do not return.
 *		Called on the undefined stack state (undefined 'ssp').
 *		Called on the interrupt disable state.
 *	dispatch_entry:
 *		Normal dispatch processing.
 *	_ret_int_dispatch:
 *		Called when dispatch is required by 'tk_ret_int().'
 *
 *	Contexts to save
 *	Save registers except for ssp(R13_svc) to a stack. Save 'ssp' to TCB.
 *
 *		+---------------+
 *	ssp ->	| R0 - R11	|
 *		| taskmode	|
 *		| R13_usr = usp	| Available only for RNG 1-3
 *		| R14_usr	|
 *		+---------------+
 *		| R14_svc	| R14_svc before interrupt
 *		|		| (Available only for tk_ret_int)
 *		+---------------+			-
 *		| SPSR_svc	|			| Save by interrupt 
 *		| R12     = ip	|			| entry routine
 *		| R14_svc = lr	| Return address (pc)	|
 *		+---------------+			-
 */


	.text
	.balign	4
	.globl	Csym(knl_dispatch_to_schedtsk)
	.globl	Csym(knl_dispatch_entry)

Csym(knl_dispatch_to_schedtsk):
	/* During SVC mode/interrupt disable CPSR.I=1 F=1 */
	ldr	sp, =(Csym(knl_tmp_stack) + TMP_STACK_SZ)	// Set temporal stack

	ldr	ip, =Csym(knl_dispatch_disabled)
	ldr	r0, =1
	str	r0, [ip]			// Dispatch disable

	ldr	r4, =Csym(knl_ctxtsk)		// R4 = &ctxtsk
	ldr	r0, =0
#if USE_DBGSPT
	ldr	r8, [r4]
#endif
	str	r0, [r4]			// ctxtsk = NULL

	msr	cpsr_c, #PSR_SVC		// Interrupt enable
	b	l_dispatch0

Csym(knl_dispatch_entry):
	/* During SVC mode/interrupt disable CPSR.I=1 F=1 */
#if USE_TRAP
	ldr	ip, [sp]
	bic	ip, ip, #PSR_DI
	str	ip, [sp]			// SPSR_svc compensation
#else
	stmfd	sp!, {ip, lr}
	mrs	ip, cpsr
	orr	ip, ip, #PSR_DI
	stmfd	sp!, {ip}
#endif
	stmfd	sp!, {lr}			// Context save (R14_svc)
						// The contents are invalid due to number adjustment
_ret_int_dispatch:
	/* During SVC mode/interrupt disable CPSR.I=1 F=1 */
	ldr	ip, =Csym(knl_dispatch_disabled)
	ldr	lr, =1
	str	lr, [ip]			// Dispatch disable

	msr	cpsr_c, #PSR_SVC		// Interrupt enable

	ldr	ip, =Csym(knl_taskmode)
	ldr	ip, [ip]
	sub	sp, sp, #15*4
	stmia	sp, {r0-r11, ip, sp, lr}^	// Context save 

	ldr	r4, =Csym(knl_ctxtsk)		// R4 = &ctxtsk
	ldr	r0, =0
	ldr	r8, [r4]
	str	sp, [r8, #TCB_tskctxb + CTXB_ssp] // Save 'ssp' to TCB
	str	r0, [r4]			// ctxtsk = NULL

  l_dispatch0:
	/* During interrupt enable CPSR.I=0 F=0 */
#if USE_DBGSPT & USE_HOOK_TRACE
	ldr	ip, =hook_stop_jmp		// Hook processing
	ldr	pc, [ip]
  ret_hook_stop:
#endif

	ldr	r5, =Csym(knl_schedtsk)		// R5 = &schedtsk
	ldr	r6, =Csym(knl_lowpow_discnt)	// R6 = &lowpow_discnt

  l_dispatch1:
	msr	cpsr_c, #PSR_SVC|PSR_DI		// Interrupt disable

	ldr	r8, [r5]			// R8 = schedtsk
	cmp	r8, #0				// Is there 'schedtsk'?
	bne	l_dispatch2

	/* Because there is no task that should be executed, move to the power-saving mode */
	ldr	ip, [r6]			// Is 'low_pow' disabled?
	cmp	ip, #0
	bleq	Csym(knl_low_pow)			// call low_pow()

	msr	cpsr_c, #PSR_SVC		// Interrupt enable
	b	l_dispatch1

  l_dispatch2:					// Switch to 'schedtsk'
	/* During interrupt disable CPSR.I=1 F=1 */
	str	r8, [r4]			// ctxtsk = schedtsk
	ldr	sp, [r8, #TCB_tskctxb + CTXB_ssp] // Restore 'ssp' from TCB

#if USE_DBGSPT & USE_HOOK_TRACE
	ldr	ip, =hook_exec_jmp		// Hook processing
	ldr	pc, [ip]
  ret_hook_exec:
#endif

	ldr	ip, =Csym(knl_dispatch_disabled)
	ldr	lr, =0
	str	lr, [ip]			// Dispatch enable 

	ldmia	sp, {r0-r11, ip, sp, lr}^	// Context restore 
	nop
	add	sp, sp, #15*4
	ldr	lr, =Csym(knl_taskmode)
	str	ip, [lr]

	ldmfd	sp!, {lr}			// R14_svc restore

	EXC_RETURN


#if USE_DBGSPT & USE_HOOK_TRACE
/*
 * Task dispatcher hook routine call
 *	void stop( ID tskid, INT lsid, UINT tskstat )
 *	void exec( ID tskid, INT lsid )
 */
	.text
	.balign	4
hook_stop:
	cmp	r8, #0			// r8 = ctxtsk
	beq	l_notask

	ldrb	r2, [r8, #TCB_state]			// tskstat
	mov	r2, r2, lsl #1
	ldr	r0, [r8, #TCB_tskid]			// tskid

	ldr	ip, =Csym(knl_hook_stopfn)
	ldr	ip, [ip]
	mov	lr, pc
	bx	ip			// call stop(tskid, lsid, tskstat)

  l_notask:
	b	ret_hook_stop

hook_exec:
					// r8 = ctxtsk
	ldr	r0, [r8, #TCB_tskid]			// tskid

	ldr	ip, =Csym(knl_hook_execfn)
	ldr	ip, [ip]
	mov	lr, pc
	bx	ip			// call exec(tskid, lsid)

	b	ret_hook_exec

/*
 * Set/Free task dispatcher hook routine
 */
	.text
	.balign	4
	.globl	Csym(knl_hook_dsp)
Csym(knl_hook_dsp):
	ldr	r0, =hook_exec_jmp
	ldr	r1, =hook_stop_jmp
	ldr	r2, =hook_exec
	ldr	r3, =hook_stop
	str	r2, [r0]
	str	r3, [r1]
	bx	lr

	.globl	Csym(knl_unhook_dsp)
Csym(knl_unhook_dsp):
	ldr	r0, =hook_exec_jmp
	ldr	r1, =hook_stop_jmp
	ldr	r2, =ret_hook_exec
	ldr	r3, =ret_hook_stop
	str	r2, [r0]
	str	r3, [r1]
	bx	lr

			.data
			.balign	4
  hook_exec_jmp:	.long	ret_hook_exec
  hook_stop_jmp:	.long	ret_hook_stop

#endif /* USE_DBGSPT & USE_HOOK_TRACE */

#if USE_HLL_INTHDR
/* ------------------------------------------------------------------------ */
/*
 * High level programming language routine for interrupt handler
 *	Called by interrupt entry routine on the state saved in the interrupt 
 *	stack as shown below.
 *		+---------------+
 *	ssp ->	| R3		| Only FIQ, IRQ
 *		+---------------+
 *	ssp ->	| SPSR		|
 *		| R12 = ip	|
 *		| R14 = lr	|
 *		+---------------+
 *
 *	The vector table address is set in 'ip.'
 *	(ip - EIT_VECTBL) / 4 = Vector number
 */
	.text
	.balign	4
	.globl	Csym(knl_inthdr_startup)
	.globl	Csym(knl_exchdr_startup)
Csym(knl_exchdr_startup):
	/* Unknown mode/During interrupt disable CPSR.I=1 F=? */
	stmfd	sp!, {r3}		// Register save
	b	l_inthdr1

Csym(knl_inthdr_startup):
	/* Unknown mode/During interrupt disable CPSR.I=1 F=? */
	mrs	lr, cpsr
	and	lr, lr, #PSR_M(31)
	cmp	lr, #PSR_SVC
	stmeqfd	sp!, {r3}		// If it is SWI, also save 'r3'
  l_inthdr1:
	stmfd	sp!, {r0-r2}		// Register save

	ldr	r3, =Csym(knl_intvec)
	sub	r3, ip, r3		// r3 = Vector table offset

					// Argument of handler
	mov	r0, r3, lsr #2		// r0 = dintno
	add	r1, sp, #4*4		// r1 = sp

  l_inthdr2:
	mrs	r2, cpsr		// r2 = CPSR save
#if USE_TRAP
	msr	cpsr_c, #PSR_SVC|PSR_DI	// Move to SVC mode/Interrupt disable

	stmfd	sp!, {r2, lr}		// SVC mode register save
	stmfd	sp!, {ip}		// In normal situation, save only for FIQ
#else
	msr	cpsr_c, #PSR_IRQ|PSR_DI	// Move to IRQ mode/Interrupt disable

	stmfd	sp!, {r2, lr}
	stmfd	sp!, {ip}
#endif

	ldr	ip, =Csym(knl_taskindp)		// Task independent part
	ldr	lr, [ip]
	add	lr, lr, #1
	str	lr, [ip]

#if USE_DBGSPT & USE_HOOK_TRACE
	ldr	ip, =hook_ienter_jmp
	ldr	pc, [ip]
  ret_hook_ienter:
#endif

	ldr	ip, =Csym(knl_hll_inthdr)
	ldr	ip, [ip, r3]
	mov	lr, pc
	bx	ip			// call hll_inthdr[n](dintno, sp)

#if USE_DBGSPT & USE_HOOK_TRACE
	ldr	ip, =hook_ileave_jmp
	ldr	pc, [ip]
  ret_hook_ileave:
#endif

#if USE_TRAP
	ldmfd	sp!, {ip}		// SVC mode register restore
	ldmfd	sp!, {r2, r3}		// r2 = Original mode
					// r3 = R14_svc
	orr	r2, r2, #PSR_DI
	msr	cpsr_c, r2		// Return to original mode/Interrupt disable
#else
	ldmfd	sp!, {ip}		// IRQ mode register restore
	ldmfd	sp!, {r2, r3}		// r2 = Original mode
					// r3 = R14_irq
	orr	r2, r2, #PSR_DI
	msr	cpsr_c, r2
#endif

	ldr	ip, =Csym(knl_taskindp)
	ldr	lr, [ip]
	sub	lr, lr, #1
	str	lr, [ip]

	ldmfd	sp!, {r0-r2}		// Register restore
	swp	r3, r3, [sp]		// r3 restore, R14_svc/irq save
#if USE_TRAP
	swi	SWI_RETINT
#else
	bl	tk_ret_int_impl
#endif


#if USE_DBGSPT & USE_HOOK_TRACE
/*
 * Interrupt handler hook routine call
 */
	.text
	.balign	4
hook_ienter:
	stmfd	sp!, {r0, r1}		// Register save
	stmfd	sp!, {r3}

	ldr	ip, =Csym(knl_hook_ienterfn)
	ldr	ip, [ip]
	mov	lr, pc
	bx	ip			// call enter(dintno, sp)

	ldmfd	sp!, {r3}		// Register restore
	ldmfd	sp, {r0, r1}		// Leave 'dintno,' 'sp' on stack
	b	ret_hook_ienter

hook_ileave:
	ldmfd	sp!, {r0, r1}		// Restore 'dintno,' 'sp' 

	ldr	ip, =Csym(knl_hook_ileavefn)
	ldr	ip, [ip]
	mov	lr, pc
	bx	ip			// call leave(dintno, info)

	b	ret_hook_ileave

/*
 * Set/Free interrupt handler hook routine
 */
	.text
	.balign	4
	.globl	Csym(knl_hook_int)
Csym(knl_hook_int):
	ldr	r0, =hook_ienter_jmp
	ldr	r1, =hook_ileave_jmp
	ldr	r2, =hook_ienter
	ldr	r3, =hook_ileave
	str	r2, [r0]
	str	r3, [r1]
	bx	lr

	.globl	Csym(knl_unhook_int)
Csym(knl_unhook_int):
	ldr	r0, =hook_ienter_jmp
	ldr	r1, =hook_ileave_jmp
	ldr	r2, =ret_hook_ienter
	ldr	r3, =ret_hook_ileave
	str	r2, [r0]
	str	r3, [r1]
	bx	lr

			.data
			.balign	4
  hook_ienter_jmp:	.long	ret_hook_ienter
  hook_ileave_jmp:	.long	ret_hook_ileave

#endif /* USE_DBGSPT & USE_HOOK_TRACE */
#endif /* USE_HLL_INTHDR */

/*
 * tk_ret_int_impl()
 *
 *	When called, the interrupt stack is configured as shown below.
 *		+---------------+
 *	ssp ->	| SPSR_svc	| Save by 'swi SWI_RETINT'
 *		| R12_usr	|
 *		| R14_svc	|
 *		+---------------+
 *
 *		+---------------+
 *	isp ->	| R14_svc	| Save when calling 'tk_ret_int'
 *		+---------------+
 *		| SPSR_xxx	| Save when the interrupt occurs
 *		| R12_xxx	|
 *		| R14_xxx	| <- Return address
 *		+---------------+
 */
	.text
	.balign	4
	.globl	Csym(tk_ret_int_impl)