#------------------------------------------------------------------------------
#-         ATMEL Microcontroller Software Support  -  ROUSSET  -
#------------------------------------------------------------------------------
# The software is delivered "AS IS" without warranty or condition of any
# kind, either express, implied or statutory. This includes without
# limitation any warranty or condition with respect to merchantability or
# fitness for any particular purpose, or against the infringements of
# intellectual property rights of others.
#-----------------------------------------------------------------------------
#- File source          : os_cpu.s
#- Object               : AT91 Ucos specific assembly functions
#-
#- 1.0 21/08/00 EL      : creation
#------------------------------------------------------------------------------

	.IFDEF AT91R40008
	.INCLUDE "eb40a.inc"
	.ENDIF

	.IFDEF AT91R40807
	.INCLUDE "eb40.inc"
	.ENDIF

	.IFDEF AT91M40800
	.INCLUDE "eb40.inc"
	.ENDIF

	.IFDEF AT91M42800
	.INCLUDE "eb42.inc"
	.ENDIF

	.IFDEF AT91M63200
	.INCLUDE "eb63.inc"
	.ENDIF

	.IFDEF AT91M55800
    .include "parts/m55800/m55800.inc"
    .INCLUDE "periph/arm7tdmi/arm.inc"
#--------------------------------
#- Advanced Interrupt Controller
#--------------------------------

     .INCLUDE         "../../periph/aic/aic.inc"

    .EQU	TC0_BASE,      TCB0_BASE      /* Channel 0 Base Address */
	.ENDIF
	
	.EXTERN OSTCBCur
	.EXTERN OSTCBHighRdy
	.EXTERN OSPrioCur
	.EXTERN OSPrioHighRdy
    .EXTERN OSRunning
    .EXTERN OSIntNesting
    .EXTERN OSTimeTick
    .EXTERN OSIntEnter
    .EXTERN OSIntExit
           
	.GLOBAL OSTickISR
 	.GLOBAL ARMDisableInt
	.GLOBAL ARMEnableInt
	.GLOBAL OSIntCtxSw
	.GLOBAL OS_TASK_SW
	.GLOBAL OSStartHighRdy


#--------------------------------------------------------------------------------------
ARMDisableInt:             	    
	mrs     r12, CPSR           /* get current CPU mode 			  */
  	orr     r12, r12, #I_BIT    /* set the interrupt disable mode bit */
  	msr     CPSR_c, r12         
  	bx	lr                  
#--------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------
ARMEnableInt:
  	mrs 	r12, CPSR             /* move current processor status into reg 12  */
  	bic 	r12, r12, #I_BIT      /* clear the interrupt disable bit 			*/
  	msr 	CPSR_c, r12           
  	bx	lr                    
#--------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------
OSIntCtxSw:

# Interrupt Exit if higher priority task ready to run
# New Task Context switch 

	add	sp,sp, #0x18             /* adjust sp to compensate for the call to OSIntExit */
	msr     CPSR_c, #0x92           /* switch to IRQ mode and disable IRQ's              */
			                /* the following 2 lines keep the stack				 */
  	ldmfd   sp!, {r12, r14}         /* restore the stack 								 */
	ldmfd   sp!, {r12}   

	ldr     r12, =AIC_BASE
    str		r12, [r12, #AIC_EOICR]  /* write any value to EOICR to signal end of int	 */
   	msr     CPSR_c, #ARM_MODE_SYS   /* switch back to system mode with IRQ and FIQ enabled */
       
	b  	OSCtxSw                 	/* perform the context switch						 */
                                      
#--------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------
OS_TASK_SW:
# Call from OSSched()
# New Task Context switch  
  	stmfd 	sp!, {r0-r12, r14}		/* store current context 		*/
	b  	OSCtxSw                 	/* perform the context switch   */                                
#--------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------
OSCtxSw:                  	        
  	mrs 	r4, CPSR                /* load current psr to r4		*/
  	stmfd 	sp!, {r4}               /* save current PSR to stack    */
  	mrs 	r4, SPSR                /* load saved psr to r4         */
  	stmfd 	sp!, {r4}               /* saved saved psr to stack     */

	# OSPrioCur = OSPrioHighRdy
  	ldr 	r4, =OSPrioCur          /* load the current priority pointer */
  	ldr 	r5, =OSPrioHighRdy      /* load address of highest prio task */
  	ldrb  	r6, [r5]                /* load value of highest prio task   */
  	strb  	r6, [r4]                /* store value of highest in current */

	# Get current task TCB address
  	ldr 	r4, =OSTCBCur           /* load pointer						*/
  	ldr 	r5, [r4]                /* load current tcb sp              */
  	str 	sp, [r5]                /* store sp in preempted tasks's TCB*/

  	# Get highest priority task TCB address
  	ldr	r6, =OSTCBHighRdy
  	ldr 	r6, [r6]                /* get highrdy's sp		*/
  	ldr 	sp, [r6]                /* set sp = highrdy[sp] */

  	# OSTCBCur = OSTCBHighRdy
 	str 	r6, [r4]                /* set new current task TCB address		*/
  	ldmfd 	sp!, {r4}               /* load saved SPSR from stack           */
  	msr 	SPSR_c, r4              /* spsr = saved spsr                    */
  	ldmfd 	sp!, {r4}               /* load saved cpsr from stack           */
  	msr 	CPSR_c, r4              /* cpsr = saved cpsr                    */
  	ldmfd 	sp!, {r0-r12, r14}      /* restore r0-r12 and lr interworking   */
  	bx  	r14                     /* interworking branch on lr            */
#--------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------
OSStartHighRdy:
    # Set OSRunning == TRUE
  	ldr 	r4, =OSRunning          /* point r4 to OSRunning  	*/
	mov 	r5, #1                  /* move 1 into r5	        */
	strb 	r5, [r4]                /* store r5 in [r4]         */

    # Get current task TCB address
  	ldr 	r4, =OSTCBCur           /* point to current tcb  	*/

    # Get highest priority task TCB address
  	ldr 	r5, =OSTCBHighRdy       /* point to highest tcb  			*/
	ldr 	r5, [r5]                /* get stack pointer  	            */
	ldr 	sp, [r5]                /* sp = highrdy[sp]  	            */
	str 	r5, [r4]                /* set current tcb = highrdy tcb    */
                                    
	ldmfd 	sp!, {r4}               /* get spsr  									*/	
	ldmfd 	sp!, {r4}               /* get cpsr new state from top of the stack  	*/
	msr 	CPSR_c, r4              /* set cpsr = saved CPSR   	                    */
	ldmfd 	sp!, {r0-r12, r14}      /* restore registers r0-r12 and lr	            */
	bx	r14                     	/* branch on new task                           */
#--------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------
OSTickISR:
#- Adjust and save LR_irq in IRQ stack
        sub     r14, r14, #4
        stmfd   sp!, {r14}

#- Save SPSR and r0 in IRQ stack
        mrs     r14, SPSR
        stmfd   sp!, {r12, r14}

#- Write in the IVR to support Protect Mode
#- No effect in Normal Mode
#- De-assert the NIRQ and clear the source in Protect Mode
        ldr     r14, =AIC_BASE
        str     r14, [r14, #AIC_IVR]
    
# read the interrupt status reg to clear it
        ldr     r12,=TC0_BASE       	/* load tc0  base address			*/
        ldr     r12,[r12, #0x020]   	/* read from status register offse  */

#- Enable Interrupt and Switch in SYS Mode
        mrs     r12, CPSR
        bic     r12, r12, #I_BIT
        orr     r12, r12, #ARM_MODE_SYS
        msr     CPSR_c, r12              

#- Save scratch/used registers and LR in SYS Stack
        stmfd   sp!, { r0-r12,r14}
	            	
        ldr     r12, =OSIntEnter         
        mov     r14,pc                   
        bx      r12			 			/* Branch to OsIntEnter */
        ldr     r12, =OSTimeTick         
        mov     r14,pc                   
        bx      r12                     /* Branch to OsTimeTick */      
        ldr     r12, =OSIntExit          
        mov     r14,pc                   
        bx      r12                     /* Branch to OSIntExit 	*/
 
# Interrupt Exit if no higher priority task ready to run
         
# restore interrupted task  
#- Restore scratch/used registers and LR from System Stack
        ldmfd   sp!, { r0-r12, r14}
                    
#- Disable Interrupt and switch back in IRQ mode
        mrs	r12, CPSR
        bic     r12, r12, #ARM_MODE_SYS
        orr     r12, r12, #I_BIT|ARM_MODE_IRQ
        msr     CPSR_c, r12
        
#- Mark the End of Interrupt on the AIC
        ldr     r12, =AIC_BASE
        str	r12, [r12, #AIC_EOICR]
#- Restore SPSR_irq and r0 from IRQ stack
        ldmfd   sp!, {r12, r14}
        msr     SPSR_c, r14
#- Restore adjusted  LR_irq from IRQ stack directly in the PC
        ldmfd   sp!, {pc}^
        .END
#--------------------------------------------------------------------------------------


.END