arch.c

MC56F802BLDC 可以使用的算法 就是电机启动有点慢

/*****************************************************************************
*
* Motorola Inc.
* (c) Copyright 2000 Motorola, Inc.
* ALL RIGHTS RESERVED.
*
******************************************************************************
*
* File Name: arch.c
*
* Description: Architecture Dependent Implementations
*
* Modules Included:
*
*****************************************************************************/
#include "types.h"
#include "arch.h"



#ifdef __cplusplus
extern "C" {
#endif



/*****************************************************************************/
asm Flag archGetLimitBit()
{
   move  #0,Y0
   brclr #0x40,SR,LClear
   move  #1,Y0
LClear:
   rts
}



/*****************************************************************************/
asm bool archGetSetSaturationMode (bool saturationMode)
{
   move  #0,X0
   brclr #0x10,OMR,SClear
   move  #1,X0
SClear:

	cmp   #0,Y0
	beq   SatOff
	bfset #0x10,OMR
	move  X0,Y0
	rts
SatOff:
	bfclr #0x10,OMR
	move  X0,Y0
	rts
}



/*****************************************************************************/
asm void archDelay(UWord16 Ticks)
{
   move    y0,lc
   do      lc,archDelay1
   nop
archDelay1:
	rts
}

/*****************************************************************************/
asm void archPushAllRegisters(void)
{

; 
; Push ALL registers onto the stack EXCEPT for the following registers:
;
;     PC           => assumed to be global
;     SR           => assumed to be global or previously saved by interrupt mechanism
;     IPR          => assumed to be global
;     SP           => assumed to be global
;     0x38 - 0x3F  => Permanent register file used by CW
;

		lea   (sp)+
		move  n,x:(SP)+
		move  x0,x:(SP)+
		move  y0,x:(SP)+
		move  y1,x:(SP)+
		move  a0,x:(SP)+
		move  a1,x:(SP)+
		move  a2,x:(SP)+
		move  b0,x:(SP)+
		move  b1,x:(SP)+
		move  b2,x:(SP)+
		move  r0,x:(SP)+
		move  r1,x:(SP)+
		move  r2,x:(SP)+
		move  r3,x:(SP)+
		
		move  omr,x:(SP)+
		move  la,x:(SP)+
		move  m01,x:(SP)+
		move  lc,x:(SP)+

		;
		; save hardware stack
		;
		move  hws,x:(SP)+
		move  hws,x:(SP)+
		
		;
		; Save temporary register file at 0x30 - 0x37 used by compiler
		;
		move  x:<$30,y1
		move  y1,x:(SP)+
		move  x:<$31,y1
		move  y1,x:(SP)+
		move  x:<$32,y1
		move  y1,x:(SP)+
		move  x:<$33,y1
		move  y1,x:(SP)+
		move  x:<$34,y1
		move  y1,x:(SP)+
		move  x:<$35,y1
		move  y1,x:(SP)+
		move  x:<$36,y1
		move  y1,x:(SP)+
		move  x:<$37,y1
		move  y1,x:(SP)+
		
; move return address (SP-1) and sr
		move x:(sp-30),x0
		move x0,x:(sp)+
		move x:(sp-30),x0
		move x0,x:(sp)
		
		rts		
}

/*****************************************************************************/

asm void archPopAllRegisters(void)
{

; 
; Pop ALL registers from the stack in reverse   
; order from the routine archPushAllRegisters
;
		; 
		; To return, we must simulate the original 
		; jsr after popping all registers saved
		;
		; We use the stack space
		; from the call to archPushAllRegisters
		; to store the PC/SR for the RTS instruction 
		;

		;move return address and sr
		pop		x0
		move x0,x:(sp-29)
		pop		x0
		move x0,x:(sp-29)

		;
		; Pop temporary register file used by the compiler
		;
		pop   y1
		move  y1,x:<$37
		pop   y1
		move  y1,x:<$36
		pop   y1
		move  y1,x:<$35
		pop   y1
		move  y1,x:<$34
		pop   y1
		move  y1,x:<$33
		pop   y1
		move  y1,x:<$32
		pop   y1
		move  y1,x:<$31
		pop   y1
		move  y1,x:<$30

		;
		; restore hardware stack
		;
	 	pop hws
	 	pop hws
		
		;
		; restore all saved registers
		;
		pop   lc
		pop   m01
		pop   la
		pop   omr
		
		pop   r3
		pop   r2
		pop   r1
		pop   r0
		pop   b2
		pop   b1
		pop   b0
		pop   a2
		pop   a1
		pop   a0
		pop   y1
		pop   y0
		pop   x0
		pop	  n
		
		rts
}

/*****************************************************************************/
/* Primitives (impyuu, impysu) added to circumvent CW problems */


#undef add

asm unsigned long impyuu(unsigned short unsigA, unsigned short unsigB)
{	/* y0 = unsigA, y1 = unsigB */
	push  OMR			/* store the status of the OMR register */
	bfclr #0x10,OMR		/* set saturation OFF (SA = 0) */
	bfset #0x0100,OMR 	/* set Condition Code bit (CC) bit to 1 */
	move  y0,x0			/* save y0 (unsigA) to x0 */
	andc  #0x7fff,y0		/* mask the sign position - represent the positive sign number */
	mpysu y0,y1,a		/* multiply signed operand (y0) and unsigned operand (unsigB = y1)*/
	tstw  x0				/* test if MSB is 0 or 1 (set N flag in SR reg) */
	bge   OVER			/* jump accordig to N flag - if 1 pass, if 0 jump */
	movei #0x7fff,x0	/* store maximum positive value to x0 */	
	macsu x0,y1,a		/* multiply 0x7fff with unsigB and accumulate with the previous product */
	clr   b				/* clear B register, used as a temporary location */
	move  y1,b0			/* move unsigB to the LSP of B */
	add   b,a				/* correct the result, needed two times because of fractional multiplication */
	add   b,a 			/*  --||-- */
  OVER:
  	asr   a               /* fractional into integer number conversion */
	bge   OVERFLOW        /* branch according to V flag - if 1 jump, if 0 pass */
	bfclr #0x8000,a1    /* correction of the sign position - clear MSB bit in A */
  OVERFLOW:	
	pop   OMR             /* restore original contents of OMR */
  	rts
}

/*****************************************************************************/
/* asm Int32 impysu(Int16 sig, UInt16 unsig) */
asm long impysu(short sig, unsigned short unsig)
{
	mpysu y0,y1,a
	asr a
	rts
}

#define add             __add

/*****************************************************************************/
/* Otimized intrinsic (shl2, shr2) added  */

/* substitute for shl intrinsic - shift only in 1 direction (max. 15 shifts),
   shl2p function has exchanged parameters macro in arch.h swaps parameters
   to be equivalent as shl(), optimized function is called by shl2() 
*/
asm Frac16 shl2p(UWord16 shifts,Frac16 num)
{
 /* input Y0,Y1
    return in Y0
 */
 asll Y1,Y0,Y0
 rts 
}

/* substitute for shr intrinsic - shift only in 1 direction (max. 15 shifts),
   shr2p function has exchanged parameters macro in arch.h swaps parameters
   to be equivalent as shr(), optimized function is called by shr2() 
*/
asm Frac16 shr2p(UWord16 shifts,Frac16 num)
{
 /* input Y0,Y1
    return in Y0
 */ 
 asrr Y1,Y0,Y0
 rts 
}



#ifdef __cplusplus
}
#endif