////////////////////////////////////////////////////////////////// 
// SP_Inti_IOB
//////////////////////////////////////////////////////////////////    

_SP_Init_IOB: .PROC
		PUSH BP,BP TO [SP];
        BP = SP + 1;
		PUSH R1,R1 TO [SP];
		R1 = [BP+3];						// Port direction
		[P_IOB_Dir] = R1;
		R1 = [BP+4];
		[P_IOB_Data] = R1;
		R1 = [BP+5];
		[P_IOB_Attrib] = R1;
		
		POP R1,R1 FROM [SP];
        POP BP,BP FROM [SP];
        RETF;
		.ENDP



_SP_Import: .PROC
        PUSH BP,BP TO [SP];
        BP = SP + 1;
		PUSH R1,R2 TO [SP];
		R1 = [BP+3]; 						// Port Number
		R2 = [BP+4]; 						// Data Buffer Pointer
		R1 = [R1] ;
		[R2] = R1 ;
		POP R1,R2 FROM [SP];
        POP BP,BP FROM [SP];
        RETF;
		.ENDP

_SP_Export: .PROC
        PUSH BP,BP TO [SP];
        BP = SP + 1;
		PUSH R1,R2 TO [SP];
		R1 = [BP+3]; 						// Port Number
		R2 = [BP+4]; 						// Value
		[R1] = R2 ;
		POP R1,R2 FROM [SP];
        POP BP,BP FROM [SP];
        RETF;        
 		.ENDP
		 


	 
///////////////////////////////////////
_SP_RampUpDAC1:	.PROC
F_SP_RampUpDAC1:
                push r1,r2 to [sp];
                r1=[P_DAC1];
                r1 &= ~0x003f;
                cmp     r1,0x8000
                jb     	L_RU_NormalUp;  
                je      L_RU_End;
                
L_RU_DownLoop:
                call    F_Delay;        
                r2 = 0x0001;
                [P_Watchdog_Clear] = r2;
                r1 -= 0x40;
                [P_DAC1] = r1;
                cmp     r1,0x8000;
                jne     L_RU_DownLoop;  
L_RD_DownEnd:
                jmp     L_RU_End;

L_RU_NormalUp:
L_RU_Loop:
                call    F_Delay;
                r2 = 0x0001;
                [P_Watchdog_Clear] = r2;
                r1 += 0x40;
                [P_DAC1] = r1;
                cmp     r1, 0x8000;
                jne     L_RU_Loop;
                

L_RU_End:
                pop     r1,r2 from [sp];
                retf;
                .ENDP
/////////////////////////////////////// 
_SP_RampDnDAC1:	.PROC
F_SP_RampDnDAC1:
				push r1,r2 to [sp];
                //int off;
                r1 = [P_DAC1];
                r1 &= ~0x003F;
                jz      L_RD_End;
L_RD_Loop:                
                call    F_Delay;        
                r2 = 0x0001;
                [P_Watchdog_Clear] = r2;
                r1 -= 0x40;
                [P_DAC1] = r1;  
                jnz     L_RD_Loop;
L_RD_End:       
                
                pop     r1,r2 from [sp];
                retf;
				.ENDP

///////////////////////////////////////
_SP_RampUpDAC2:	.PROC
F_SP_RampUpDAC2:
                push r1,r2 to [sp];
                r1=[P_DAC2];
                r1 &= ~0x003f;
                cmp     r1,0x8000
                jb     	L_RU_NormalUp_;  
                je      L_RU_End;
                
L_RU_DownLoop_:
                call    F_Delay;        
                r2 = 0x0001;
                [P_Watchdog_Clear] = r2;
                r1 -= 0x40;
                [P_DAC2] = r1;
                cmp     r1,0x8000;
                jne     L_RU_DownLoop_;  
L_RD_DownEnd_:
                jmp     L_RU_End_;

L_RU_NormalUp_:
L_RU_Loop_:
                call    F_Delay;
                r2 = 0x0001;
                [P_Watchdog_Clear] = r2;
                r1 += 0x40;
                [P_DAC2] = r1;
                cmp     r1, 0x8000;
                jne     L_RU_Loop_;
                

L_RU_End_:
                pop     r1,r2 from [sp];
                retf;
                .ENDP
/////////////////////////////////////// 
_SP_RampDnDAC2:	.PROC
F_SP_RampDnDAC2:
				//int off;
				push r1,r2 to [sp];
                
                r1 = [P_DAC2];
                r1 &= ~0x003F;
                jz      L_RD_End_;
L_RD_Loop_:                
                call    F_Delay;        
                r2 = 0x0001;
                [P_Watchdog_Clear] = r2;
                r1 -= 0x40;
                [P_DAC2] = r1;  
                jnz     L_RD_Loop_;
L_RD_End_:       
                
                pop     r1,r2 from [sp];
                retf;
				.ENDP
////////////////////////////////////////

F_Delay:
                push r1 to [sp];
                r1 = 8;
L_D_Loop:
                r1 -= 1;
                jnz     L_D_Loop; 
                pop     r1 from [sp];
                retf;	 
                
                
///////////////////////////////////////////
// Function: Initial Queue
// Destory: R1,R2
///////////////////////////////////////////	

F_SP_InitQueue:
		R1 = R_Queue;
		R2 = 0;
L_ClearQueueLoop:		
		[R1++] = R2;
		cmp	R1, R_Queue+C_QueueSize;
		jne	L_ClearQueueLoop;
			
		R1 = 0;
		[R_ReadIndex] = R1;
		[R_WriteIndex] = R1;
				
		retf;
		
///////////////////////////////////////////
// Function: Get a data form Queue
// Output:  R1: Data
//			R2: return value
// Destory: R1,R2
///////////////////////////////////////////		
F_SP_ReadQueue:

		R2 = [R_ReadIndex];
		cmp R2,[R_WriteIndex];
		je	L_RQ_QueueEmpty;

		R2 += R_Queue;				// get queue data address
		R1 = [R2];
		
		R2 = [R_ReadIndex];
		R2 += 1;
		cmp	R2, C_QueueSize;
		jne	L_RQ_NotQueueBottom;
		R2 = 0;
L_RQ_NotQueueBottom:	
		[R_ReadIndex] = R2;			
		//r2 = 0x0000;						// get queue data
		retf;

L_RQ_QueueEmpty:
		//r2 = 0x8000;						// queue empty
		retf;
///////////////////////////////////////////
// Function: Put a data to Queue
// R1: Input
// Destory: R1,R2,R3
///////////////////////////////////////////	
			
F_SP_WriteQueue:
		R2 = [R_WriteIndex];			// put data to queue
		R2 += R_Queue;
		[R2] = R1;

		R2 = [R_WriteIndex];
		R2 += 1;
		cmp	R2, C_QueueSize;
		jne	L_WQ_NotQueueBottom;
		R2 = 0;
L_WQ_NotQueueBottom:
		[R_WriteIndex] = R2;
		retf;
		
//..........................................
F_SP_TestQueue:
		//... Test Queue Empty ...
		R1 = [R_ReadIndex];
		cmp R1,[R_WriteIndex];
		je	L_TQ_QueueEmpty;

		//... Test Queue Full ...
		R1 = [R_ReadIndex];				// For N Queue Full: 1. W=R-1 2.R=0/W=N-1
		jnz	L_TQ_JudgeCond2;
		R1 = [R_WriteIndex];
		cmp	R1, C_QueueSize-1;			// Cond1
		je	L_TQ_QueueFull;			
L_TQ_JudgeCond2:		
		R1 = [R_ReadIndex];
		R1 -=1;
		cmp R1,[R_WriteIndex];
		je	L_TQ_QueueFull;	

		r1 = 0;							// not Full, not empty
		retf;
L_TQ_QueueFull:
		r1 = 1;							// full
		retf;
L_TQ_QueueEmpty:
		r1 = 2;							// empty
		retf;
//.........................................                

//////////////////////////////////////////////////////
// SACM_GetResource(Address,Page,offset);
//////////////////////////////////////////////////////

_SP_GetResource:	.PROC
		push bp to [sp];
		bp = sp + 1;
		//push r2 to [sp];
		r1 = [bp+3];					// address
		r2 = [bp+4];					// bank
		
		r2 = r2 lsl 4;
		r2 = r2 lsl 4;
		r2 = r2 lsl 2;
		
		sr &= 0x03f;
        r2 |=sr;
        sr = r2;
  
  		r1 = D:[r1];
       
		//pop	r2 from [sp];
		pop	 bp from [sp];
		retf;
		.ENDP 

_SP_Delay:	.PROC
F_SP_Delay:


		retf;
		.ENDP


//========================================================================================        
// End of hardware.asm
//========================================================================================