windalib.s

vxworks source code, used for develop vxworks system.

* non-volatile registers and errno in TCB, then call-out the rescheduler.
*
* INTERNAL
*
*	r1 = 0				_________________
*	r2 ->	WIND_TCB_SR	0x17c	|     sr (=0)	|	 0
*	 v	WIND_TCB_PC	0x178	|     pc (=pr)	|	-4
*	 v	WIND_TCB_R15	0x174	|     sp	|	-8
*	 v	WIND_TCB_R14	0x170	|     r14	|	-12
*	 v	WIND_TCB_R13	0x16c	|     r13	|	-16
*	 v	WIND_TCB_R12	0x168	|     r12	|	-20
*	 v	WIND_TCB_R11	0x164	|     r11	|	-24
*	 v	WIND_TCB_R10	0x160	|     r10	|	-28
*	 v	WIND_TCB_R9	0x15c	|     r9	|	-32
*	r2 ->	WIND_TCB_R8	0x158	|     r8	|  0	-36
*		WIND_TCB_MACL	0x154	|		| -4	-40
*		WIND_TCB_MACH	0x150	|		| -8	-44
*		WIND_TCB_R7	0x14c	|		| -12	-48
*		WIND_TCB_R6	0x148	|		| -16	-52
*		WIND_TCB_R5	0x144	|		| -20	-56
*		WIND_TCB_R4	0x140	|		| -24	-60
*		WIND_TCB_R3	0x13c	|		| -28	-64
*		WIND_TCB_R2	0x138	|		| -32	-68
*		WIND_TCB_R1	0x134	|		| -36	-72
*		WIND_TCB_R0	0x130	|     r0 (=0)	|	-76
*		WIND_TCB_PR	0x12c	|		|	-80
*		WIND_TCB_GBR	0x128	|		|
*		WIND_TCB_VBR	0x124	|		|	(regsSh.h)
*					|		|
*		WIND_TCB_REGS	0x124	|_______________|	(taskLibP.h)
*				0x120	|_____ sr ______|
*				0x11c	|_____ pc ______|
*		EXC_INFO	0x118	|_valid_|_vecNum|	(excShLib.h)
*					|		|
*		WIND_TCB_ERRNO		|    _errno	|
*					|		|
*	r7 ->	WIND_TCB	0x0	|_______________|	(taskLib.h)
*/
	.type	saveTaskContext,@function
						/* r4: ReadyQHead     */
						/* r5: TaskIdCurrent  */
						/* r6: _readyQHead    */
						/* r7: _taskIdCurrent */
saveTaskContext:
	mov.l	WX_WINDTCB_SR,r2;		/* get TCB_SR offset in r2    */
	mov.l	WX_Errno,r3;			/* fetch errno address        */
	add	r7,r2				/* add taskIdCurrent          */
	mov.l	@r3,r1;				/* get errno                  */
	mov	#WIND_TCB_ERRNO,r0		/* get TCB offset             */

#if	(CPU==SH7600 || CPU==SH7000)
	extu.b	r0,r0
	mov.l	r1,@(r0,r7)			/* save errno in tcb          */
	xor	r6,r6				/* use this zero twice        */
	mov.l	r6,@r2				/* save a brand new SR (zero) */
#else
	mov.l	WX_intUnlockSR,r6;		/* IL_ unavailable if portable*/
	extu.b	r0,r0
	mov.l	@r6,r6;
	mov.l	r1,@(r0,r7)
	mov.l	r6,@r2				/* save a brand new SR        */
	xor	r6,r6
#endif
	sts.l	pr,  @-r2			/* save return address as PC  */
	mov.l	sp,  @-r2			/* save r15                   */
	mov.l	r14, @-r2			/* save r14                   */
	mov.l	r13, @-r2			/* save r13                   */
	mov.l	r12, @-r2			/* save r12                   */
	mov.l	r11, @-r2			/* save r11                   */
	mov.l	r10, @-r2			/* save r10                   */
	mov.l	r9,  @-r2			/* save r9                    */
	mov.l	r8,  @-r2			/* save r8                    */
	add	#-36,r2				/* mac[lh],r1-r7 are volatile */
	mov.l	r6,  @-r2			/* clear saved r0 for return  */
/*	sts.l	pr,  @-r2			/@ Not necessary              */

						/* r3: Errno          */
						/* r4: ReadyQHead     */
						/* r5: TaskIdCurrent  */
						/* r6: 0              */
						/* r7: _taskIdCurrent */
#ifdef PORTABLE
	mov.l	WX_Reschedule,r0;		/* void reschedule (void)     */
	jsr	@r0;				/* goto rescheduler           */
	nop					/* should never return!       */

		.align	2
WX_Reschedule:	.long	_reschedule
#else	/* !PORTABLE */

	/* FALL THRU TO RESCHEDULE */

/******************************************************************************
*
* reschedule - rescheduler for VxWorks kernel
*
* This routine is called when either intExit, or windExit, thinks the
* context might change.  All of the contexts of all of the tasks are
* accurately stored in the task control blocks when entering this function.
* The STORED status register is 0x40000000 for SH7750/SH7700,
* 0x00000000 for SH7600/SH7000. (Interrupts UNLOCKED)
*
* At the conclusion of this routine, taskIdCurrent will equal the highest
* priority task eligible to run, and the kernel work queue will be empty.
* If a context switch to a different task is to occur, then the installed
* switch hooks are called.
*
* NOMANUAL

* void reschedule ()

* INTERNAL
*
*     _________        ___________________                  _reschedule
*    /         \      /                   \                      |
*   |           v    v                     |                     |
*   |           doWork <==> workQDoWork()  |      taskIdPrevious = taskIdCurrent
*   |              |                       |                     |
*   |              v                       |                     v
*   |  +----------------------+            ^           +----------------------+
*   |  | readyQHead != NULL ? |-(No)---> idle <---(No)-| readyQHead != NULL ? |
*   |  +----------------------+                        +----------------------+
*   |              |(Yes)                                        |(Yes)
*   |              |                                             |
*   |              v                                             |
*   |   +--------------------------------+                       |
*   |   | readyQHead == taskIdPrevious ? |-(No)------------+     |
*   |   +--------------------------------+                 |     |
*   |              |(Yes)                          taskIdCurrent = readyQHead
*   |              |                                       |     |
*   |              |                                       v     v
*   |              |                                     switchTasks
*   |               \                                        /
*   |                \___________           ________________/
*   |                            \         /     /     /
*   |                             \       /     /     /
*   |                              |     |     |   doSwapHooks <==> C(r4,r5)
*   |     r8:  ReadyQHead          |     |     |     |      \____________
*   |     r9:  TaskIdCurrent       |     |     v     v                   \
*   |     r10:                     |     |   doSwitchHooks <==> C(r4,r5)  |
*   |     r11: WorkQIsEmpty        |     |    /  ________________________/
*   |     r12:                      \    |   /  /
*   |     r13: taskIdPrevious        \   |  /  /
*   |     r14: taskIdCurrent          \  | |  /
*   |                                  | | | |
*   |                                  v v v v
*   |                                 dispatch
*   |                                     |
*   |                      taskIdPrevious = taskIdCurrent
*    \                                    |
*     \                                  / \
*      `------ doWorkUnlock <-----------'   `-----------------> rte
*/
	/*   |                          */
	/* (saveTaskContext)            */	/* r3: Errno          */
	/*   |                          */	/* r4: ReadyQHead     */
	/*   |   +---- saveIntContext   */	/* r5: TaskIdCurrent  */
	/*   |   |                      */	/* r6: (invalid)      */
	/*   V   V                      */	/* r7: _taskIdCurrent */

	.type	_reschedule,@function

_reschedule:
	mov	r4,r8
	mov	r5,r9
	mov.l	@r8,r14;
	mov	r7,r13			/* taskIdPrevious = taskIdCurrent */
	mov.l	WX_WorkQIsEmpty,r11;
	tst     r14,r14
	bt	idle;			/* idle if nobody ready */

	mov.l	r14,@r9			/* taskIdCurrent = readyQHead */

	/*   |                          */	/* r14: _readyQHead     */
	/*   |                          */	/* r13: _taskIdPrevious */
	/*   |                          */	/* r12:                 */
	/*   |                          */	/* r11: WorkQIsEmpty    */
	/*   |                          */	/* r10:                 */
	/*   |   +---- doWork           */	/* r9:  TaskIdCurrent   */
	/*   |   |                      */	/* r8:  ReadyQHead      */
	/*   V   V                      */	/* r0-r7: (don't care)  */
	.type	switchTasks,@function

switchTasks:
	mov	#WIND_TCB_SWAP_IN,r0
	mov.w	@(r0,r14),r12;
	mov	#WIND_TCB_SWAP_OUT,r0
	mov.w	@(r0,r13),r0;
	or	r0,r12
	extu.w	r12,r12				/* r12: swap hook mask */
	tst     r12,r12
	bf	doSwapHooks			/* any swap hooks to do */

	mov.l   WX_TaskSwitchTable,r10;
	mov.l	@r10,r6;
	tst	r6,r6				/* r6: _taskSwitchTable */
	bf	doSwitchHooks

	/*   |                          */	/* r14: _taskIdCurrent  */
	/*   |   +-- doSwapHooks        */	/* r13: _taskIdPrevious */
	/*   |   |                      */	/* r12:                 */
	/*   |   |   +-- doSwitchHooks  */	/* r11: WorkQIsEmpty    */
	/*   |   |   |                  */	/* r10:                 */
	/*   |   |   |   +-- doWork     */	/* r9:  TaskIdCurrent   */
	/*   |   |   |   |              */	/* r8:  ReadyQHead      */
	/*   V   V   V   V              */	/* r0-r7: (don't care)  */
	.type	dispatch,@function

#if (CPU==SH7750 || CPU==SH7700)

dispatch:
	/* taskIdPrevious = taskIdCurrent */
	mov	r14,r13
					mov	#WIND_TCB_ERRNO,r0
					extu.b	r0,r0
					mov.l	@(r0,r14),r2
	mov.l	DI_WINDTCB_PR,r1;	mov.l	WX_Errno,r3
	add	r1,r14;			mov.l	r2,@r3	/* restore errno */

	/* load register set */
	lds.l	@r14+,pr
	mov.l	@r14+,r0
	mov.l	@r14+,r1
	mov.l	@r14+,r2
	mov.l	@r14+,r3
	mov.l	@r14+,r4
	mov.l	@r14+,r5
	mov.l	@r14+,r6
	mov.l	@r14+,r7
	lds.l	@r14+,mach		/* LOCK INTERRUPTS */
	lds.l	@r14+,macl;		mov.l	DI_IntLockSR,r12
	mov.l	@r14+,r8;		mov.l	@r12,r12
	mov.l	@r14+,r9;		ldc	r12,sr
					/* is work q empty? */
					mov.l	@r11,r12
	mov.l	@r14+,r10;		tst	r12,r12
					bt	doWorkUnlock /* r8,r9: broken */
#ifdef	WV_INSTRUMENTATION
	mov.l	DI_EvtAction,r11
	mov.l	@r11,r12
	tst	r12,r12
	bt	dispatchNoInstr
	bra	dispatchInstr;		/* ==> dispatchInstr */
	nop
		.align	2
DI_EvtAction:	.long	_evtAction
dispatchNoInstr:			/* <== dispatchInstr */
#endif	/*WV_INSTRUMENTATION*/
					/* release kernel mutex */
	stc	vbr,r11;		mov.l	DI_KernelState,r13
	add	#DISPATCH_STUB,r11;	mov	#0,r12
	jmp	@r11;			mov.l	r12,@r13

			.align	2
DI_WINDTCB_PR:		.long	WIND_TCB_PR

#elif (CPU==SH7600 || CPU==SH7000)

dispatch:
	/* taskIdPrevious = taskIdCurrent */
	mov	r14,r13
					mov	#WIND_TCB_ERRNO,r0
					extu.b	r0,r0
					mov.l	@(r0,r14),r2
	mov.l	DI_WINDTCB_R15,r1;	mov.l	WX_Errno,r3
	add	r1,r14;			mov.l	r2,@r3	/* restore errno */

	mov.l	@r14+,sp		/* set new task's sp */
	mov.l	@r14+,r0		/* get new task's pc */
	mov.l	@r14, r1		/* get new task's sr */
	add	#-80,r14
	mov.l	r1, @-sp		/* push sr as dummy exception frame */
	mov.l	r0, @-sp		/* push pc as dummy exception frame */

	/* load register set */
	lds.l	@r14+,pr
	mov.l	@r14+,r0
	mov.l	@r14+,r1
	mov.l	@r14+,r2
	mov.l	@r14+,r3
	mov.l	@r14+,r4
	mov.l	@r14+,r5
	mov.l	@r14+,r6
	mov.l	@r14+,r7
	lds.l	@r14+,mach		/* LOCK INTERRUPTS */
	lds.l	@r14+,macl;		mov.l	DI_IntLockSR,r12
	mov.l	@r14+,r8;		mov.l   @r12,r12
	mov.l	@r14+,r9;		ldc     r12,sr

					/* is work q empty? */
					mov.l	@r11,r12
	mov.l	@r14+,r10;		tst	r12,r12
					bt	doWorkUnlock /* r8,r9: broken */
#ifdef	WV_INSTRUMENTATION
	mov.l	DI_EvtAction,r11
	mov.l	@r11,r12
	tst	r12,r12
	bt	dispatchNoInstr
	bra	dispatchInstr;		/* ==> dispatchInstr */
	nop
		.align	2
DI_EvtAction:	.long	_evtAction
dispatchNoInstr:			/* <== dispatchInstr */
#endif	/*WV_INSTRUMENTATION*/
					/* release kernel mutex */
					mov.l	DI_KernelState,r13
					mov	#0,r12
	mov.l	@r14+,r11;		mov.l	r12,@r13
	mov.l	@r14+,r12
	mov.l	@r14+,r13;		rte
	mov.l	@r14,r14		/* INTERRUPTS UNLOCKED */

			.align	2
DI_WINDTCB_R15:		.long	WIND_TCB_R15

#endif /* CPU==SH7600 || CPU==SH7000 */

			.align	2
DI_IntLockSR:		.long	_intLockTaskSR
DI_KernelState:		.long	_kernelState
WX_TaskSwitchTable:	.long	_taskSwitchTable
WX_WorkQIsEmpty:	.long	_workQIsEmpty

/******************************************************************************
*
* idle - spin here until there is more work to do
*
* When the kernel is idle, we spin here continually checking for work to do.
*/
	.align	2
	.type	idle,@function
					/* r14: 0              */
	/************************/	/* r13:                */
	/*                      */	/* r12:                */
	/*   +---- _reschedule  */	/* r11: WorkQIsEmpty   */
	/*   |                  */	/* r10:                */
	/*   |   +---- doWork   */	/* r9:                 */
	/*   |   |              */	/* r8:                 */
	/*   V   V              */	/* r0-r7: (don't care) */
idle:

#ifdef	WV_INSTRUMENTATION
	mov.l	IL_EvtAction,r1;
	mov.l	@r1,r0;
	tst	r0,r0
	bt	idleNoInstr
	bra	idleInstr;		/* ==> idleInstr */
	nop
		.align	2
IL_EvtAction:	.long	_evtAction
idleNoInstr:				/* <== idleInstr */
#endif

#if (CPU==SH7600 || CPU==SH7000)
	ldc	r14,sr			/* UNLOCK INTERRUPTS (just in case) */
	mov.l   IL_KernelIsIdle,r1;
	mov	#1,r0
#else
					mov.l	IL_intUnlockSR,r3
	mov.l   IL_KernelIsIdle,r1;	mov.l	@r3,r2
	mov	#1,r0;			ldc	r2,sr
#endif
	mov.l	r0,@r1			/* _kernelIsIdle = 1 */

idleLoop:
	mov.l	@r11,r0;
	tst	r0,r0
	bt	bra_doWork		/* branch if _workQIsEmpty == 0 */
	mov.l	IL_VxIdleLoopHook,r0
	mov.l	@r0,r0
	tst	r0,r0
	bt	idleLoop		/* loop _func_vxIdleLoopHook == NULL */
	jsr	@r0
	nop
	bra	idleLoop
	nop

bra_doWork:
	bra	doWork;
	mov.l	r0,@r1			/* _kernelIsIdle = 0 */

/******************************************************************************
*
* doSwapHooks - execute the tasks' swap hooks
*
*/
	.align	2
	.type	doSwapHooks,@function
					/* r14: _taskIdCurrent  */
					/* r13: _taskIdPrevious */
					/* r12: swap hook mask  */
	/************************/	/* r11:                 */
	/*                      */	/* r10:                 */
	/*   +---- switchTasks  */	/* r9:  TaskIdCurrent   */
	/*   |                  */	/* r8:                  */
	/*   V                  */	/* r0-r7: (don't care)  */

doSwapHooks:
	mov.l   DSW_TaskSwapTable,r10;	/* r10: TaskSwapTable   */
	add     #-4,r10			/* start index at -1, heh heh       */
	bra	doSwapShift;		/* jump into the loop               */
	shll16	r12			/* align 16-bit mask to 32-bit MSB  */

doSwapHook:
	mov.l   @r10,r6;
	mov     r13,r4
	jsr	@r6;			/* f(_taskIdPrevious, _taskIdCurrent) */
	mov	r14,r5			/*        r4               r5         */

doSwapShift:
	add     #4,r10			/* bump swap table index           */
	shll    r12			/* shift swapMask bit pattern left */
	bt	doSwapHook		/* if T