////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		
KernelInitial proc
// simulator initial

  ResetReg

  //P0  ori R29, R0, StackTmp5+DataSize
  P0  ori R29, R0, StackStart
  P0  store.8 R0, R30, CurTask            //current task = NULL
   
  //initial kernel context
//  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
  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
  P0 loadi.z.0 R1, IdleProcess
//P0 loadi.z.0 R1, 4000h
  P0 ori R2, R0, 1
  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 WriteSFR.set	sfr_intren,0x04  //enable interrupt 0x12
  P0 STI			               //enable interrupts  
  P0 ret
  endp
  

intr_T0		proc
		StopT0				
		WriteSFR.set	sfr_debugport,0x1000		
	//--
	  push R19	  
	  
	  P0 addi R19, R0, 1 	  
	  P0 store.8 R19, R0, KernelStructStart+DataSize*33
	  
	  pop R19
		jmp scheduler		
  //--
		reti
		endp	

intr_T1		proc
		WriteSFR.set	sfr_debugport,0x1100
		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  
  
//initial Task Struct
  P0  ori R22, R0, TaskIndexStart-DataSize           //tast index 指向 task structure 位址, R23 的位址
  P0  ori R23, R0, 0                                 //kernel 中 task stack 的第幾個位址
  P0  addi 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
  P0  store.8   R2, R24,0             //R2 = task priority
  P0  store.8   R1, R24, DataSize*31  //R31 = R1 = task address

InsertExit:
  P0 jmp.reg R31
  endp
  
	
Sys_RemoveTask	proc		                     //remove current task & move to next task  
  StopT0    
  push R27
  P0 load.8 R27, R0, CurTask                        //R27 指向目前將被移除的 task index  
  P0 store.8 R0, R0, CurTask                        //將目前 task 移除
  P0 store.8 R0, R27, 0
  pop R27

Remove_Exit:  
  P0 jmp.reg R31
  endp  

/*begintask proc
  P0 jmp.reg.link R31           //call task subroutine
  P0 trap RemoveTask
  endp
*/

ContextSaving	proc
  push R22
  push R23
 
  P0 load.8 R22, R0, CurTask
  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 R30, R23, DataSize*30
  
  //--
  pop R20      
  pop R21      
  pop R30      
  
  P0 store.8 R30, R23, DataSize*31
  P0 store.8 R21, R23, DataSize*22
  P0 store.8 R20, R23, DataSize*23
  P0 load.8 R0, R0, CurTask    // 清掉 running task
  
  push R30
  push R21
  push R20
  //--
NonContextSaving:  
  
  pop R23
  pop R22  
  P0 ret
	endp								

scheduler	proc
  push R19
  //--judge flag--
  P0 load.8 R19, R0, KernelStructStart+DataSize*33
  P0 cmpi.eq R19, 1, P3, P0
  //--    
  P3 ReadSFR R31, sfr_IRA                //store interrupt return addr to R31(old task)    
  pop R19
  
  push R31
  P0 jmp.link ContextSaving              //context saveing, store and restore R31  
  pop R31       
  LoadKernelData    

  //--judge flag--
  P0 load.8 R19, R0, KernelStructStart+DataSize*33
  P0 cmpi.eq R19, 1, P6, P7
  P6 ori R19, R0, 0              
  P6 store.8 R19, R0, KernelStructStart+DataSize*33        //reset flag
  P6 jmp RR_Execution  
  //--
    
  P0 ori R21, R0, 0                            //R21 初始值為 0
  P0 addi R22, R21, TaskIndexStart-DataSize    //指向 task index 的前一個位址, 方便在 loop 中先加上 DataSize
  P0 ori R23, R0, 0                            //R23 表示目前最大的 priority number
  P0 addi 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

  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--  
  P0 addi R16, R0, StackTmp
  P0 addi R15, R0, StackStart         //StackTmp+DataSize*5
Reset_Stack:  
  P0 store.8 R0, R16, 0
  P0 addi R16, R16, DataSize  
  P0 cmp.eq R16, R15, P6, P5  
  P5 jmp Reset_Stack
  P6 ori R17, R0, 0         									//reset complete, R17 store how many highest-priority tasks
  P6 addi R16, R0, StackTmp-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:   
  P0 addi R16, R0, StackTmp-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 addi R16, R0, StackTmp-DataSize
  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               
  P0 addi R19, R0, 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:    
  P0 store.8 R21, R0, CurTask  
  P0 subi R15, R15, 1  
  //--

  SaveReg R0, KernelStructStart
  
  LoadTaskData

  ResetClock
  STI//P0 WriteSFR.or sfr_intrctrl,0x01       			//STI, enable interrupt        
  StartT0 
  P0 ret
  endp
	
intr_Key	proc
  StopT0  
  ReadSFR R31, sfr_IRA    
  
  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 = 5
  P7  loadi.z.0	R1,task1
  P7  addi R2, R0, 5
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit
  
  P0  cmpi.eq  R31, 98, P6, P0 // press b, add task2, priority = 5
  P6  loadi.z.0	R1,task2
  P6  addi R2, R0, 5
  P6  jmp.link Sys_InsertTask
  P6  jmp Key_exit
  
  P0  cmpi.eq  R31, 99, P7, P0 // press c, add task1, priority = 4
  P7  loadi.z.0	R1,task1
  P7  addi R2, R0, 4
  P7  jmp.link Sys_InsertTask
  P7  jmp Key_exit
  
  P0  cmpi.eq  R31, 100, P6, P0 // press d, add task1, priority = 2
  P6  loadi.z.0	R1,task2
  P6  addi R2, R0, 2
  P6  jmp.link Sys_InsertTask
  P6  jmp Key_exit
  
Key_exit:
  pop R2
  pop R1
  pop R31
  P0 jmp scheduler     

  P0  reti
		endp	


		org		TaskCodeStart

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

task1		proc
  P0 WriteSFR.set sfr_debugport, 1
  /*  
  P0 addi R6, R0, -1
  P0 ori R7, R0, 2000
  
  P0 pmul.even R8, R6, R7
  cmp.ne R8, R0, P1, P2
  */
  P0 addi R22, R0, 2000  
  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
  
		endp		

task2		proc  
  P0 WriteSFR.set sfr_debugport, 2

  P0 addi R22, R0, 1000
  //P0 addi R21, R0, 1000  
  P0 ori R5, R0, 0
Task2_loop:  
  /*P0 pcmp.8.eq R5, R21, R22
  P0 cmp.ne R5, R0, P2, P0
  P2 WriteSFR.set sfr_debugport, 6  
  */
  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

		endp
