altivec.c

ge公司的dv4av4信号处理板的bsp源代码

    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
/* Macro Definition for sysAltivecStore Dino M. Diab compiler */


#ifdef __DCC__

asm void storeAltivec (rsave)
{
% reg rsave



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


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

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

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

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


/* Save registers at 'rsave' */


		or	3,rsave,rsave
		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


}

#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
    )
    {
#ifdef __DCC__
	
	storeAltivec (rsave);
#else

	__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,%0,%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");
#endif
    }


/* Macro Definition for sysAltivecLoad Dino M. Diab Compiler */

#ifdef __DCC__

asm loadAltivec (rsave)
{
% reg rsave;


		or	3,rsave,rsave
		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

}

#endif

/*******************************************************************************
*
* 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
    )
    {
#ifdef __DCC__

	loadAltivec (rsave);
#else

	__asm__ ("

		or	3,%0,%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");
#endif
    }

/* Macro Definition for  setAltivecNonJavaMode Dino M. Diab Compiler */

#ifdef __DCC__

asm void nonJavaModeSet ()
{
% lab vnojava, skipnj; 
! "r3"
		 				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:

}

#endif


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

#ifdef INCLUDE_ALTIVEC
LOCAL void setAltivecNonJavaMode
    (
    void
    )

    {
#ifdef __DCC__
	
	nonJavaModeSet ();

#else

	__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

    }
#endif