#define OS_CPU_A
#include "../CONFIG/Os_cpu.h"

.text
.section .text

OSCtxSw:
 		PUSHRS	
		
		lds	r24,Os_Enter_Sum
		push 	r24

		in	r24, SREG	;
		push	r24

;	SaveSP=SP;
		in	r6, SPL
		in	r7, SPH

#if  EN_SP2>0  
.extern Sp2
		in r0,SREG
		cli
		ldi r25,hi8(Sp2+Sp2Space-1)
		ldi r24,lo8(Sp2+Sp2Space-1)
		out SPH,r25
		out SREG,r0
		out SPL,r24
#endif
	
	;cp1=SaveSP;
      	movw	r8, r6
				
;	temp_p= (uint16)OSTaskStackBottom[OSNextTaskID+1];
		lds	r24, OSNextTaskID
     	mov	r18, r24
      	eor	r19, r19
       	movw	r24, r18
      	add	r24, r18
      	adc	r25, r19
      	movw	r30, r24
      	subi	r30, lo8(-(OSTaskStackBottom+2))	
      	sbci	r31, hi8(-(OSTaskStackBottom+2))
       	ld	r12, Z
      	ldd	r13, Z+1	
				
	;cp2=(uint16)(OSTaskStackBottom[OSTaskID+1]);
 		lds	r24, OSTaskID
      	mov	r18, r24
       	eor	r19, r19
       	movw	r24, r18
      	add	r24, r18
      	adc	r25, r19
      	movw	r30, r24
       	subi	r30, lo8(-(OSTaskStackBottom+2))
       	sbci	r31, hi8(-(OSTaskStackBottom+2))
      	ld	r10, Z
       	ldd	r11, Z+1	


;	temp16=cp1-cp2;
      	movw	r4, r8
      	sub	r4, r10
      	sbc	r5, r11
	
	;if(OSTaskID<OSNextTaskID)
		lds	r24, OSTaskID
		lds	r25, OSNextTaskID
      	cp	r24, r25
      	brcs	.+2      
      	rjmp	OSCtxSw_5    
		
		;{while(cp2!=temp_p)
OSCtxSw_1:
      	cp	r10, r12
       	cpc	r11, r13
     	breq	OSCtxSw_2    
	
		;{*(uint8 *)cp1=*(uint8 *)cp2;
       	movw	r30, r8
      	movw	r26, r10
      	ld	r24, X
     	st	Z, r24
		
		;	cp1--;cp2--;}
       	sec
     	sbc	r8, r1
      	sbc	r9, r1
      	sec
      	sbc	r10, r1
     	sbc	r11, r1
      	rjmp	OSCtxSw_1    			
	;	
OSCtxSw_2:
	
	;	temp16=cp1-cp2;
      	movw	r4, r8
      	sub	r4, r10
      	sbc	r5, r11
		
;		for(i8=OSTaskID+1;i8<OSNextTaskID+1;i8++)
		lds	r24,OSTaskID
      	mov	r2, r24
      	inc	r2

OSCtxSw_3:        
	  	mov	r18, r2
     	eor	r19, r19
 		lds	r24, OSNextTaskID
      	eor	r25, r25
       	adiw	r24, 0x01	; 1
      	cp	r18, r24
      	cpc	r19, r25
      	brge	OSCtxSw_4    
		
		;	{	OSTaskStackBottom[i8]+=temp16;	}
     	mov	r18, r2
      	eor	r19, r19
    	movw	r24, r18
      	add	r24, r18
      	adc	r25, r19
     	movw	r26, r24
      	subi	r26, lo8(-(OSTaskStackBottom))	
      	sbci	r27, hi8(-(OSTaskStackBottom))	
      	
		movw r30,r26
       	ld	r24, Z
      	ldd	r25, Z+1
      	add	r24, r4
       	adc	r25, r5
       	adiw	r26, 0x01	; 1
      	st	X, r25
      	st	-X, r24
       	inc	r2
       	rjmp	 OSCtxSw_3    
	
	;	OSTaskID=OSNextTaskID;
OSCtxSw_4:
	lds	r24, OSNextTaskID
 	sts	OSTaskID, r24

		;SP=cp1;
		in R0,SREG
		cli
 		out	SPH, r9
		out SREG,R0
 		out	SPL, r8
	
;		goto loadctx;
       	rjmp	OSCtxSw_11  	


;	else if(OSNextTaskID<OSTaskID)
OSCtxSw_5:
		lds	r25, OSNextTaskID
 		lds	r24, OSTaskID
      	cp	r25, r24
      	brcs	.+2      	
      	rjmp	OSCtxSw_10    	
		
		
		;{	while(cp1!=temp_p)
OSCtxSw_6:
    	cp	r8, r12
     	cpc	r9, r13
     	breq	OSCtxSw_7     
			
			;{		*(uint8 *)(++cp2)=*(uint8 *)(++cp1);}
     	sec
     	adc	r10, r1
     	adc	r11, r1
       	movw	r30, r10
      	sec
      	adc	r8, r1
      	adc	r9, r1
      	movw	r26, r8
      	ld	r24, X
      	st	Z, r24
      	rjmp	OSCtxSw_6    

	;
OSCtxSw_7:

	;	temp16=cp1-cp2;
      	movw	r4, r8
     	sub	r4, r10
     	sbc	r5, r11

		;for(i8=OSNextTaskID+1;i8<OSTaskID+1;i8++)
		lds	r24, OSNextTaskID
      	mov	r2, r24
      	inc	r2
OSCtxSw_8:     
	 	mov	r18, r2
      	eor	r19, r19
		lds	r24,OSTaskID
     	eor	r25, r25
      	adiw	r24, 0x01	
      	cp	 r18,r24
      	cpc	 r19,r25
     	brge	OSCtxSw_9   
	
;		{		OSTaskStackBottom[i8]-=temp16;}
     	mov	r18, r2
      	eor	r19, r19
      	movw	r24, r18
      	add	r24, r18
      	adc	r25, r19
      	movw	r26, r24
      	subi	r26, lo8(-(OSTaskStackBottom))	
       	sbci	r27, hi8(-(OSTaskStackBottom))	
      	
		movw r30,r26
  		ld	r24, Z
      	ldd	r25, Z+1
      	sub	r24, r4
       	sbc	r25, r5
     	adiw	r26, 0x01	; 1
      	st	X, r25
      	st	-X, r24
      	inc	r2
        	rjmp	OSCtxSw_8     

	;	OSTaskID=OSNextTaskID;
OSCtxSw_9:
 		lds	r24, OSNextTaskID
		sts	OSTaskID, r24

	;SP=cp1
		in R0,SREG
		cli
 		out	SPH, r9
		out SREG,R0
 		out	SPL, r8

	;	goto loadctx;
   		rjmp	OSCtxSw_11     	
OSCtxSw_10:
 ;      else SP=SaveSP;
	   	in r0,SREG
	   	cli
		out	SPH, r7
		sts 0x5f,r0
		sts	0x005D, r6
	;	goto loadctx;

OSCtxSw_11:	


		#if EN_OSStkChk
		.extern OS_Stk_Null
		.extern OS_Stk_Min_Null
		sts OS_Stk_Null+1,r5
		sts OS_Stk_Null,r4
		lds r25,OS_Stk_Min_Null+1
		lds r24,OS_Stk_Min_Null
		cp r24,r4
		cpc r25,r5
		brcc 	.+2
		Rjmp OSCtxSw_11_1
		sts OS_Stk_Min_Null+1,R5
		sts OS_Stk_Min_Null,R4
#endif


OSCtxSw_11_1:	
;loadctx:

		pop r0		//SREG暫存
		pop r24		//Os_Enter_Sum
		sts Os_Enter_Sum,r24	
		and r24,r24
		brne	OSCtxSw_12		
		OS_SET_INT

OSCtxSw_12:	
		out SREG,r0	
		POPRS	
		ret