;*****************************************************************************************
;	Mega 128 Scope Controller Program
;	(c) Martin Cibulski
;*****************************************************************************************

;*****************************************************************************************
;	Simple Multitasking

;*****************************************************************************************
.DSEG

.EQU	MT_EXISTS	= 0
.EQU	MT_ACTIVE	= 1
.EQU	MT_SP_LOW	= 2
.EQU	MT_SP_HIGH	= 3
.EQU	MT_TASK_SIZE	= 4

.EQU	MT_TASKS	= 4

mt_irq_level:	.byte	1
mt_cur_task:	.byte	1
mt_task_blocks:	.byte	MT_TASK_SIZE * MT_TASKS

;*****************************************************************************************
;	Init task table
;	All tasks non existent
.CSEG
mt_init:
	ldi	r16	,0
	sts	mt_irq_level	,r16

	ldi	r16	,0
	sts	mt_cur_task	,r16

	ldi	yl	,low(mt_task_blocks)
	ldi	yh	,high(mt_task_blocks)

	ldi	r16	,MT_TASKS

mt_init_loop:
	ldi	r17	,0
	std	Y+MT_EXISTS	,r17
	std	Y+MT_ACTIVE	,r17
	std	Y+MT_SP_LOW	,r17
	std	Y+MT_SP_HIGH	,r17

	adiw	yh:yl	,MT_TASK_SIZE
	dec	r16
	brne	mt_init_loop

	ret

;*****************************************************************************************
;	Create new task
;r16/r17	program counter
;r18/r19	top of stack + 1
;r20	task number
mt_create_task:
	in	r0	,sreg
	push	r0
	push	xl
	push	xh
	push	yl
	push	yh

	ldi	yl	,low(mt_task_blocks)	;calc adress of task block
	ldi	yh	,high(mt_task_blocks)
	ldi	r21	,MT_TASK_SIZE
	mul	r20	,r21
	add	yl	,r0
	add	yh	,r1

	mov	xl	,r18		;Stack
	mov	xh	,r19

	st	-X	,r17		;program counter
	st	-X	,r16

	ldi	r17	,0x80		;status register I flag set	
	clr	r16

	st	-X	,r16		;r16
	st	-X	,r17		;SREG
	st	-X	,r16		;r17
	st	-X	,r16		;r18
	st	-X	,r16		;r19
	st	-X	,r16		;r20
	st	-X	,r16		;r21
	st	-X	,r16		;r22
	st	-X	,r16		;r23
	st	-X	,r16		;r24
	st	-X	,r16		;r25
	st	-X	,r16		;r26
	st	-X	,r16		;r27
	st	-X	,r16		;r28
	st	-X	,r16		;r29
	st	-X	,r16		;r30
	st	-X	,r16		;r31
	st	-X	,r16		;r0
	st	-X	,r16		;r1
	st	-X	,r16		;r2
	st	-X	,r16		;r3
	st	-X	,r16		;r4
	st	-X	,r16		;r5
	st	-X	,r16		;r6
	st	-X	,r16		;r7
	st	-X	,r16		;r8
	st	-X	,r16		;r9
	st	-X	,r16		;r10
	st	-X	,r16		;r11
	st	-X	,r16		;r12
	st	-X	,r16		;r13
	st	-X	,r16		;r14
	st	-X	,r16		;r15

	cli

	std	Y+MT_SP_LOW	,xl		;???
	std	Y+MT_SP_HIGH	,xh
	ldi	r16	,1
	std	Y+MT_EXISTS	,r16
	ldi	r16	,0
	std	Y+MT_ACTIVE	,r16


	pop	yh
	pop	yl
	pop	xh
	pop	xl
	pop	r0
	out	sreg	,r0
	reti

;*****************************************************************************************

mt_enter_interrupt:
	lds	r16	,mt_irq_level		;increment IRQ level
	inc	r16
	sts	mt_irq_level	,r16

	sei				;enable nested interrupts

	pop	r16			;return adress
	pop	r17

	push	r18			;save all registers
	push	r19
	push	r20
	push	r21
	push	r22
	push	r23
	push	r24
	push	r25
	push	r26
	push	r27
	push	r28
	push	r29
	push	r30
	push	r31
	push	r0			;save all registers
	push	r1
	push	r2
	push	r3
	push	r4
	push	r5
	push	r6
	push	r7
	push	r8
	push	r9
	push	r10
	push	r11
	push	r12
	push	r13
	push	r14
	push	r15

	push	r17			;return adress
	push	r16

	ret

;*****************************************************************************************
;	Task Scheduler
;	Called before return from interrupt
mt_leave_interrupt:
	lds	r16	,mt_irq_level		;check interrupt level
	cpi	r16	,1		;1 = not nested
	brne	mt_leave_interrupt_end		;nested, no task switching

mt_leave_interrupt_end:
	pop	r15
	pop	r14
	pop	r13
	pop	r12
	pop	r11
	pop	r10
	pop	r9
	pop	r8
	pop	r7
	pop	r6
	pop	r5
	pop	r4
	pop	r3
	pop	r2
	pop	r1
	pop	r0
	pop	r31
	pop	r30
	pop	r29
	pop	r28
	pop	r27
	pop	r26
	pop	r25
	pop	r24
	pop	r23
	pop	r22
	pop	r21
	pop	r20
	pop	r19
	pop	r18
	pop	r17

	lds	r16	,mt_irq_level
	dec	r16
	cli
	sts	mt_irq_level	,r16
	pop	r16
	out	SREG	,r16
	pop	r16

	reti

;*****************************************************************************************