cpu_support.s

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

/*
 *----------------------------------------------------------------------
 *    T-Kernel
 *
 *    Copyright (C) 2004 by Ken Sakamura. All rights reserved.
 *    T-Kernel is distributed under the T-License.
 *----------------------------------------------------------------------
 *
 *    Version:   1.01.00
 *    Released by T-Engine Forum(http://www.t-engine.org) at 2004/6/28.
 *
 *----------------------------------------------------------------------
 */

/*
 *	cpu_support.S (MB87Q1100)
 *	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 "cpu_conf.h"
#include "isysconf.h"
#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. Called by 'swi 8.'
 *	_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)	|
 *		+---------------+			-
 */

/* Temporal stack used when 'dispatch_to_schedtsk' is called */

#define	TMP_STACK_SZ	(4*1024)
#define	TMP_STACK_TOP	(tmp_stack + TMP_STACK_SZ)
	.lcomm	tmp_stack, TMP_STACK_SZ

	.text
	.balign	4
	.globl	Csym(dispatch_to_schedtsk)
	.globl	Csym(dispatch_entry)

Csym(dispatch_to_schedtsk):
	/* During SVC mode/interrupt disable CPSR.I=1 F=1 */
	ldr	sp, =TMP_STACK_TOP		// Set temporal stack

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

	ldr	r4, =Csym(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(dispatch_entry):
	/* During SVC mode/interrupt disable CPSR.I=1 F=1 */
	ldr	ip, [sp]
	bic	ip, ip, #PSR_DI
	str	ip, [sp]			// SPSR_svc compensation

	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(dispatch_disabled)
	ldr	lr, =1
	str	lr, [ip]			// Dispatch disable

	msr	cpsr_c, #PSR_SVC		// Interrupt enable

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

	ldr	r4, =Csym(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
	ldr	ip, =hook_stop_jmp		// Hook processing
	ldr	pc, [ip]
  ret_hook_stop:
#endif

	ldr	r5, =Csym(schedtsk)		// R5 = &schedtsk
	ldr	r6, =Csym(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(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

	/* Switch task eigenspace */
	ldr	ip, =Csym(cur_uatb);
	ldr	r1, [r8, #TCB_tskctxb + CTXB_uatb]
	ldr	r0, [ip]
	cmp	r1, r0				// If it is the same as the current
	beq	l_nochg_uatb			// task space, no need to switch.
	str	r1, [ip]			// Update 'cur_uatb'
	cmp	r1, #0				// If the task does not have task
	beq	l_nochg_uatb			// space, no need to switch.

	ldr	r0, =0
	mcr	p15, 0, r0, cr7, c5, 0		// Invalidate instruction cache
  l_flush_dcache:
	mrc	p15, 0, r15, cr7, c14, 3	// Write back and invalidate
	bne	l_flush_dcache			// data cache

	ldr	ip, =Csym(SystemUATB)		// UATB copy
	ldr	r2, [ip]
	ldr	r3, =NUM_PDIR_ENTRIES
  l_copy_uatb:
	ldr	ip, [r1], #4
	str	ip, [r2], #4
	subs	r3, r3, #1
	bne	l_copy_uatb

	mcr	p15, 0, r0, cr8, c7, 0		// TLB disable
  l_nochg_uatb:

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

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

	ldr	ip, [r8, #TCB_reqdct]		// DCT request
	cmp	ip, #1

	ldmia	sp, {r0-r11, ip, sp, lr}^	// Context restore 
	nop
	add	sp, sp, #15*4
	ldr	lr, =TASKMODE
	str	ip, [lr]
	movne	ip, #0				// If there is no DCT request ip = 0

	ldmfd	sp!, {lr}			// R14_svc restore

	ands	ip, ip, #TMF_CPL(3)		// If the protected level is 0, DCT disable
	bne	Csym(dct_startup)		// To DCT processing

	EXC_RETURN


#if USE_DBGSPT
/*
 * 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	r1, [r8, #TCB_tskctxb + CTXB_lsid]	// lsid
	ldr	r0, [r8, #TCB_tskid]			// tskid

	ldr	ip, =Csym(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	r1, [r8, #TCB_tskctxb + CTXB_lsid]	// lsid
	ldr	r0, [r8, #TCB_tskid]			// tskid

	ldr	ip, =Csym(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(hook_dsp)
Csym(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(unhook_dsp)
Csym(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 */

/* ------------------------------------------------------------------------ */
/*
 * 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(defaulthdr_startup)
	.globl	Csym(inthdr_startup)
	.globl	Csym(exchdr_startup)
Csym(defaulthdr_startup):
	/* Unknown mode/During interrupt disable CPSR.I=1 F=? */
	mrs	lr, cpsr
	and	lr, lr, #PSR_M(31)
	cmp	lr, #PSR_FIQ
	cmpne	lr, #PSR_IRQ
	stmnefd	sp!, {r3}		// Register save
	stmfd	sp!, {r0-r2}

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

	ldr	r3, =EIT_DEFAULT * 4	// r3 = Vector table offset
	b	l_inthdr2

Csym(exchdr_startup):
	/* Unknown mode/During interrupt disable CPSR.I=1 F=? */
	stmfd	sp!, {r3}		// Register save
	b	l_inthdr1

Csym(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, =EIT_VECTBL
	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
	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

	ldr	r2, =base(AICRMN)
	ldr	r2, [r2, #offs(AICRMN)]	// r2 = level (ICRMN)

	ldr	ip, =TASKINDP		// Task independent part
	ldr	lr, [ip]
	add	lr, lr, #1
	str	lr, [ip]

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

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

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

	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

	ldr	ip, =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 save
	swi	SWI_RETINT		// tk_ret_int()


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

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

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

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

	ldr	ip, =Csym(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(hook_int)
Csym(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(unhook_int)
Csym(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 */

/*
 * tk_ret_int() processing
 *	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'