altivec_dy4.c

Curtiss-Wright Controls Embedded Computing公司的cw183板bsp源代码

*/

LOCAL void taskCreateAV 
    (
    WIND_TCB *t		/* pointer to TCB of task being created */
    )
    {
#ifndef INCLUDE_ALTIVEC
    int i;
    unsigned long *rsave; 

    t->ALTIVEC_RSAVE = 0;

    if (t->options & VX_ALTIVEC)
        {
        if ((rsave = memalign (32, 4*ALTIVEC_RSAVE_SIZE)) != 0)
            {
            (void *) (t->ALTIVEC_RSAVE) = rsave;

            for (i = 0; i < ALTIVEC_RSAVE_SIZE; i++)
                rsave[i] = 0;

            VSCR_PTR(rsave)[3] |= NJ;

            t->regs.msr |= MSR_ALTIVEC_ENABLE;
            }
        }
#else
    if (t->options & VX_ALTIVEC)
        {
        t->options &= ~VX_ALTIVEC;
        t->options |= VX_ALTIVEC_TASK;
        }
#endif
    }

/***********************************************************
*
* taskDeleteAV - Altivec task delete hook
*
* This local function is installed as the task delete hook.
* If the task being deleted has an altivec register save area
* allocated, then this function frees that area.
*
* If the task being deleted is the last task to use Altivec
* registers, then clear currentAVTask.  This prevents the
* context switch hook from saving registers for this task
* when switching to a new task.
*
* SEE ALSO: taskSwitchAV(), taskCreateAV()
*/

#ifndef INCLUDE_ALTIVEC
LOCAL void taskDeleteAV 
    (
    WIND_TCB *t		/* pointer to TCB of task being deleted */
    )
    {
    /*
     *  If the task being deleted owns the Altivec
     *  registers (indicated by t == current task),
     *  then set the current task pointer to zero.
     *  This signifies that no task owns the Altivec 
     *  registers at this time, and that they need to 
     *  be reloaded when a context switch to an
     *  Altivec task occurs.
     */

    if (t == currentAVTask)
        currentAVTask = 0;

    /*
     *  If the task being deleted had an Altivec 
     *  register area, then free it up now.
     */

    if ((t->regs.msr & MSR_ALTIVEC_ENABLE) && t->ALTIVEC_RSAVE)
        {
        free ((void *) (t->ALTIVEC_RSAVE));

        t->ALTIVEC_RSAVE = 0;
        }
    }
#endif

/*******************************************************************************
*
* taskSwitchAV - Altivec task switch hook
*
* This local function is installed as the task switch hook.
* If we are switching into an Altivec enabled task,
* then we need to load registers from the TCB's
* Altivec register save area.
*
* Furthermore, if the Altivec registers still contain
* live values from another task, then we need to 
* save those registers within the task's register
* save area.
*
* If the Altivec registers contain live data from 
* the task to which we are switching, then no action
* is needed.
*
* SEE ALSO: taskCreateAV(), taskDeleteAV()
*/

#ifndef INCLUDE_ALTIVEC
LOCAL void taskSwitchAV 
    (
    WIND_TCB *old, 	/* ptr to tcb being switched out */
    WIND_TCB *new	/* ptr to tcb being switched in */
    )

    {
    /*
     *  If the new TCB does not require Altivec  
     *  support, then exit.
     */

    if ((new->regs.msr & MSR_ALTIVEC_ENABLE) == 0)
        return;

    /*
     *  If the new TCB is the last TCB that owned 
     *  the Altivec registers, then do nothing.
     */

    if (new == currentAVTask)
        return;

    /*
     *  If the old TCB is nonzero, then we need to save 
     *  the AV context for the old task.
     */

    if (currentAVTask != 0 && currentAVTask->ALTIVEC_RSAVE != 0)
        sysAltivecStore ((void *) (currentAVTask->ALTIVEC_RSAVE));

    if (new->ALTIVEC_RSAVE != 0)
        sysAltivecLoad ((void *) (new->ALTIVEC_RSAVE));

    currentAVTask = new;
    }
#endif

/*******************************************************************************
*
* sysAltivecStore - Store Altivec registers into memory
*
* This function stores altivec registers into memory.
* 32 vector registers, the vector control/status register,
* and spr 256 (vrsave) are stored into memory.  The register
* values are unchanged by this function.
*
* SEE ALSO: sysAltivecLoad ()
* NOMANUAL
*/

void sysAltivecStore 
    (
    void *rsave
    )
    {

	__asm__ ("

#
#  Macro definitions.  Because these are in an embedded
#  string, we have to use two consecutive backslashes.
#  These only need to be defined once.
#

            .macro      lvx vd,ra,rb
	    .long	(31<<26)+(\\vd<<21)+(\\ra<<16)+(\\rb<<11)+103*2
            .endm

            .macro      stvx vs,ra,rb
	    .long	(31<<26)+(\\vs<<21)+(\\ra<<16)+(\\rb<<11)+231*2
            .endm

            .macro      mfvscr vd
	    .long	(4<<26)+(\\vd<<21)+770*2
            .endm

            .macro      mtvscr vb
	    .long	(4<<26)+(\\vb<<11)+802*2
            .endm


#
#  Save registers at 'rsave'
#

		or	3,3,%0
		mfmsr	8
		li	4,16		# index value 1
		oris	8,8,0x0200      # enable altivec 
		li	5,32		# index value 2 
		mtmsr	8
		isync

		li	6,48		# 4x vector reg size

		stvx	0,0,3
		addi	7,3,64
		stvx	1,4,3
		stvx	2,5,3
		stvx	3,6,3

		stvx	4,0,7
		addi	3,7,64
		stvx	5,4,7
		stvx	6,5,7
		stvx	7,6,7

		stvx	8,0,3
		addi	7,3,64
		stvx	9,4,3
		stvx	10,5,3
		stvx	11,6,3

		stvx	12,0,7
		addi	3,7,64
		stvx	13,4,7
		stvx	14,5,7
		stvx	15,6,7

		stvx	16,0,3
		addi	7,3,64
		stvx	17,4,3
		stvx	18,5,3
		stvx	19,6,3

		stvx	20,0,7
		addi	3,7,64
		stvx	21,4,7
		stvx	22,5,7
		stvx	23,6,7

		stvx	24,0,3
		addi	7,3,64
		stvx	25,4,3
		stvx	26,5,3
		stvx	27,6,3

		stvx	28,0,7
		addi	3,7,64
		stvx	29,4,7
		stvx	30,5,7
		stvx	31,6,7

#
#  Obtain the vscr register and save it also.  Then restore the
#  register we clobbered (v28) from memory.
#
		mfvscr	28
		stvx	28,0,3
                lvx     28,0,7		# restore v28	

#
#  Save spr256 as well.
#
		mfspr	0,256
		stw	0,16(3)

		mtmsr	8
	" : : "r" (rsave) : "0", "3", "4", "5", "6", "7", "8");
    }



/*******************************************************************************
*
* sysAltivecLoad - Load Altivec registers from memory
*
* This function loads altivec registers from memory.
* 32 vector registers, the vector control/status register,
* and spr 256 (vrsave) are loaded from memory.  The register
* values are unchanged by this function.
*
* SEE ALSO: sysAltivecStore ()
* NOMANUAL
*/

void sysAltivecLoad
    (
    void *rsave
    )
    {

	__asm__ ("

		or	3,3,%0
		mfmsr	8
		li	4,16		# index value 1 
		oris	8,8,0x0200      # enable altivec 
		li	5,32		# index value 2 
		mtmsr	8
		isync

		li	0,512
		li	6,48

		lvx	0,3,0		# get vxxx reg	
		mtvscr	0		# restore it

		lvx	0,0,3
		addi	7,3,64
		lvx	1,4,3
		lvx	2,5,3
		lvx	3,6,3

		lvx	4,0,7
		addi	3,7,64
		lvx	5,4,7
		lvx	6,5,7
		lvx	7,6,7

		lvx	8,0,3
		addi	7,3,64
		lvx	9,4,3
		lvx	10,5,3
		lvx	11,6,3

		lvx	12,0,7
		addi	3,7,64
		lvx	13,4,7
		lvx	14,5,7
		lvx	15,6,7

		lvx	16,0,3
		addi	7,3,64
		lvx	17,4,3
		lvx	18,5,3
		lvx	19,6,3

		lvx	20,0,7
		addi	3,7,64
		lvx	21,4,7
		lvx	22,5,7
		lvx	23,6,7

		lvx	24,0,3
		addi	7,3,64
		lvx	25,4,3
		lvx	26,5,3
		lvx	27,6,3

		lvx	28,0,7
		addi	3,7,64
		lvx	29,4,7
		lvx	30,5,7
		lvx	31,6,7

		lwz	0,16(3)		# restore vrsave spr
		mtspr	256,0

		mtmsr	8		# restore msr

	" : : "r" (rsave) : "0", "3", "4", "5", "6", "7", "8");
    }



/*******************************************************************************
*
* setAltivecNonJavaMode
*
* Place Altivec in non-Java mode
*
* NOMANUAL
*/

#ifdef INCLUDE_ALTIVEC
LOCAL void setAltivecNonJavaMode
    (
    void
    )

    {

	__asm__ ("      li      3,vnojava@h
                    ori     3,3,vnojava@l
                    lvx     0,0,3
                    mtvscr  0
                    b       skipnj
                    .align	4,0
        vnojava:    .long	0,0,0,0x00010000
        skipnj: " : : : "3");

    }
#endif