syslib.c

56f8300E系列dsp的BOOTloader

			asm (tst.w   X0);
			asm (beq     SkipShift);
			asm (adda    R4,R3);
			asm (lsrr.w  #8,Y0);
SkipShift:
			asm (suba    R4,R3);
			asm (eorc    #1,X0);
			asm (move.bp Y0,X:(R2)+);
			asm (nop);
EndDo:
			asm (move.l  x:(SP)-,R4);
			asm (move.w  x:(SP),X0);
			asm (suba    #2,SP);
			
//			; R2 - Contains *dest return value

 }
/********************************************************************        
; void  * memMemset (void *dest, int c, size_t count);
; #pragma interrupt  /* Can be called from a pragma interrupt ISR */
/*;
;    Register usage:
;       R2 - dest
;       Y0 - c
;       Y1 - count
;
;********************************************************************/
void  * memMemset (void *dest, int c, ssize_t count)
{

#if __m56800E_lmm__ 
		    asm(move.w A0,A);
	    	asm(move.w  A1,Y1);
#else
			asm(nop);
			asm(nop);
#endif
			asm (adda    #2,SP);       
			asm (move.w  X0,x:(SP));//   ; Save X0 to restore later
			
			asm (tst.w   Y1); //        ; Q: count > 0 ?
			asm (beq     EndMemset);
			asm (nop);
			asm (nop);
			
			asm (brclr   #$0001,R2,AddrEven);
			asm (nop);
			asm (nop);
			asm (move.bp Y0,x:(R2)+);//  ; move one byte
			asm (dec.w   Y1);
AddrEven:
			asm (asra    R2);//          ; create word address
			asm (move.w  Y0,X0); //      ; create 16-bit data value
			asm (asll.w  #8,X0);
			asm (andc    #$00FF,Y0);
			asm (or.w    X0,Y0);
			asm (move.w  Y1,X0);//       ; create word count
			asm (lsr.w   X0);
			asm (rep     X0);       //   ; move words
			asm (move.w  Y0,X:(R2)+);
			
			asm (asla    R2,R2);//       ; restore byte address
			
			asm (brclr   #$0001,Y1,EndMemset);//  ; Q: Odd byte?
			asm (move.bp Y0,x:(R2)+ ); // ; 
EndMemset:
			//; R2 - Contains *dest return value
			asm (move.w  x:(SP),X0);//  ; Restore X0 register
			asm (suba    #2,SP);

}
/********************************************************************        
;
; void * memCopyXtoP   ( void *dest, const void *src, size_t count );
; #pragma interrupt  /* Can be used in a pragma interrupt ISR */
/*;
;    Register usage:
;       R2 - dest
;       R3 - src
;       Y0 - count/temp
;
;********************************************************************/
 void * memCopyXtoP   ( void *dest, const void *src, ssize_t count )
 {
 			asm (brset   #1,R2,OddBytes); //    ; Optimize move for words
			asm (nop);
			asm (nop);
			asm (brset   #1,R3,OddBytes);
			asm (nop);
			asm (nop);

#if __m56800E_lmm__ 
	    	asm(move.w A0,A);
	    	asm(move.w  A1,Y0);
#else
			asm(nop);
			asm(nop);
#endif
			asm (brset   #1,Y0,OddBytes);
			asm (nop);
			
/*			
;
; Move words between word addresses
;
*/
			asm (lsra    R2);//            ; convert to word address
			asm (lsr.w   Y0 );//           ; convert to word count
			asm (lsra    R3);//            ; convert to word address
			
			asm (do      Y0,EndDoEven);//  ; move all words
			
			asm (move.w  X:(R3)+,Y0);
			asm (move.w  Y0,P:(R2)+);
			asm (nop);
			asm (nop);
			asm (nop);
EndDoEven:
			asm (asla    R2);//            ; convert to byte address
/*			; R2 - Contains *dest return value*/
			asm (rts);     
			
/*			
;
; Move bytes or words between possible non-word addresses
; 
*/	
OddBytes:		
			asm (adda    #2,SP);//         ; save registers
			asm (move.l  R4,x:(SP)+);//    ; push R4
			asm (move.w  X0,x:(SP-1));//   ; push X0
			asm (move.w  Y1,x:(SP));//     ; push Y1
			
			asm (move.l  #1,R4);//         ; R4 = constant 1
			asm (move.w  R2,X0);
			asm (lsra    R2);
			asm (andc    #$0001,X0);

			asm (do      Y0,EndDo);
			
			asm (move.bp X:(R3)+,Y1);
			
			asm (move.w  P:(R2)+,Y0);
			asm (tst.w   X0);
			asm (beq     SkipShift);
			asm (asll.w  #8,Y1);
			asm (andc    #$FF,Y0);
			asm (suba    R4,R2);
			asm (or.w    Y1,Y0);
			asm (move.w  Y0,P:(R2)+);
			asm (bra     SwitchBytes);
SkipShift:
			asm (andc    #$FF00,Y0);
			asm (andc    #$FF,Y1);
			asm (suba    R4,R2);
			asm (or.w    Y1,Y0);
			asm (move.w  Y0,P:(R2)+);
/*			
			; These three NOPs are required on VIPER
			; but not necessary in Wedge
			nop
			nop
			nop
*/			
			asm (suba    R4,R2);
SwitchBytes:
			asm (eorc    #1,X0);
			
EndDo:
			asm (move.w  X0,R3);
			asm (adda.l  R2,R3);
			asm (tfra    R3,R2);
			
			asm (move.w  x:(SP-1),X0);//  ; restore saved registers
			asm (move.w  X:(SP),Y1);
			asm (suba    #2,SP);
			asm (move.l  x:(SP)-,R4);
/*			
			; R2 - Contains *dest return value
*/
			asm (rts);
 }
 
/********************************************************************        
;
; void  * memMemsetP (void *dest, int c, size_t count);
; #pragma interrupt  /* Can be used in a pragma interrupt ISR */
/*;
;    Register usage:
;       R2 - dest
;       Y0 - c
;       Y1 - count
;
; Note:  The current implementation only supports setting P memory
;        on word boundaries.
;
;********************************************************************/
void  * memMemsetP (void *dest, int c, ssize_t count)
{
#if __m56800E_lmm__ 
		    asm(move.w A0,A);
		    asm(move.w  A1,Y1);
#else
			asm(nop);
			asm(nop);
#endif
			asm (brset   #$0001,R2,ByteAlignment);
			asm (brclr   #$0001,Y1,ParamsOK);
ByteAlignment:
			asm (debughlt); //   ;  Current implementation does not support byte alignment
ParamsOK:
			asm (asra    R2);
			asm (lsr.w   Y1);
			asm (tst.w   Y1);
			asm (beq     EndMemsetP);
			
			asm (do      Y1,EndMemsetP);
			
			asm (move.w  Y0,P:(R2)+);
EndMemsetP:
			asm (asla    R2);
			/*; R2 - Contains *dest return value*/
			asm (rts);
}

/*
; Word16 memReadP16  (Word16 *pX);
; #pragma interrupt  /* Can be used in a pragma interrupt ISR */
/*
;
;    Register usage:
;       R2 - pX
;       Y0 - return value
;
*/
UWord16 memReadP16  (UWord16 *pX)
{
			asm (move.w P:(R2)+,Y0);
			asm (rts);
}
/*
; void memWriteP32  (Word32 x, Word32 *pX);
; #pragma interrupt  /* Can be used in a pragma interrupt ISR */
/*
; NB: R2 willl be change
;    Register usage:
;       R2   - pX
;       A10  - x
;       X0   - temp
;
*/
void   memWriteP32 (UWord32 Data, Word32 *pDest)
{
			asm (adda   #2,SP);
			asm (move.w X0,x:(SP));
			asm (move.w A0,X0);
			asm (move.w X0,P:(R2)+);
			asm (move.w A1,P:(R2)+);
			asm (move.w x:(SP),X0);
			asm (suba   #2,SP);
			asm (rts);    
}
/*
; void memWriteP16  (Word16 x, Word16 *pX);
; #pragma interrupt  /* Can be used in a pragma interrupt ISR */
/*
;
;    Register usage:
;       R2 - pX
;       Y0 - x
;
*/
void   memWriteP16 (UWord16 Data, Word16 *pDest)
{
			asm (move.w Y0,P:(R2)+);
			asm (rts);
}
/*;********************************************************************        
;
; void *memcpy( void *dest, const void *src, size_t count );
; #pragma interrupt /* Can be called from a pragma interrupt ISR */
/*
;
;    Register usage:
;       R2 - dest
;       R3 - src
;       Y0 - count/temp
;
;********************************************************************/
void * memMemcpy   (void *dest, const void *src, ssize_t count)
{
#if __m56800E_lmm__ 
		    asm(move.w A0,A);
		    asm(move.w  A1,Y0);
#else
			asm(nop);
			asm(nop);
#endif
			asm(tst.w   Y0);
			asm(beq     EndDo);
			asm(nop);
			asm(nop);
			asm(do      Y0,EndDo);
			
			asm(move.bp X:(R3)+,Y0);
			asm(move.bp Y0,X:(R2)+);
EndDo:
			//; R2 - Contains *dest return value
			asm(rts);
}
/*****************************************************************************
*
* Functions:      memCopyWordXtoP()
*
* Description:    Copy src words from X:src to P:dest memory location
*                 Register usage:
*                    R2 - dest
*                    R3 - src
*                    Y0 - count, tmp variable
*
* Returns:        None
*
* Arguments:      dest - data destination
*                 scr  - data source
*                 count - data length in words 
*
* Range Issues:   
*
* Special Issues: Inline assembler used, "do" hardware cycle used
*                 No error checking for incorrect counter value.
*
*****************************************************************************/
asm void memCopyWordXtoP ( UWord32 *dest, const UWord16 *src, UWord16 count )
{
			do     Y0,EndDo
			move.w X:(R3)+,Y0
			move.w Y0,P:(R2)+
			nop
			nop
			nop
EndDo:
			rts
}

/*****************************************************************************
*
* Functions:      memCopyWordPtoX()
*
* Description:    Copy src words from P:src to X:dest memory location
*                 Register usage:
*                    R2 - addr
*                    R3 - dest
*                    Y0 - count, tmp variable
*
* Returns:        None
*
* Arguments:      dest - data destination
*                 scr  - data source
*                 count - data length in words 
*
* Range Issues:   
*
* Special Issues: Inline assembler used, "do" hardware cycle used
*                 No error checking for incorrect counter value.
*
*****************************************************************************/
asm void memCopyWordPtoX ( UWord32 *addr, const UWord16 *dest, UWord16 count )
{
			do     Y0,EndDo
			move.w P:(R2)+,Y0
			move.w Y0,X:(R3)+
			nop
			nop
			nop
EndDo:
			rts
}

/*****************************************************************************
*
* Functions:      memCopyWordXtoX()
*
* Description:    Copy src words from X:src to X:dest memory location
*                 Register usage:
*                    R2 - dest
*                    R3 - src
*                    Y0 - count, tmp variable 
*
* Returns:        None
*
* Arguments:      dest - data destination
*                 scr  - data source
*                 count - data length in words 
*
* Range Issues:   
*
* Special Issues: inline assembler used, "do" hardware cycle used
*                 No error checking for incorrect counter value.
*
* Test Method:    bootest.mcp
*
*****************************************************************************/
asm void memCopyWordXtoX( UWord32 *dest, const UWord16 *src, UWord16 count )
{
			do     Y0,EndDo
			move.w X:(R3)+,Y0
			move.w Y0,X:(R2)+
EndDo:
			rts     
}



extern int * _StackAddr;     /* Defined in linker.cmd */
extern int * _StackEndAddr;

/*******************************************************
* Interface to check for stack overflow
*
* To enable the stack check, define #INCLUDE_STACK_CHECK in appconfig.h
*	
* stackcheckSizeAllocated() returns the stack size that was allocated in linker.cmd
*	
* stackcheckSizeUsed() returns the stack size actually used so far in the application	
*	
* Note that stack overflow has occurred if
*      stackcheckSizeUsed () > stackcheckSizeAllocated ()
*
*******************************************************/

asm void stackcheckInitialize (void)
{
		tfra     SP,r2
		move.l   r2,a
		inc.l    a
		move.l   #>>_StackEndAddr,r2
		move.l   r2,b
		sub      a,b
		blo      EndStkChk
		move.l   a10,R2
	
		move.w   #$A55A,X0
StoreStk:
		move.w   X0,X:(R2)+
		dec.l    b
		bge      StoreStk
		nop
		nop
EndStkChk:
		rts	
}


asm long stackcheckSizeUsed (void)
{

		move.l   #>>_StackEndAddr,r2
		move.l   r2,a
		dec.l a
		move.l   a10,R2
		nop
		nop
UnusedLoop:
		move.w   X:(R2)-,Y0
		cmp.w    #$A55A,Y0
		beq    UnusedLoop
		move.l   #>>_StackAddr,r3
		move.l   R3,b
		move.l   R2,a
		sub      b,a
		inc.l    a
		inc.l    a
		rts
}


asm long stackcheckSizeAllocated (void)
{
/*	return (int)_StackEndAddr - (int)_StackAddr + 1; */

		move.l   #>>_StackEndAddr,r2
		move.l   #>>_StackAddr,r3
		move.l r2,a
		move.l r3,b
		sub      b,a
		rts
}