/*
 *  TOPPERS/OSEK Kernel
 *      Toyohashi Open Platform for Embedded Real-Time Systems/
 *      OSEK Kernel
 * 
 *  Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
 *                              Toyohashi Univ. of Technology, JAPAN
 *  Copyright (C) 2004 by Embedded and Real-Time Systems Laboratory
 *              Graduate School of Information Science, Nagoya Univ., JAPAN
 *  Copyright (C) 2006 by Witz Corporation, JAPAN
 *  Copyright (C) 2010 by Meika Sugimoto
 * 
 *  上記著作権者は，以下の (1)〜(4) の条件か，Free Software Foundation 
 *  によって公表されている GNU General Public License の Version 2 に記
 *  述されている条件を満たす場合に限り，本ソフトウェア（本ソフトウェア
 *  を改変したものを含む．以下同じ）を使用・複製・改変・再配布（以下，
 *  利用と呼ぶ）することを無償で許諾する．
 *  (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
 *      権表示，この利用条件および下記の無保証規定が，そのままの形でソー
 *      スコード中に含まれていること．
 *  (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
 *      用できる形で再配布する場合には，再配布に伴うドキュメント（利用
 *      者マニュアルなど）に，上記の著作権表示，この利用条件および下記
 *      の無保証規定を掲載すること．
 *  (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
 *      用できない形で再配布する場合には，次のいずれかの条件を満たすこ
 *      と．
 *    (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
 *        作権表示，この利用条件および下記の無保証規定を掲載すること．
 *    (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
 *        報告すること．
 *  (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
 *      害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
 * 
 *  本ソフトウェアは，無保証で提供されているものである．上記著作権者お
 *  よびTOPPERSプロジェクトは，本ソフトウェアに関して，その適用可能性も
 *  含めて，いかなる保証も行わない．また，本ソフトウェアの利用により直
 *  接的または間接的に生じたいかなる損害に関しても，その責任を負わない．
 * 
 */

/*	プロセッサ依存モジュール アセンブリ言語部（SH2用）*/
 
#define _MACRO_ONLY
#include "sys_defs.h"

/*	外部参照宣言 */
	.globl	_runtsk
	.globl	_schedtsk
	.globl	_tcxb_pc
	.globl	_tcxb_sp
	.globl	_tinib_task
	.globl	_callevel
	.globl	_runisr
	.globl	_tinib_exepri
	.globl	_tcb_curpri
	.globl	_call_pretaskhook
	.globl	_call_posttaskhook
	.globl	_PreTaskHook
	.globl	_PostTaskHook
	.globl	_lock_flag
	.globl	_intmask

/*	外部公開宣言 */
	.globl	_dispatch
	.globl	_start_dispatch
	.globl	_exit_and_dispatch
	.globl	_activate_r
	.globl	_interrupt
	.globl	dispatch_r

.section	".text" , "ax"
.align	2

/*
 *	タスクディスパッチャ
 *
 *	呼び出し条件:
 *		割込み：IPL=0, FLGレジスタIビット=0 (CPUロック状態)
 *		コンテキスト：callevel=1(タスクコンテキスト)
 *		スタック： タスクスタック
*/

_dispatch:
	/* スタックへレジスタ保存 */
	mov.l	 r8 , @-r15
	mov.l	 r9 , @-r15
	mov.l	r10 , @-r15
	mov.l	r11 , @-r15
	mov.l	r12 , @-r15
	mov.l	r13 , @-r15
	mov.l	r14 , @-r15
	sts.l	pr , @-r15
	mov.l	runtsk_adr , r1
	mov.b	@r1 , r0
	extu.b	r0 , r0
	shll2	r0
	mov.l	tcxb_sp_adr , r2
	mov.l	r15 , @(r0 , r2)	/* スタックポインタをTCBに保存 */
	mov.l	dispatch_r_adr , r3
	mov.l	tcxb_pc_adr , r2	/* 実行再開番地をTCBに保存 */
	mov.l	r3 , @(r0 , r2)
	bra		dispatcher			/* dispatcherへ */
	nop
	
.align	4
dispatch_r_adr:
	.long	dispatch_r

/*
 *	呼び出し条件:
 *		割込み：IPL=0xf, (CPUロック状態)
 *		コンテキスト：callevel=1(タスクコンテキスト)
 *		スタック： タスクスタック
 */

.align	2
dispatch_r:
	/* タスクスタックからレジスタ復帰 */
	lds.l	@r15+ ,  pr
	mov.l	@r15+ , r14
	mov.l	@r15+ , r13
	mov.l	@r15+ , r12
	mov.l	@r15+ , r11
	mov.l	@r15+ , r10
	mov.l	@r15+ ,  r9
	mov.l	@r15+ ,  r8
	rts						/* dispatch 呼び出し元へ戻る. */
	nop

/*
 *	タスク起動時処理
 *	ここでは, CPUロック解除状態にし, タスクを起動する.
 *
 *	呼出条件: FLGレジスタIビット=0xf (CPUロック状態), タスクスタック
 *			callevel = 1(タスクコンテキスト), タスクスタック
 */

.align	2
_activate_r:
	mov.l	runtsk_adr	,r2			/* 実行開始タスクID */
	mov.b	@r2 , r0
	extu.b	r0 , r0
	mov.l	tinib_exepri_adr , r2		/* 初期優先度の読み込み */
	mov.b	@(r0 , r2) , r1
	extu.b	r1 , r1
	mov.l	tcb_curpri_adr , r3		/* 現在優先度に設定 */
	mov.b	r1 , @(r0 , r3)
	
	mov		#0 , r3
	shll2	r0
	mov.l	tinib_task_adr , r1
	mov.l	@(r0 , r1) , r2
	jmp		@r2
	ldc		r3 , sr				/* 遅延スロット、割り込み許可 */

.align	4
tinib_exepri_adr:
	.long	_tinib_exepri
tcb_curpri_adr:
	.long	_tcb_curpri
tinib_task_adr:
	.long	_tinib_task

/*
 *	dispatcher呼び出し条件:
 *		・すべてのタスクのコンテキストは保存されている.
 *		・SRレジスタIPLビット=0xf (CPUロック状態),
 *		・コンテキストはカーネル起動時を除きタスクコンテキスト(callevel=1)
 *				callevelの初期値は1？
 *		起動時は非タスクコンテキスト(callevel=1)で, その場合にも対応.
 *	dispatcher 呼出時のスタック:
 *		__kernel_dispatch からきた場合: タスクスタック
 *		exit_and_dispatch からきた場合: 
 *			exit_task からきた場合はタスクスタック
 *			カーネル起動時は割込みスタック
 *		ret_int からきた場合: タスクスタック
 *		dispatcher_1 での割込み待ちからきた場合: 割込みスタック
 */

.align	2
_exit_and_dispatch:
dispatcher:
	/* ポストタスクフックの呼び出し */
	mov.l	PostTaskHook_adr , r0
	tst		r0 , r0
	bt		_start_dispatch
	mov.l	call_posttaskhook_adr , r0
	jsr		@r0
	nop
_start_dispatch:
	mov.l	schedtsk_adr, r0	/* schedtsk を runtsk に */
	mov.b	@r0 , r2
	extu.b	r2 , r2
	mov.l	runtsk_adr , r1
	mov.b	r2 , @r1
	
	mov.l	invalid_tsk , r0
	cmp/eq	r0 , r2		/* schedtsk が無効ならばアイドルループへ */
	bt		pre_idle
	shll2	r2
	mov.l	tcxb_sp_adr , r0	
	mov.l	@(r0 , r2) , r15		/* タスクスタックポインタを復帰 */
	mov.l	tcxb_pc_adr , r0
	mov.l	@(r0 , r2) , r3
	mov.l	PreTaskHook_adr , r1
	tst		r1 , r1
	bt		PreTaskHook_r
	mov.l	call_pretaskhook_adr , r1
	mov.l	r3 , @-r15		/* PreTaskHook用に戻りPCをスタック退避 */
	jsr		@r1
	nop
	mov.l	@r15+ , r3		/* 戻りPCを戻す */
PreTaskHook_r:
	jmp		@r3				/* 戻り番地へ */
	nop

.align	4
_activate_r_adr:
	.long	_activate_r
PreTaskHook_adr:
	.long	_PreTaskHook
PostTaskHook_adr:
	.long	_PostTaskHook
call_pretaskhook_adr:
	.long	_call_pretaskhook
call_posttaskhook_adr:
	.long	_call_posttaskhook

/*
 *	ここからは schedtsk がない場合
 */
.align	2
pre_idle:
	/*
	 *	ここで非タスクコンテキスト,割込みスタックに切り換えたのは,
	 *	ここで発生する割込み処理にどのスタックを使うかという問題の解決と,
	 *	割込みハンドラ内でのディスパッチ防止という2つの意味がある．
	 */
	mov.l	int_stack, r15		/* 割込み用のスタックへ切替え */
	mov.l	callevel_adr , r0
	mov		#TCL_ISR2 , r1
	mov.b	r1 , @r0			/* 非タスクコンテキスト */
	/* アイドルループの準備 */
	mov		#0 , r2
	mov.l	_lock_flag_adr , r8
	mov.l	r2 , @r8
	mov.l	mask_ipm , r3
	mov.l	schedtsk_adr , r5
	mov.l	invalid_tsk , r4
idle_loop:
	ldc		r2 , sr				/* 割り込み待ち */
	nop
	ldc		r3 , sr				/* 割り込み許可 */
	mov.b	@r5 , r1			/* schedtskを読み出し */
	extu.b	r1 , r1
	cmp/eq	r4 , r1				/* スケジュールすべきタスクがあるか？ */
	bt		idle_loop			/* 再びアイドルループ */
	
	mov		#TCL_TASK , r3
	mov.b	r3 , @r0			/* callevelを戻す */
	mov		#1 , r2
	mov.l	r2 , @r8
	/* CPUロック状態に */
	xor		r0 , r0
	mov.l	_intmask_adr , r1
	mov.l	r0 , @r1
	mov.l	_lock_flag_adr , r1
	mov		#1 , r0
	mov.l	r0 , @r1
	bra		_start_dispatch		/* ディスパッチャへ */
	nop

/*
 *  割込みハンドラ出口処理(ディスパッチ要求あり)
 *
 *	呼出し条件:
 *	・FLGレジスタのIPL=0xf, タスクコンテキスト(callevel=1)
 *	・使用スタックはタスクスタック
 *	・タスクコンテキストはr0-r7,mach,machl,gbr,prがスタックに積まれた状態
 */
	.align	2
	.globl	ret_int
ret_int:
	/* 残りのレジスタを保存 */
	mov.l	r8 , @-r15
	mov.l	r9 , @-r15
	mov.l	r10 , @-r15
	mov.l	r11 , @-r15
	mov.l	r12 , @-r15
	mov.l	r13 , @-r15
	mov.l	r14 , @-r15
	shll2	r0						/* runtskはr0に入っている */
	mov.l	tcxb_sp_adr , r1
	mov.l	r15 , @(r0 , r1)		/* スタックポインタを保存 */
	mov.l	tcxb_pc_adr , r1
	mov.l	ret_int_r_adr , r2		/* 戻り番地をret_int_rに設定 */
	mov.l	r2 , @(r0 , r1)			/* スタックポインタを保存 */
	/* CPUロック状態に */
	xor		r0 , r0
	mov.l	_intmask_adr , r1
	mov.l	r0 , @r1
	mov.l	_lock_flag_adr , r1
	mov		#1 , r0
	mov.l	r0 , @r1
	bra		dispatcher				/* ディスパッチャへ */
	nop

.align	4
ret_int_r_adr:
	.long	ret_int_r
_intmask_adr:
	.long	_intmask
_lock_flag_adr:
	.long	_lock_flag


/*
 *	呼び出し条件: IPL=0xf, (CPUロック状態),
 *			callevel=1(タスクコンテキスト), タスクスタック
 *	スタックにはr14-r8,gbr,mach,machl,pr,r7-r0の順で積まれている
 */
.align	2
ret_int_r:
	mov.l	@r15+ , r14		/* レジスタを復帰 */
	mov.l	@r15+ , r13
	mov.l	@r15+ , r12
	mov.l	@r15+ , r11
	mov.l	@r15+ , r10
	mov.l	@r15+ , r9
	mov.l	@r15+ , r8
	ldc.l	@r15+ , gbr
	lds.l	@r15+ , mach
	lds.l	@r15+ , macl
	lds.l	@r15+ , pr
	mov.l	@r15+ , r7
	mov.l	@r15+ , r6
	mov.l	@r15+ , r5
	mov.l	@r15+ , r4
	mov.l	@r15+ , r3
	mov.l	@r15+ , r2
	mov.l	@r15+ , r1
	mov.l	@r15+ , r0
	rte						/* 割り込み元へ復帰 */
	nop

/*
 *	割込み出入口処理(アセンブリ言語記述部分)
 *
 *	呼出し条件:
 *	・SRレジスタのIPLは0xf
 *	・スタックは多重割り込みなら割込みスタック, そうでなければ
 *	  タスクスタック
 *	・r0 には割込みハンドラのアドレスが格納されている.
 *	・r1 には前回の実行中割込みIDが格納されている．
 *	・r2 には割り込み受付時のSRレジスタの値が格納されている
 *
 *	レジスタがスタック上にどのように保存されているかを以下に示す.
 *	この図では上が低位, 下が高位のアドレスで, スタックは下から
 *	上方向に向かって積み上げられるものとする.
 *
 *	-------------------------
 *	|       R3(4byte)        |
 *	--------------------------
 *	|       R2(4byte)        |
 *	--------------------------
 *	|       R1(4byte)        |
 *	--------------------------
 *	|       R0(4byte)        |
 *	--------------------------
 *	|       SR(4byte)        | <- 割り込み発生時のSRレジスタ(H/Wにて保存)
 *	--------------------------
 *	|       PC(4byte)        | <- 割り込みから復帰するPC(H/Wにて保存)
 *	--------------------------
 * 
 *  割り込み共通部で残りのレジスタを退避した後のスタックを以下に示す．
 *	-------------------------
 *	|      GBR(4byte)        |
 *	-------------------------
 *	|     MACH(4byte)        |
 *	-------------------------
 *	|     MACL(4byte)        |
 *	-------------------------
 *	|       PR(4byte)        |
 *	-------------------------
 *	|       R7(4byte)        |
 *	-------------------------
 *	|       R6(4byte)        |
 *	-------------------------
 *	|       R5(4byte)        |
 *	-------------------------
 *	|       R4(4byte)        |
 *	-------------------------
 *	|       R3(4byte)        |
 *	--------------------------
 *	|       R2(4byte)        |
 *	--------------------------
 *	|       R1(4byte)        |
 *	--------------------------
 *	|       R0(4byte)        |
 *	--------------------------
 *	|       SR(4byte)        | <- 割り込み発生時のSRレジスタ(H/Wにて保存)
 *	--------------------------
 *	|       PC(4byte)        | <- 割り込みから復帰するPC(H/Wにて保存)
 *	--------------------------
 * 
 *	遅延ディスパッチ要求時にret_intにてスタックに積まれるレジスタを
 *	以下に示す．
 *
 *	-------------------------
 *	|      R14(4byte)        |
 *	-------------------------
 *	|      R13(4byte)        |
 *	-------------------------
 *	|      R12(4byte)        |
 *	--------------------------
 *	|      R11(4byte)        |
 *	--------------------------
 *	|      R10(4byte)        |
 *	--------------------------
 *	|       R9(4byte)        |
 *	-------------------------
 *	|       R8(4byte)        |
 *	-------------------------
 *	|     MACH(4byte)        |
 *	-------------------------
 *	|     MACL(4byte)        |
 *	-------------------------
 *	|       PR(4byte)        |
 *	-------------------------
 *	|       R7(4byte)        |
 *	-------------------------
 *	|       R6(4byte)        |
 *	-------------------------
 *	|       R5(4byte)        |
 *	-------------------------
 *	|       R4(4byte)        |
 *	-------------------------
 *	|       R3(4byte)        |
 *	--------------------------
 *	|       R2(4byte)        |
 *	--------------------------
 *	|       R1(4byte)        |
 *	--------------------------
 *	|       R0(4byte)        |
 *	--------------------------
 *	|       SR(4byte)        | <- 割り込み発生時のSRレジスタ(H/Wにて保存)
 *	--------------------------
 *	|       PC(4byte)        | <- 割り込みから復帰するPC(H/Wにて保存)
 *	--------------------------
 *
 *	ハンドラからリターンした後は, 多重割込みでなく, 
 *	ディスパッチ要求がある時(schedtsk != runtsk)に，
 *	ret_int へ分岐する．
 *
 *	多重割込みかどうかはcallevelの値で判定する.
 *	callevel != 1 ならば多重割込みであると判定する.
 *
 */
 
.align	2
_interrupt:
	/* 残りのスタックを退避 */
	mov.l	r4 , @-r15
	mov.l	r5 , @-r15
	mov.l	r6 , @-r15
	mov.l	r7 , @-r15
	sts.l	pr , @-r15
	sts.l	macl , @-r15
	sts.l	mach , @-r15
	stc.l	gbr , @-r15
	mov		#TCL_TASK , r4		/* 多重割り込みかをチェック */
	mov.l	callevel_adr , r3
	mov.b	@r3 , r5
	cmp/eq	r4 , r5
	bf int_from_int
int_from_task:
	mov.l	int_stack , r4			/* スタックを切り替え */
	mov.l	r15 , @-r4
	mov		r4 , r15
	mov		#TCL_ISR2 , r6		/* 実行レベルを変更 */
	mov.b	r6 , @r3
int_from_int:
	mov.w	r1 , @-r15		/* 前の割り込みIDをスタックに保存 */
	mov.w	r5 , @-r15		/* 前の実行レベルをスタックに保存 */
							/* ここでワードで保存しているのは、 */
							/* SPを4バイト境界に保つためである */
	ldc		r2 , sr			/* 割り込み許可 */
	jsr		@r0				/* 割り込みルーチン呼び出し */
	nop
	mov.l	mask_ipm , r0	/* 割り込み禁止 */
	ldc		r0 , sr
	mov.w	@r15+ , r0		/* 前の実行レベルを復帰 */
	mov.l	callevel_adr , r1
	mov.b	r0 , @r1
	mov.w	@r15+ , r2		/* 前の割り込みIDを復帰 */
	mov.l	runisr_adr , r1
	mov.b	r2 , @r1
	mov		#TCL_TASK , r1	/* 多重割り込みかどうか？ */
	cmp/eq	r0 , r1
	bf		int_return
	
	/* 初段の割り込みから復帰する処理 */
	mov.l	@r15 , r15			/* タスクスタックに戻す */
	mov.l	callevel_adr , r2
	mov.b	r1 , @r2
	
	/* 割り込み入り口で割り込みが入ってないかを確認 */
	mov		r15 , r0
	mov.l	@(52, r0) , r0	/* 割り込み前のSRレジスタを取り出し */
	mov.l	mask_ipm , r1		/* IPMを取り出し */
	and		r1 , r0
	tst		r0 , r0				/* 0かどうかチェック */
	bf		int_return			/* 0でない場合は多重割り込み */
	
	mov.l	runtsk_adr , r1	/* ディスパッチ要求があるか？ */
	mov.b	@r1 , r0
	extu.b	r0 , r0
	mov.l	schedtsk_adr , r1
	mov.b	@r1 , r2
	extu.b	r2 , r2
	cmp/eq	r0 , r2
	bf		ret_int
int_return:
	ldc.l	@r15+ , gbr		/* レジスタを復帰 */
	lds.l	@r15+ , mach
	lds.l	@r15+ , macl
	lds.l	@r15+ , pr
	mov.l	@r15+ , r7
	mov.l	@r15+ , r6
	mov.l	@r15+ , r5
	mov.l	@r15+ , r4
	mov.l	@r15+ , r3
	mov.l	@r15+ , r2
	mov.l	@r15+ , r1
	mov.l	@r15+ , r0
	rte						/* 割り込み元に復帰 */
	nop


/* 未使用割込みの処理 */
.globl _unused_interrupt
_unused_interrupt:
	rte
	nop

.align	4
runtsk_adr:
	.long	_runtsk
schedtsk_adr:
	.long	_schedtsk
tcxb_sp_adr:
	.long	_tcxb_sp
tcxb_pc_adr:
	.long	_tcxb_pc
int_stack:
	.long	STACK_TOP
callevel_adr:
	.long	_callevel
runisr_adr:
	.long	_runisr
mask_ipm:
	.long	0x000000f0
invalid_tsk:
	.long	INVALID_TASK

/* 定数の定義 */
.equ TCL_TASK , 1
.equ TCL_ISR2 , 2
.equ INVALID_TASK , 0xff
	
	

	.end