//***************************************************************************
// File name      : Dynload.asm
// Author         : Chih-Chieh Tsao
// Final modified : Jan. 3, 2008 (Thr.)
// Description :
//     NEW ISR implementation based on NEW memory allocation
//     and only two trivial tasks,  (without removetask by itself)
//     extended with FIVE physical task address, support dynamic loading!!!
//***************************************************************************

////SFR
#include "intr.h"
#include "defines.h"

//R29 = stack pointer
//R30 = frame pointer
//R31 = return address


///////services
		org	0
					
		jmp	KernelInitial	//00				
		reti		//01
		reti		//02
		reti		//03
		reti		//04
		reti		//05
		reti		//06
		reti		//07
		reti		//08
		reti		//09
		reti		//0A
		reti		//0B
		reti		//0C
		reti		//0D
		reti		//0E
		reti		//0F
		jmp	intr_T0	   //reti 10
		reti    //jmp	intr_T1	//11
		jmp	intr_Key   // 12
		reti    //13        jmp	scheduler	
		
		reti		//14
		reti		//15
		reti		//16
		reti		//17
		reti    //18        
		reti		//19
		reti		//1A
		reti		//1B
		reti		//1C
		reti		//1D
		reti		//1E
		reti		//1F
//APIs
  //P0  jmp	Sys_EnableIntr	//20
  //P0  jmp	Sys_InsertTask	//21
  //P0  jmp	Sys_RemoveTask	//22  // 34 call
  //P0  jmp scheduler       //23
////////////////////////////////////////////////////////////////////////////////////////////
		
		//org 710h
//CurTask:	    dd		0,0
//StackTmp:     dd		0,0
//StackTmp2:    dd		0,0
//StackTmp3:    dd		0,0
//StackTmp4:    dd		0,0
		
		//org		3000h
		org		KernelCode
KernelInitial proc
// simulator initial

  ResetReg
  
  lea R29, StackStart         
  lea R30, CurTask
    
  P0  store.8 R0, R30, 0            //current task = NULL
  P0  ori R30, R0, 0

  //initial kernel context
  lea R30, KernelStructStart
  InitReg R30, 0
  P0 ori R30, R0, 0
//  SaveReg R0, KernelStructStart

/*  initial Point to Task Struct address
    TaskIndexStart : 指向 Task Entry Table 的位址, 所有 Task 程式的 PC 值記錄在此
    R22 : iterator, 指向每個 Task Entry 的位址
    R23 : 記錄最多可能的 Task 個數, MaxTask, 每次遞減, 直到 0 跳出 loop
  Description:
    一開始先將 Task Entry Table 的值全設為 NULL
    接著加入一個 idle process 至 Task EntryTable
    接著啟動 Interrupt (EnableIntr)
    接著程式跳到 scheduler, 由 scheduler 決定將由哪個 process 執行
    ps : 一開始執行 idle process
*/
  //P0  ori R22, R0, TaskIndexStart
  lea R22, TaskIndexStart
  P0  ori R23, R0, MaxTask
Kernel_Initial_Loop:// set all tasks' pointer to NULL
  P0  store.8 R0, R22, 0
  P0  subi R23, R23, 1
  P0  addi R22, R22, DataSize
    
  P0  cmp.ne R23, R0, P1, P0
  P1  jmp Kernel_Initial_Loop  
 
// add IdleProcess  
  lea R1, IdleProcess
  P0 ori R2, R0, IdleTaskP
  P0 ori R3, R0, IdleTaskID
  
  P0 jmp.link Sys_InsertTask  

  //enable interrupt
  P0  jmp.link Sys_EnableIntr	             //call Sys_EnableIntr in supervisor mode  
  P0  jmp scheduler  
  endp
  
Sys_EnableIntr	proc		           //setup interrupt should in supervisor mode
    
  P0 WriteSFR.set sfr_T0, 0
  //P0 WriteSFR.set sfr_TC0, 0x03E8         //timer0 count 0 to 65535 (655350 ns)
  P0 WriteSFR.set sfr_TC0, 0x07D0     			//timer0 count 0 to 65535 (655350 ns)
  P0 WriteSFR.set	sfr_intren,0x05     			//enable interrupt 0x10 and 0x12 
  P0 WriteSFR.set sfr_intrpriority, 0x04    //set interrupt priority, key:high, timer:low
  
  //ResetClock    
  P0 STI			               //enable interrupts  
  P0 ret
  endp
  
intr_T0		proc
		StopT0				
		WriteSFR.set	sfr_debugport,0x1000		
			
	//-- do context save --            
	  P0 addi R27, R29, 0             // R27 = task R29
	  P0 addi R28, R30, 0					    // R28 = task R30
	  
	  lea R25, KernelStructStart
	  P0 load.8 R29, R25, DataSize*29   // get kernel stack pointer
	  
    push R31    
    P0 jmp.link ContextSaving              //context saveing, store and restore R31
    pop R31   	              
	//--
	
	  push R28	  	  
	  push R20	  
	  loadi.z.1 R28, 0x1
	  
	  lea R20, (KernelStructStart+DataSize*33)	  	   	  	  
	  P0 store.8 R28, R20, 0
	  
	  pop R20	  
	  pop R28
		jmp scheduler		
  //--
		reti
		endp	

intr_T1		proc
		WriteSFR.set	sfr_debugport,0x1100
		reti
		endp	


intr_Key	proc
  StopT0  
  //ReadSFR R31, sfr_IRA 
  
  //-- do context save --   
    P0 addi R27, R29, 0             // R27 = task stack pointer = R29
	  P0 addi R28, R30, 0					    // R28 = task R30
	  
	  lea R25, KernelStructStart
	  P0 load.8 R29, R25, DataSize*29
	//--  
  
  push R31
    
  P0 jmp.link ContextSaving           //ContextSaving, final will push R31
  //--  
  push R1
  push R2
  
  P0 loadi.z.0	R31,a_Key&0xFFFF
  P0 loadi.k.1	R31,a_Key>>16
  P0 load.4		R2,R31,0	//get Key
  P0 addi		R31,R2,0	//****force wait read ready,R1&R2 should different	
  P0 WriteSFR	sfr_debugport,R31
  
  P0  cmpi.eq  R31, 97, P7, P0 // press a, add task1, priority = 8   (DCT)
  P7  loadi.z.0	R1,Task1Start&0xFFFF
  P7  loadi.k.1	R1,Task1Start>>16    
  //P7  loadi.z.1	R1,0x3000  
  //P7  loadi.z.1	R1,0x4008  
  
  P7  addi R2, R0, Task1Priority 
  P7  addi R3, R0, Task1ID
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit
  
  P0  cmpi.eq  R31, 98, P7, P0 // press b, add task2, priority = 7   (IDCT)
  P7  loadi.z.0	R1,Task2Start&0xFFFF
  P7  loadi.k.1	R1,Task2Start>>16    
  //P6  loadi.z.1	R1,0x4000  
  
  P7  addi R2, R0, Task2Priority
  P7  addi R3, R0, Task2ID
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit  

  P0  cmpi.eq  R31, 101, P7, P0 // press e, add task3, priority = 6 
  //P7  loadi.z.0	R1,task1
  P7  loadi.z.0	R1,Task3Start&0xFFFF
  P7  loadi.k.1	R1,Task3Start>>16    
  
  P7  addi R2, R0, Task3Priority
  P7  addi R3, R0, Task3ID
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit

  P0  cmpi.eq  R31, 102, P6, P0 // press f, add task4, priority = 5 
  //P7  loadi.z.0	R1,task1
  P6  loadi.z.0	R1,Task4Start&0xFFFF
  P6  loadi.k.1	R1,Task4Start>>16    
  
  P6  addi R2, R0, Task4Priority
  P6  addi R3, R0, Task4ID
  P6  jmp.link Sys_InsertTask
  P6  jmp Key_exit
  
  P0  cmpi.eq  R31, 103, P7, P0 // press g, add task5, priority = 4 
  //P7  loadi.z.0	R1,task1
  P7  loadi.z.0	R1,Task5Start&0xFFFF
  P7  loadi.k.1	R1,Task5Start>>16    
  
  P7  addi R2, R0, Task5Priority
  P7  addi R3, R0, Task5ID
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit  

  P0  cmpi.eq  R31, 99, P7, P0 // press c, add small task1, priority = 3
  P7  loadi.z.0	R1,task1&0xFFFF
  P7  loadi.k.1	R1,task1>>16
  
  P7  addi R2, R0, SmallTask1P 
  P7  addi R3, R0, SmallTask1ID 
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit
  
  P0  cmpi.eq  R31, 100, P6, P0 // press d, add small task2, priority = 2
  P6  loadi.z.0	R1,task2&0xFFFF
  P6  loadi.k.1	R1,task2>>16
  
  P6  addi R2, R0, SmallTask2P
  P6  addi R3, R0, SmallTask2ID   
  P6  jmp.link Sys_InsertTask
  P6  jmp Key_exit    
  
Key_exit:
  pop R2
  pop R1
  pop R31
  P0 jmp scheduler     

  P0  reti
		endp	

 
DumpMemory proc  //dump memory from R2 to R3
  push R2
  push R3
dump_memory_loop:
  P0 cmp.gtu R2, R3, P7, P0
  P7 jmp dump_memory_exit
  P0 load.8 R4, R2, 0
  P0 WriteSFR sfr_debugport, R2
  P0 WriteSFR sfr_debugport, R4
  P0 addi, R2, R2, DataSize
  P0 jmp dump_memory_loop
dump_memory_exit:
  pop R3
  pop R2
  endp



		

Sys_InsertTask	proc		                      //input R1=task address, R2=task priority, R3=task id
  
//initial Task Struct
  lea R22, TaskIndexStart
  P0 subi R22, R22, DataSize
  //P0 ori R22, (TaskIndexStart-DataSize)                   //tast index 指向 task structure 位址, R23 的位址
  //P0  ori R22, R0, TaskIndexStart-DataSize           //tast index 指向 task structure 位址, R23 的位址
  
  P0  ori R23, R0, 0                                 //kernel 中 task stack 的第幾個位址
  
  //lea R24, (TaskStructStart)-TaskStructSize        //真正 task structure 的位址
  lea R24, TaskStructStart
  P0 subi R24, R24, TaskStructSize
  //P0  ori R24, R0, (TaskStructStart)-TaskStructSize  //真正 task structure 的位址
Initial_InsertTaskLoop:
  P0  cmpi.eq R23, MaxTask, P3, P0                         //若達到最大的 task number, 直接離開
  P3  jmp  InsertExit
  P0  addi R22, R22, DataSize
  P0  addi R24, R24, TaskStructSize
  P0  addi  R23, R23, 1
  P0  load.8 R25, R22, 0
  P0  cmpi.ne R25, 0, P4, P0
  P4  jmp Initial_InsertTaskLoop
  P0  store.8	R24,R22,0	              //save TaskStructure Address in Task Structure of Kernel Stack
  	

//initial task register
  InitReg R24, 0                      
  // R2 = task priority, R3 = task id
  P0  mix.4.r R2, R3, R2              
  P0  cmp.eq R0, R2, P5, P4
  
  P0  store.8   R2, R24,0             //R2 = | task priority(low) | task id(high) |
  P0  store.8   R1, R24, DataSize*31  //R31 = R1 = task address
  P0  store.8   R1, R24, DataSize*32  //initially, reti = task address  
    
  loadi.z.1 R25, 0x1
  P0 store.8 R25, R24, DataSize*33    //set task flag at the first time for jmp.link

InsertExit:
  P0 jmp.reg R31
  endp


Sys_RemoveTask	proc		                     //remove current task & move to next task  
  StopT0    
  push R27
  push R20
  
  lea R20, CurTask       
  P0 load.8 R27, R20, 0                        //R27 指向目前將被移除的 task index      
  P0 store.8 R0, R20, 0                        //將目前 task 移除  
  P0 store.8 R0, R27, 0                        //將目前task 從task index queue 移除
  
  pop R20
  pop R27

Remove_Exit:  
  P0 jmp.reg R31
  endp  


Sys_Loader proc  
  push R31
  push R8
  push R9
  push R10
  
  lea R10, CurTask           
  P0  load.8 R9, R10, 0              
  P0  load.8 R8, R9, 0

  P0  load.8 R10, R8, 0              //get R10 = | task priority(low) | task id(high) |
  
  P0  srli R10, R10, 32              //shift right 32 bits, R10 = | task id(low) | 0x0 |
  
  //if the task is idle task
  P0  cmpi.eq R10, IdleTaskID, P7, P0
  P7  jmp Loader_Exit
  
  //if the task is small task 1
  P0  cmpi.eq R10, SmallTask1ID, P7, P0
  P7  jmp Loader_Exit
  
  //if the task is small task 2
  P0  cmpi.eq R10, SmallTask2ID, P7, P0
  P7  jmp Loader_Exit
    
  
  //--- if the task are the other five tasks ---
  
  //---load task1's text and data---
  P0  cmpi.eq R10, Task1ID, P7, P0

  //clear bss to zero
  P7  loadi.z.0	R9, linkinfo1&0xFFFF	
  P7  loadi.k.1	R9, linkinfo1>>16
  P7  load.8  R8, R9, 8
  P7  mix.4.r  R8, R0, R8  
  P7  loadi.z.0	R9, Task1Data&0xFFFF	
  P7  loadi.k.1	R9, Task1Data>>16
  P7  jmp.link setBSSZero
  
  //copy data segment
  P7  loadi.z.0	 R9, task1dataLMA&0xFFFF	
  P7  loadi.k.1	 R9, task1dataLMA>>16  
  P7  loadi.z.0	R10, linkinfo1&0xFFFF	
  P7  loadi.k.1	R10, linkinfo1>>16
  P7  load.8 R10, R10, 4
  //P7  srli  R10, R8, 32
  P7  jmp.link LoadTextData
  
  //copy text segment
  P7  loadi.z.0	 R8, Task1Start&0xFFFF	
  P7  loadi.k.1	 R8, Task1Start>>16 
  P7  loadi.z.0	 R9, task1textLMA&0xFFFF	
  P7  loadi.k.1	 R9, task1textLMA>>16  
  P7  loadi.z.0	R10, linkinfo1&0xFFFF	
  P7  loadi.k.1	R10, linkinfo1>>16
  P7  load.8 R10, R10, 0
  P7  mix.4.r  R10, R0, R10  
  P7  jmp.link LoadTextData
  
  P7  jmp  Loader_Exit
  //---  
  
  //---load task2's text and data---
  P0  cmpi.eq R10, Task2ID, P7, P0

  //clear bss to zero
  P7  loadi.z.0	R9, linkinfo2&0xFFFF	
  P7  loadi.k.1	R9, linkinfo2>>16
  P7  load.8  R8, R9, 8
  P7  mix.4.r  R8, R0, R8  
  P7  loadi.z.0	R9, Task2Data&0xFFFF	
  P7  loadi.k.1	R9, Task2Data>>16
  P7  jmp.link setBSSZero
  
  //copy data segment
  P7  loadi.z.0	 R9, task2dataLMA&0xFFFF	
  P7  loadi.k.1	 R9, task2dataLMA>>16  
  P7  loadi.z.0	R10, linkinfo2&0xFFFF	
  P7  loadi.k.1	R10, linkinfo2>>16
  P7  load.8 R10, R10, 4
  //P7  srli  R10, R8, 32
  P7  jmp.link LoadTextData
  
  //copy text segment
  P7  loadi.z.0	 R8, Task2Start&0xFFFF	
  P7  loadi.k.1	 R8, Task2Start>>16 
  P7  loadi.z.0	 R9, task2textLMA&0xFFFF	
  P7  loadi.k.1	 R9, task2textLMA>>16  
  P7  loadi.z.0	R10, linkinfo2&0xFFFF	
  P7  loadi.k.1	R10, linkinfo2>>16
  P7  load.8 R10, R10, 0
  P7  mix.4.r  R10, R0, R10  
  P7  jmp.link LoadTextData
  
  P7  jmp  Loader_Exit
  //---  
  
  //---load task3's text and data---
  P0  cmpi.eq R10, Task3ID, P7, P0

  //clear bss to zero
  P7  loadi.z.0	R9, linkinfo3&0xFFFF	
  P7  loadi.k.1	R9, linkinfo3>>16
  P7  load.8  R8, R9, 8
  P7  mix.4.r  R8, R0, R8  
  P7  loadi.z.0	R9, Task3Data&0xFFFF	
  P7  loadi.k.1	R9, Task3Data>>16
  P7  jmp.link setBSSZero
  
  //copy data segment
  P7  loadi.z.0	 R9, task3dataLMA&0xFFFF	
  P7  loadi.k.1	 R9, task3dataLMA>>16  
  P7  loadi.z.0	R10, linkinfo3&0xFFFF	
  P7  loadi.k.1	R10, linkinfo3>>16
  P7  load.8 R10, R10, 4
  //P7  srli  R10, R8, 32
  P7  jmp.link LoadTextData
  
  //copy text segment
  P7  loadi.z.0	 R8, Task3Start&0xFFFF	
  P7  loadi.k.1	 R8, Task3Start>>16 
  P7  loadi.z.0	 R9, task3textLMA&0xFFFF	
  P7  loadi.k.1	 R9, task3textLMA>>16  
  P7  loadi.z.0	R10, linkinfo3&0xFFFF	
  P7  loadi.k.1	R10, linkinfo3>>16
  P7  load.8 R10, R10, 0
  P7  mix.4.r  R10, R0, R10  
  P7  jmp.link LoadTextData
  
  P7  jmp  Loader_Exit
  //--- 

  //---load task4's text and data---
  P0  cmpi.eq R10, Task4ID, P7, P0

  //clear bss to zero
  P7  loadi.z.0	R9, linkinfo4&0xFFFF	
  P7  loadi.k.1	R9, linkinfo4>>16
  P7  load.8  R8, R9, 8
  P7  mix.4.r  R8, R0, R8  
  P7  loadi.z.0	R9, Task4Data&0xFFFF	
  P7  loadi.k.1	R9, Task4Data>>16
  P7  jmp.link setBSSZero
  
  //copy data segment
  P7  loadi.z.0	 R9, task4dataLMA&0xFFFF	
  P7  loadi.k.1	 R9, task4dataLMA>>16  
  P7  loadi.z.0	R10, linkinfo4&0xFFFF	
  P7  loadi.k.1	R10, linkinfo4>>16
  P7  load.8 R10, R10, 4
  //P7  srli  R10, R8, 32
  P7  jmp.link LoadTextData
  
  //copy text segment
  P7  loadi.z.0	 R8, Task4Start&0xFFFF	
  P7  loadi.k.1	 R8, Task4Start>>16 
  P7  loadi.z.0	 R9, task4textLMA&0xFFFF	
  P7  loadi.k.1	 R9, task4textLMA>>16  
  P7  loadi.z.0	R10, linkinfo4&0xFFFF	
  P7  loadi.k.1	R10, linkinfo4>>16
  P7  load.8 R10, R10, 0
  P7  mix.4.r  R10, R0, R10  
  P7  jmp.link LoadTextData
  
  P7  jmp  Loader_Exit
  //--- 

  //---load task5's text and data---
  P0  cmpi.eq R10, Task5ID, P7, P0

  //clear bss to zero
  P7  loadi.z.0	R9, linkinfo5&0xFFFF	
  P7  loadi.k.1	R9, linkinfo5>>16
  P7  load.8  R8, R9, 8
  P7  mix.4.r  R8, R0, R8  
  P7  loadi.z.0	R9, Task5Data&0xFFFF	
  P7  loadi.k.1	R9, Task5Data>>16
  P7  jmp.link setBSSZero
  
  //copy data segment
  P7  loadi.z.0	 R9, task5dataLMA&0xFFFF	
  P7  loadi.k.1	 R9, task5dataLMA>>16  
  P7  loadi.z.0	R10, linkinfo5&0xFFFF	
  P7  loadi.k.1	R10, linkinfo5>>16
  P7  load.8 R10, R10, 4
  //P7  srli  R10, R8, 32
  P7  jmp.link LoadTextData
  
  //copy text segment
  P7  loadi.z.0	 R8, Task5Start&0xFFFF	
  P7  loadi.k.1	 R8, Task5Start>>16 
  P7  loadi.z.0	 R9, task5textLMA&0xFFFF	
  P7  loadi.k.1	 R9, task5textLMA>>16  
  P7  loadi.z.0	R10, linkinfo5&0xFFFF	
  P7  loadi.k.1	R10, linkinfo5>>16
  P7  load.8 R10, R10, 0
  P7  mix.4.r  R10, R0, R10  
  P7  jmp.link LoadTextData
  
  P7  jmp  Loader_Exit
  //--- 
  

  //--------------------------------------------
Loader_Exit:
  pop R10
  pop R9
  pop R8
  pop R31  

  P0 jmp.reg R31
  endp


setBSSZero proc                 //input argument: R8 = bss_size, R9 = task data start address
  P0  padd.8 R8, R8, R9
              
setbss_loop:        
  P0  cmp.ge R9, R8, P6, P0
  P6  jmp setbss_exit
  P0  store.8 R0, R9, 0
  P0  addi R9, R9, 8    
  P0  jmp setbss_loop

setbss_exit:  
  P0 ret
  endp


LoadTextData proc                 
//input argument: R10 = size, R9 = LMAstart address, R8 = VMAstart address
  
  push R11        
  push R12
  P0  padd.8 R10, R10, R9           //R10 = LMA + size  
  P0  ori  R12, R0, 0
      
loadtd_loop:      
  P0  psub.8  R12, R10, R9          //R10 = boundary, R9 = current load address
  P0  cmpi.eq  R12, 0, P6, P0
  P6  jmp loadtd_exit
  
  P0  cmpi.lt R12, 8, P6, P0
  P6  load.4  R11, R9, 0            //assume only 4 bytes remained
  P6  load.8  R12, R8, 0            //R12 = loaded data in RAM
  P6  check.4  R12, R12, R11        //Merge R12 and R11 in R11
  P6  store.8  R12, R8, 0           //restore to RAM
  P6  jmp  loadtd_exit
  
  P0  cmp.ge R9, R10, P6, P0        
  P6  jmp  loadtd_exit
  
  P0  load.8 R11, R9, 0             //R11 as load result
  P0  load.8 R12, R9, 4
  P0  mix.4.r  R11, R12, R11
  P0  store.8 R11, R8, 0
  P0  addi R9, R9, 8
  P0  addi R8, R8, 8  
  P0  jmp loadtd_loop
  
loadtd_exit:  
  pop R12
  pop R11
  P0 ret
  endp


begintask proc  
  LoadTaskData    
  
  //----------------
  testbit R25.16, P0, P5
  P5 jmp no_load
  
  lea R29, KernelStack
  //P0 WriteSFR sfr_debugport, R29
  
  push R31
    
  P0 jmp.link Sys_Loader
  
  pop R31
  
  //----------------
  
no_load:
  ResetClock
  StartT0  
  STI                             //STI, enable interrupt            

  //P0 andi R28, R25, 0xFFFF   
  //P0 ori R28, R28, 0x1   
  
  //WriteSFR	sfr_PRED,R28        //restore predicate bits
//
  //ReadSFR R28, sfr_PRED
  //cmp.eq R28, R0, P7, P6
//
  //StartT0
  testbit R25.16, P7,P6     
  

  P6 jmp.reg R26                //call task subroutine, R26 may be sfr_IRA  (reserve R31)
  P7 jmp.reg.link R26           //call task subroutine, R26 not sfr_IRA   (change R31)   
        
  P0 WriteSFR.and	sfr_intrctrl, 0xFE        //return from task, disable interrupt
  
  lea R26, KernelStructStart
	P0 load.8 R29, R26, DataSize*29           //restore kernel stack pointer
	
  P0 jmp.link Sys_RemoveTask                //do task removal
  P0 jmp scheduler                          //jmp scheduler, select next task  
  endp


ContextSaving	proc
  push R22
  push R23 
  push R20  
  
  lea R20, CurTask
  //P0 ori R20, R0, CurTask
  
  P0 load.8 R22, R20, 0
  pop R20

// store predicate bits
  //ReadSFR		R26,sfr_PRED
//
  
  P0 cmp.eq R22, R0, P3, P4
  P3 jmp NonContextSaving
  P0 load.8 R23, R22, 0
  P0 store.8 R1, R23, DataSize*1
  P0 store.8 R2, R23, DataSize*2
  P0 store.8 R3, R23, DataSize*3
  P0 store.8 R4, R23, DataSize*4
  P0 store.8 R5, R23, DataSize*5
  P0 store.8 R6, R23, DataSize*6
  P0 store.8 R7, R23, DataSize*7
  P0 store.8 R8, R23, DataSize*8
  P0 store.8 R9, R23, DataSize*9
  P0 store.8 R10, R23, DataSize*10
  P0 store.8 R11, R23, DataSize*11
  P0 store.8 R12, R23, DataSize*12
  P0 store.8 R13, R23, DataSize*13
  P0 store.8 R14, R23, DataSize*14
  P0 store.8 R15, R23, DataSize*15
  P0 store.8 R16, R23, DataSize*16
  P0 store.8 R17, R23, DataSize*17
  P0 store.8 R18, R23, DataSize*18
  P0 store.8 R19, R23, DataSize*19
  P0 store.8 R20, R23, DataSize*20
  P0 store.8 R21, R23, DataSize*21

  P0 store.8 R25, R23, DataSize*25
  //P0 store.8 R26, R23, DataSize*26
  //P0 store.8 R27, R23, DataSize*27
  //P0 store.8 R28, R23, DataSize*28
  
  //P0 store.8 R29, R23, DataSize*29
  P0 store.8 R27, R23, DataSize*29          //R27 = R29  
  P0 store.8 R28, R23, DataSize*30				  //R28 = R30
  
  //--
  //R20 = R23
  pop R20      
  //R21 = R22
  pop R21      
  //R30 = R31
  pop R30
  ReadSFR R19, sfr_IRA        //R19 = interrupted address
  
  P0 store.8 R30, R23, DataSize*31
  P0 store.8 R19, R23, DataSize*32
  
  P0 loadi.k.1 R19, 0x0
  P0 store.8 R19, R23, DataSize*33          //clear task flag
  //P0 store.8 R26, R23, DataSize*33          //clear task flag
  
  P0 store.8 R21, R23, DataSize*22
  P0 store.8 R20, R23, DataSize*23
  //P0 load.8 R0, R0, CurTask    // 清掉 running task
  //P0 store.8 R0, R0, CurTask    // 清掉 running task
  
  push R18  
  lea R18, CurTask
  //P0 ori R18, R0, CurTask
  
  P0 store.8 R0, R18, 0
  pop R18
  
  //R30 = R31
  push R30     
  //R21 = R22
  push R21     
  //R20 = R23
  push R20     
  //--
NonContextSaving:  

  //WriteSFR	sfr_PRED,R26          //restore predicate bits   
  pop R23
  pop R22  
  P0 ret
	endp								

scheduler	proc          
  
  push R20
  push R28  
  lea R20, KernelStructStart
  
  P0 addi R20, R20, DataSize*15
  P0 load.8 R15, R20, 0
    
  P0 addi R20, R20, DataSize   
  P0 load.8 R16, R20, 0
  
  P0 addi R20, R20, DataSize   
  P0 load.8 R17, R20, 0
  
  //--judge flag--
  lea R20, (KernelStructStart+DataSize*33)
  P0 load.8 R28, R20, 0
      
  P0 testbit R28.16, P6,P7  
  P6 store.8 R0, R20, 0  
  pop R28
  pop R20  
  P6 jmp RR_Execution  
  //--
      
  //指向 task index 的前一個位址, 方便在 loop 中先加上 DataSize
  lea R22, (TaskIndexStart-DataSize)    
  P0 ori R23, R0, 0                            //R23 表示目前最大的 priority number
  P0 ori R24, R0, MaxTask                      //最多需比較的 task 個數
  P0 ori R27, R0, 0
  
Scheduler_loop:                                // 尋找優先權最高的 task
  P0 cmp.eq R24, R0, P4, P0                    //R24 記錄還需要比較多少個 task 才能找到優先權最高的 task
  P4 jmp Scheduler_Exit                        //若全部找完, 跳離這個 loop
  P0 subi R24, R24, 1                          //所需要尋找的 task 遞減
  P0 addi R22, R22, DataSize                   //指向 task index
  P0 load.8 R25, R22, 0                        //R25 = task index 所指向的記憶體位址, 即存放 task context 的位址
  P0 cmp.eq R25, R0, P6, P0                    //若 task index 指向 NULL, 則此 task 尚未被 insert
  P6 jmp Scheduler_loop
  P0 load.8 R26, R25, 0                        //R26 = 此 task 的 priority
  //-- add 07.10.30 --
  P0 mix.4.r R26, R0, R26
  P0 cmp.eq R0, R26, P5, P1
  //--

  P0 cmp.gt R26, R23, P5, P1                   //R23 記錄目前最高的 priority, 初始值為0
  P1 jmp Start_Judge  
  P0 ori R23, R26, 0
  P0 ori R21, R22, 0                          //R21 = 指向目前 priority 最高的 task structure 的 index
  P0 ori R27, R25, 0                          //R27 = the TCB of the task with the current higest priority
  
  //--Reset all StackTmp--  
  lea R16, StackTmp           
  lea R15, StackTmpEnd  
  
Reset_Stack:  
  P0 store.8 R0, R16, 0
  P0 addi R16, R16, DataSize      
  P0 cmp.eq R16, R15, P0, P5  
  P5 jmp Reset_Stack
  P0 ori R17, R0, 0         									//reset complete, R17 store how many highest-priority tasks
  
  //--lea R16, StackTmp-DataSize-- 
  lea R16, StackTmp
  P0 subi R16, R16, DataSize       

Start_Judge:    
  //--judge and store all high priority task index--
  P0 cmp.eq R26, R23, P3, P0
  P3 addi R17, R17, 1
  P3 addi R16, R16, DataSize  
  P3 store.8 R22, R16, 0                      //store task index to stacktmp queue
  //--
  P0 jmp Scheduler_loop

Scheduler_Exit:     
  lea R16, StackTmp
  P0 subi R16, R16, DataSize  
  P0 ori R15, R17, 0
  
RR_Execution:
  //--Round Robin execution--
  P0 addi R16, R16, DataSize
  P0 load.8 R21, R16, 0                  //R21=task index
  P0 load.8 R27, R21, 0					         //R27=task struct address
  
  P0 cmp.eq R15, R0, P6, P7              //complete one round
  P6 ori R15, R17, 0
  
  P6 loadi.z.0	R16,(StackTmp-DataSize)&0xFFFF
	P6 loadi.k.1	R16,(StackTmp-DataSize)>>16  
  P6 jmp RR_Execution
  
  P0 cmp.eq R21, R0, P4, P5              //if StackTmp[n] = zero, mean task has removed, select next
  P4 jmp RR_Execution                    
  //--  
  
  P0 addi R18, R0, MaxTask                 
  lea  R19, TaskIndexStart  

Test_Loop:                                       //測試 task 是否已從 task queue 中移除
  P0 cmp.eq R18, R0, P3, P0
  P3 store.8 R0, R16, 0                          //task index queue沒找到, 代表task has removed, reset StackTmp[n]
  P3 jmp RR_Execution
    
  P0 load.8 R22, R19, 0                          //R22 = TaskIndex所儲存的某個Task struct address
  P0 cmp.eq R27, R22, P1, P4                     //若 R21 == R22, Task尚未被remove  
  P1 jmp Test_Exit
  
  P0 subi R18, R18, 1  
  P0 addi R19, R19, DataSize                     //R19 往下一個可能的 task index尋找
  P0 jmp Test_Loop                               //重新開始loop, 找TaskIndex

Test_Exit:        
  lea R20, CurTask  
  P0 store.8 R21, R20, 0      
  P0 subi R15, R15, 1  
  
  //SaveReg R0, KernelStructStart  
  
  lea R20, KernelStructStart
  
  P0 addi R20, R20, DataSize*15
  P0 store.8 R15, R20, 0
  
  P0 addi R20, R20, DataSize   
  P0 store.8 R16, R20, 0
  
  P0 addi R20, R20, DataSize     
  P0 store.8 R17, R20, 0
  
  P0 addi R20, R20, DataSize*12    //store kernel stack pointer 
  P0 store.8 R29, R20, 0
  
  P0 addi R20, R20, DataSize       //store kernel frame pointer
  P0 store.8 R30, R20, 0
  //--   
  
  P0 jmp begintask
  P0 ret
  endp
	

		
		//org    Task3Start

IdleProcess		proc
idleInit:
  P0 addi R1, R0, 1000//4000
  P0 WriteSFR.set sfr_debugport, 4
wait:
  //--
  P0 cmpi.eq R1, 100, P2, P0
  P2 WriteSFR.set sfr_debugport, 0x100
  P0 cmpi.eq R1, 50, P2, P0
  P2 WriteSFR.set sfr_debugport, 0x50
  //--
  P0 subi R1, R1, 1    
  
  P0 cmp.ne R1, R0, P2, P0
  P2 jmp wait
  P0 WriteSFR.set sfr_debugport, 0x20
  P0 jmp idleInit

  P0  jmp		wait
		stop
		endp	

		//org		5000h
    //org    Task1Start
    
task1		proc  
  P0 WriteSFR.set sfr_debugport, 1
  P0 addi R22, R0, 500
  P0 ori R5, R0, 0
Task1_loop:   
  P0 subi R22, R22, 1
  P0 WriteSFR.set sfr_debugport, 10
  
  P0 cmp.ne R22, R0, P2, P0
  P2 jmp Task1_loop 

Task1_exit:
  P0 WriteSFR.set sfr_debugport, 7
  //P0 WriteSFR.and	sfr_intrctrl, 0xFE        //disable interrupt
  //P0 jmp.link Sys_RemoveTask  
  //P0 jmp scheduler
  P0 ret 
  
		endp		

    //org    Task2Start

task2		proc
  P0 WriteSFR.set sfr_debugport, 2
  P0 addi R22, R0, 200  
  P0 ori R5, R0, 0
Task2_loop:    
  P0 subi R22, R22, 1
  P0 WriteSFR.set sfr_debugport, 11  
  
  P0 cmp.ne R22, R0, P2, P0
  P2 jmp Task2_loop          

Task2_exit:  
  P0 WriteSFR.set sfr_debugport, 8
  //P0 WriteSFR.and	sfr_intrctrl, 0xFE        //disable interrupt
  //P0 jmp.link Sys_RemoveTask  
  //P0 jmp scheduler
  P0 ret

		endp

