$include (C8051F020.inc)

extrn code(scheduler)
public context_init, taskdesc, current_task, yield;
;
; Configuration
;
num_taskdesc   equ 4     ; maximum number of tasks
timer_period   equ 18432 ; 10ms time slices with 22MHz crystal

;-----------------------------------------------------------------------------
;
; task descriptors are stored in xdata at tascdesc, taskdesc+taskdesc_size,
; taskdesc+2*taskdesc_size... offsets of individual fields are given below
;
t_delay        equ  0  ; 2 bytes, integer delay time
t_reg          equ  2  ; 9 bytes, r0-r7, B
t_stat         equ 11  ; 4 bytes, acc, psw, dph, dpl
t_pc           equ 15  ; 2 bytes, program counter
t_sp           equ 17  ; 1 byte, stack pointer.

taskdesc_size  equ 18


;-----------------------------------------------------------------------------
;
; allocate task descriptors. This MUST NOT cross page boundaries so that dec dpl
; will decrement the data pointer properly.
;
taskdesc_seg segment xdata page
  rseg taskdesc_seg

taskdesc: ds num_taskdesc * taskdesc_size ; less than or equal to 256


;-----------------------------------------------------------------------------
;
; allocate variables for timer 2
;
t2_data_seg segment data
  rseg t2_data_seg

current_task: ds 2 ; // pointer to the current_task



;-----------------------------------------------------------------------------
;
; Timer 2 interrupt vector
;
  cseg at 2BH
	push dpl
	push dph
	jmp timer2_int

;
; Timer 2 ISR
;
t2_code_seg segment code
  rseg t2_code_seg

timer2_int:
	push psw
	push acc
	anl T2CON, #3FH	; clear interrupt flags
	mov  dph,current_task+0 ; big endian
	mov  dpl,current_task+1
	inc  dptr
	inc  dptr         ; skip over delay
	mov  a,r0
	movx @dptr,a      ; save R0
	inc  dptr
	mov  a,r1
	movx @dptr,a      ; save R1
	inc  dptr
	mov  a,r2
	movx @dptr,a      ; save R2
	inc  dptr
	mov  a,r3
	movx @dptr,a      ; save R3
	inc  dptr
	mov  a,r4
	movx @dptr,a      ; save R4
	inc  dptr
	mov  a,r5
	movx @dptr,a      ; save R5
	inc  dptr
	mov  a,r6
	movx @dptr,a      ; save R6
	inc  dptr
	mov  a,r7
	movx @dptr,a      ; save R7
	inc  dptr
	mov  a,b
	movx @dptr,a      ; save B
	inc  dptr
	pop  acc
	movx @dptr,a      ; save ACC
	inc  dptr
	pop  acc
	movx @dptr,a      ; save PSW
	inc  dptr
	pop  acc
	movx @dptr,a      ; save DPH
	inc  dptr
	pop  acc
	movx @dptr,a      ; save DPL
	inc  dptr
	pop  acc
	movx @dptr,a      ; save PC (high)
	inc  dptr
	pop  acc
	movx @dptr,a      ; save PC (low)
	inc  dptr
	mov  a,sp
	movx @dptr,a      ; save stack pointer

	lcall scheduler   ; update current_task

	mov  dph,current_task+0 ; big endian
	mov  dpl,current_task+1
	inc	 dptr
	inc  dptr
	movx a,@dptr      ; restore r0
	mov  r0,a
	inc  dptr
	movx a,@dptr      ; restore r1
	mov  r1,a
	inc  dptr
	movx a,@dptr      ; restore r2
	mov  r2,a
	inc  dptr
	movx a,@dptr      ; restore r3
	mov  r3,a
	inc  dptr
	movx a,@dptr      ; restore r4
	mov  r4,a
	inc  dptr
	movx a,@dptr      ; restore r5
	mov  r5,a
	inc  dptr
	movx a,@dptr      ; restore r6
	mov  r6,a
	inc  dptr
	movx a,@dptr      ; restore r7
	mov  r7,a
	inc  dptr
	movx a,@dptr      ; restore B
	mov  b,a
	mov  a,current_task+1
	add  a,#t_sp      ; point to sp (end of status regs)
	mov  dpl,a
	movx a,@dptr      ; restore SP
	mov  sp,a
	dec  dpl
	movx a,@dptr
	push acc          ; push pc (low) onto stack
	dec  dpl
	movx a,@dptr
	push acc          ; push pc (high) onto stack
	dec  dpl
	movx a,@dptr
	push acc          ; push dp (low) onto stack
	dec  dpl
	movx a,@dptr
	push acc          ; push dp (high) onto stack
	dec  dpl
	movx a,@dptr
	mov  psw,a        ; restore psw
	dec  dpl
	movx a,@dptr      ; restore acc
	pop  dph
	pop  dpl
	reti

yield:
	setb exf2         ; trigger a timer 2 interrupt
	ret

context_init:
	mov   RCAP2H,#high(~timer_period)
	mov   RCAP2L,#low(~timer_period)
	mov   T2CON,#4     ; start timer
	mov   current_task+0,#high(taskdesc)
	mov   current_task+1,#low(taskdesc)
	orl   IE,#020H     ; enable timer 2 interrupt
	ret

end