syslib.c

56f8300E系列dsp的BOOTloader

/*******************************************************************************
*
* Motorola Inc.
* (c) Copyright 2002 Motorola, Inc.
* ALL RIGHTS RESERVED.
*
* $Element: /project/dsp568_sdk/sdk/src/dsp56838evm/nos/tools/test.c $ 
* $Author: saa $ 
* $Revision: /main/2 $ 
* $VOB: /project/dsp568_sdk $ 
* $OS: solaris $ 
*
* Description:  Standard SDK test support routines
*
* Notes:     Use non-standard access to Metrowerks output libraray 
*            to eliminate "stdio" overhead for memory reduction
*
******************************************************************************/

#include "port.h"
#include "syslib.h"
#include <size_t.h>

/*****************************************************************************
*
* Module: U64addU32
*
* Description: addition unsigned 64 and unsigned 32 bits numbers
*
* Returns: none
*
* Arguments: pFirst     - pointer to unsigned U64 value
*            second     - 32 bits unsigned number
*            pRezult    - not null pointer to U64 value
*
* Range Issues: 
*
* Special Issues:  
*           Register discription:
*                       R2 - pointer to U64 value
*                       A  - 32 bits number
*                       R3 - not null pointer to U64 value
*
*                       Module doesn't save register: B
*
* if overflow occurs then carry bit will be set
*
* Test Method: 
*
*****************************************************************************/
void U64addU32(const struct U64 * pFirst, const unsigned long second, struct U64 * pResult)
{
    asm(adda    #1, SP      );

    asm(move.w  A0, X:(SP)+ );
    asm(move.w  A1, X:(SP)+ );
    asm(move.w  B0, X:(SP)+ );
    asm(move.w  B1, X:(SP)  );
    

    asm(move.l  X:(R2), B   );      /* Prepare data for addition least 32-bits */
    asm(add     A, B            );      /* Add least 32-bits  */
    asm(move.l  B10, X:(R3) );      /* Save least 32-bits */                               
    asm(move.l  X:(R2+2), B );      /* copy high 32-bits to result */
    asm(move.l  #0, Y       );
    asm(adc     Y, B            );
    asm(move.l  B10, X:(R3+2));         /* Save least 32-bits */    
    
    asm(move.w  X:(SP)-, B1 );
    asm(move.w  X:(SP)-, B0 );
    asm(move.w  X:(SP)-, A1 );
    asm(move.w  X:(SP)-, A0 );
        
    asm(rts                 );
}

/*****************************************************************************
*
* Module: U64mulU32
*
* Description: multiplication unsigned 64 and unsigned 32 bits multiplier
*
* Returns: none
*
* Arguments: pFirst     - pointer to U64 value
*            second     - 32 bits multiplier
*            pRezult    - not null pointer to U64 value
*
* Range Issues: 
*
* Special Issues:  
*           Register discription:
*                       R2 - pointer to U64 value
*                       A  - 32 bits number
*                       R3 - not null pointer to U64 value
*
*                       Module doesn't save registers: B, Y
*
* Return: return higest bits of results (64..96 bits) in register A
*
* Test Method: 
*
*****************************************************************************/
unsigned long U64mulU32(const struct U64 * , const unsigned long , struct U64* )
{       
    asm(adda    #1, SP      );

    asm(move.w  B0, X:(SP)+ );
    asm(move.w  B1, X:(SP)+ );
    asm(move.w  Y0, X:(SP)+ );
    asm(move.w  Y1, X:(SP)  );

    asm(move.l  #0, Y       ); /* prepare register to use as carry number*/
                
    asm(do      #4, ENDDO   ); /* multiply step by step evry 16-bits of U64 */      
    asm(move.w  X:(R2), B0  ); /* take current 16-bits of U64 (start from least) */             
    asm(impyuu  A0, B0, B       ); /* multiply least part of multiplier */
    asm(add     Y, B            ); /* add carry number */
    asm(move.w  B0, Y0      ); /* save 16 bits of resuls */
    asm(move.w  X:(R2)+, B0 ); /* take next 16-bits of U64 */
    asm(move.w  Y0, X:(R3)+  ); /* save our current result to memory ( it's use to realise such possibility U64mul32(&a, b, &a)) */
    asm(impyuu  A1, B0, Y       ); /* multiply high part of multiplier */
    asm(move.w  B1, B0      ); /* sum up carry number */
    asm(clr.w   B1          );
    asm(add     B, Y            );
    
    asm(ENDDO:              );  

    asm(move.w  Y0, A0      ); /* if overflow occures, the higest bits will be in register A (return value) */
    asm(move.w  Y1, A1      );

    asm(adda    #-4, R2     );
    asm(adda    #-4, R3     );
    
    asm(move.w  X:(SP)-, Y1 );
    asm(move.w  X:(SP)-, Y0 );  
    asm(move.w  X:(SP)-, B1 );  
    asm(move.w  X:(SP)-, B0 );  
    
    asm(rts                 ); 
};

/*****************************************************************************
*
* Module: incU64
*
* Description: increase U64
*
* Returns: carry bit set if pU64 == 0xFFFF FFFF FFFF FFFF
*
* Arguments: pU64 - not null pointer to 64 bits dividend
*
*
* Range Issues: 
*
* Special Issues:  For executive loop only. Module doesn't save register: A
*                                           
* Test Method: 
*
*****************************************************************************/
void incU64(struct U64 * )
{                       
    asm(adda    #1, SP      );

    asm(move.w  A0, X:(SP)+ );
    asm(move.w  A1, X:(SP)  );
                                                            
    asm(move.l  X:(R2), A           );  /* movel low bits to A */
    asm(inc.l   A                   );  /* increase A */
    asm(move.l  A10, X:(R2)         );  /* save result */
    asm(move.l  X:(R2+2), A         );  /* movel high bits to A */
    asm(brclr   #1, SR, CarryBitClear   );  /* test carry bit ; carry bit set if overflow occurs*/
    asm(inc.l   A                   );  /* inc hifh bits if overflow occurs */
            
    asm(CarryBitClear:              );      
    asm(move.l  A10, X:(R2+2)       );  /* save high bits */
    
    asm(move.w  X:(SP)-, A1         );
    asm(move.w  X:(SP)-, A0         );

    asm(rts                         );  
}

/*****************************************************************************
*
* Module: U32divU16s
*
* Description: division unsigned 32 bits value by unsigned 16 bits value divider
*
* Returns: quotient
*
* Arguments: Dividend - not null pointer to 32 bits dividend
*            Divisor    - 16 bits unsigned divider
*
* Range Issues: 
*
* Special Issues:  For executive loop only. Module doesn't save registers: A, B, Y, X0
*
* Test Method: 
*
*****************************************************************************/
unsigned int U32divU16s(const unsigned long , const unsigned int , unsigned long * )
{
    
    asm(move.w  Y0, Y1      );  /* save real divider */
    asm(brclr   #$8000, Y0, Core);/* if (sign bit is not set) then go core */
    asm(lsr.w   Y1          );  /* new divider:=divider>>1 */

        /* division */       
    asm(Core:               );  /* main part of division */
    
    asm(clr     B           );  /* preparing for division */
    asm(move.w  A1, B0      );  /* move high number position in register B */
    asm(bfclr   #$0001, SR  );  /* clear carry flag ( for first shifting )) */

    asm(rep     #17         );  /* 16-bit for division + 1-bit for sign */
    asm(div     Y1, B       );  /* Positive quotient in B0 */

    asm(move.w  B0, A1      );  /* Save quotient in A1 */
        
    asm(add     Y1, B       );  /* Restore remainder in B1 */
    asm(asr     B           );  /* Required for correct integer remainder */
        
    asm(move.w  A0, B0      );  /* move low number position in register B */        
    asm(bfclr   #$0001, SR  );  /* clear carry flag ( for first shifting )) */
        
    asm(rep     #17         );  /* 16-bit for division + 1-bit for sign */
    asm(div     Y1, B       );  /* Positive quotient in B0 */
        
    asm(move.w  B0, A0      );  /* Save quotient in A0 */
    
    asm(add     Y1, B       );  /* Restore remainder in B1 */
    asm(asr     B           );  /* Required for correct integer remainder */
        
    /* correct the result if divisor is odd number */       
    asm(bfclr   #$FF, A2        );
    asm(bfclr   #$FF, B2        );
        
    asm(brclr   #$8000, Y0, Endofdiv); /* if sign bit not set go Endofdiv */
    asm(brclr   #$0001, A0, EvenA   );   /* if quotient(A/[Y>>1]) is even go OddA */
    
    asm(add     Y1, B       );       /* odd quotient. remainder(A/Y)=remainder(A/[Y>>1])+Y>>1 */
    
    asm(EvenA:              );
    
    asm(asr     A           );       /* quotient(A\[Y>>1])>>=1 */
    asm(brclr   #$0001, Y0, Endofdiv); /* if divider is even go Endofdiv */
    asm(move.w  B1, B0      );       /* for addition operation */
    asm(clr.w   B1          );
    asm(clr.w   X0          );
    asm(clr.w   Y1          );
    
    /* while(remainder<0) do {remainder+=divider; quotient+=1 } */  
    asm(Circle:             );
    
    asm(cmp     A, B            );  /* no more than 4 times */
    asm(bge     GetK        );      
    asm(add     Y, B            );  /* Y = divider */
    asm(inc.w   X0          );
    asm(bra     Circle      );
    
    asm(GetK:               );
    
    asm(sub     A, B            );  /* the same: remainder+=divider*k, k={0, 1, 2, 3, 4} */
    asm(move.w  B0, B1      );  /* calling convention */
    asm(move.w  Y0, B0      );  
    asm(move.w  X0, Y0      );
    asm(bfclr   #$0001, SR  );
    asm(sbc     Y, A            );
    asm(move.w  B0, Y0      );
    
    asm(Endofdiv:           ); /* end of division */
    
    asm(move.l  A10, X:(R2)  ); /* save results of division */
    asm(move.w  B1, Y0      );
    asm(rts                 );
}

/*****************************************************************************
*
* Module: U64divU16
*
* Description: division unsigned 64 bits value by unsigned 16 bits value divider
*
* Returns: remainder
*
* Arguments: puDividend - not null pointer to 64 bits dividend
*            uDivider   - 16 bits unsigned divider
*            puQuotient - not null pointer to 64 bits quotient
*
* Range Issues: 
*
* Special Issues:  For executive loop only. Module doesn't save registers: A, B, Y, X0, C1, D1
*                                           Use SP
* Test Method: 
*
*****************************************************************************/
unsigned int U64divU16(const struct U64 * , const unsigned int , struct U64 * )
{       
    asm(adda    #1, SP      );
    asm(move.w  Y1, X:(SP)+ );
    asm(move.w  B0, X:(SP)+ );
    asm(move.w  B1, X:(SP)+ );
    asm(move.w  A0, X:(SP)+ );
    asm(move.w  A1, X:(SP)+ );
    asm(move.w  D1, X:(SP)+ );
    asm(move.w  C1, X:(SP)+ );
                                                                        
    asm(move.l  X:(R2), B   );  /* save X:(R2), X:(R2+1) to X:(SP) */
    asm(move.w  B0, X:(SP)+  );
    asm(move.w  B1, X:(SP)  );          
    asm(move.w  Y0, C1      );  /* save divider (Y0) to C1 */

    asm(move.l  X:(R2+2), A  ); /* move 2 high number positions in register A */    
    asm(jsr     U32divU16s  );  /* divide high positions */     
    asm(move.l  A10, x:(R3+2)); /* save high number positions of results */ 
                
    asm(move.w  Y0, A1      );  /* move reminder of first division  */
    asm(move.w  X:(SP), A0  );  /* prepare next 16 bit for division */
    asm(move.w  C1, Y0      );  /* restore divider */
    asm(jsr     U32divU16s  );  /* divide middle number position */
    asm(move.w  A0, D1      );  /* save middle number position of result */
                                /* not at once to R3 because if user write U64divU16(&a, b, &a) */      
                                /* next division destroy this result */
    asm(move.w  Y0, A1      );  /* move reminder of second division  */
    asm(move.w  X:(SP-1), A0    );  /* prepare next 16 bit for division */
    asm(move.w  C1, Y0      );  /* restore divider */
    asm(jsr     U32divU16s  );  /* divide low number position */ 

    asm(move.w  X:(SP)-, B1 );  /* restore X:(R2), X:(R2+1) */
    asm(move.w  X:(SP)-, B0 );
    asm(move.l  B10, X:(R2) );
    asm(move.w  D1, A1      );  /* save results of low bits D1A0 -> X:(R3) */
    asm(move.l  A10, x:(R3) );


    asm(move.w  X:(SP)-, C1 );
    asm(move.w  X:(SP)-, D1 );
    asm(move.w  X:(SP)-, A1 );
    asm(move.w  X:(SP)-, A0 );
    asm(move.w  X:(SP)-, B1 );
    asm(move.w  X:(SP)-, B0 );  
    asm(move.w  X:(SP)-, Y1 );
    
    asm(rts                 );  
}

unsigned int U32divU16(const unsigned long dividend, const unsigned int divisor, unsigned int * pReminder)
{
    //asm( move.w  #DAC_CLUSTER_SIZE , Y0 );
    asm( asl     A );
    asm( bfclr   #$0001 , SR );
    asm( rep     16 );
    asm( div     Y0 , A );

    asm( add     Y0 , A );
    asm( move.w	 A0 , Y0 );	/* quotient */
    asm( asr     A );
    asm( move.w  A1 , X:(R2) ); /* remainder */
};

/********************************************************************        
;
; void * memCopyPtoP ( void *dest, const void *src, ssize_t count );
; #pragma interrupt  // Can be used in a pragma interrupt ISR 
;
;    Register usage:
;       R2 - dest
;       R3 - src
;       Y0 - count/temp
;
; Note:  The current implementation only supports setting P memory
;        on word boundaries.
;
;********************************************************************/

void * memCopyPtoP ( void *dest, const void *src, ssize_t count )
{
	asm (brset   #$0001,R2,ByteAlignment);
	asm (brset   #$0001,R3,ByteAlignment);
	asm (brclr   #$0001,Y1,ParamsOK);
ByteAlignment:
	asm(debughlt);    //  Current implementation does not support byte alignment
ParamsOK:
	asm (asra    R2);
	asm (asra    R3);

#if __m56800E_lmm__ 
    asm(move.w A0,A);
    asm(move.w  A1,Y0);
#else
	asm(nop);
	asm(nop);
#endif

	asm (asr     Y0);
	asm (tst.w   Y0);
	asm (beq     EndDo);
			
	asm (do      Y0,EndDo);
			
	asm (move.w  P:(R3)+,Y0);
	asm (move.w  Y0,P:(R2)+);
EndDo:
	asm (asla    R2);
	// R2 - Contains *dest return value
} 

/********************************************************************        
;
; Word32 memReadP32  (Word32 *pX);
; #pragma interrupt  // Can be used in a pragma interrupt ISR 
;
; ;    Register usage:
;       R2 - pX
;       A  - return value
;       X0 - temp
;
;
;********************************************************************/
UWord32 memReadP32 ( UWord32 *pX )
{
	asm (adda   #2,SP);
	asm (move.w X0,x:(SP));
	asm (move.w A0,X0);
	asm (move.w P:(R2)+,X0);
	asm (move.w P:(R2)+,A);
	asm (move.w X0,A0);
	asm (suba   #2,SP);
} 
/********************************************************************        
;
; void * memCopyPtoX ( void *dest, const void *src, ssize_t count );
; #pragma interrupt   /* Can be used in a pragma interrupt ISR */
/*;
;    Register usage:
;       R2 - dest
;       R3 - src
;       Y0 - count/temp
;
;********************************************************************/
 void * memCopyPtoX ( 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    P:(R3)+,Y0);
			asm (move.w    Y0,X:(R2)+);
			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);
			asm (move.l  R4,x:(SP)+);
			asm (move.w  X0,x:(SP)); /*    ; push X0*/
			
			asm (move.l  #1,R4); /*        ; R4 = constant 1*/
			
			asm (move.w  R3,X0);
			asm (lsra    R3);
			asm (andc    #$0001,X0);

			asm (do      Y0,EndDo);
			
			asm (move.w  P:(R3)+,Y0);