;--------------------------------------------------------------------------
;
;	A Minimal Real-Time OS for AVR microprocessor
;
;
;
;    Copyright (C) 2021	Peter Schranz
;
;    This program is free software: you can redistribute it and/or modify
;    it under the terms of the GNU General Public License as published by
;    the Free Software Foundation, either version 3 of the License, or
;    (at your option) any later version.
;
;    This program is distributed in the hope that it will be useful,
;    but WITHOUT ANY WARRANTY; without even the implied warranty of
;    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;    GNU General Public License for more details.
;
;    You should have received a copy of the GNU General Public License
;    along with this program.  If not, see <http://www.gnu.org/licenses/>.;
;
;
;	2021-06-09	PS	Initial version, this version has only
;				undergone minimal testing. It was
;				created just as an experiment and is
;				partially based on other examples and
;				ideas of other system monitors. It has
;				been developed and used on a Amtega1248P
;
;--------------------------------------------------------------------------
;
;	A minimal set of routines that allow parallel jobs on a AVR
;	microcontroller. You need to provide at least the following
;	data section
;
;runjob:	.byte	2		; Initialise with 0
;curjob:	.byte	2		; Initialise with 0
;hibjob:	.byte	2		; Initialise with 0
;usersp:	.byte	2		; 
;		.byte	n
;kstack:
;intlvl:	.byte	1		; Initialise with -1 (0xFF)
;
;
;	and of course data sections for the job control blocks (see create:)
;	and the individual stacks of each process and other resources you
;	need in your application (see other routines)
;
;--------------------------------------------------------------------------
;
;	Each interrupt routine is supposed to call the interrupt
;	dispatcher when it needs a lot of stack or system resources
;
;	Z	pointer to callback routine
;		previous Z register is saved on the stack
;
;
;
;	Whenever we enter intdis from an interrupt service routine 
;	then the stack has to be setup as follows
;
;	.dseg
; SP--->
;	.byte	1	; zl
;	.byte	1	; zh
;	.byte	1	; R8
;	.byte	1	; pch	
;	.byte	1	; pcl
;
;	R8 contains the saved SREG value which must not be altered by the
;	callback routine!
;
;	The callback routine may freely use the registers r28..r31 all 
;	other registers must be saved and restored. The callback routine
;	must return to the dispatcher executing a simple ret instruction
;
intdispatch:
	push	yh
	push	yl			;;; Save minimal register
	lds	yl, intlvl		;;; Check Interrupt Level
	inc	yl
	sts	intlvl, yl		;;;
	brne	intstk			;;; oh nested interrupt! (AVR128?)
	in	yl, SPL			;;; if we hit 0 then switch stacks
	in	yh, SPH
	sts	usersp+0, yl
	sts	usersp+1, yh		;;; save current user stack pointer
	ldi	yl, low(kstack)
	ldi	yh, high(kstack)	;;; set kernal stack pointer
	out	SPL, yl
	out	SPH, yh
intstk:
	icall				;;; return to the interrupt service routine
	lds	yl, intlvl
	dec	yl
	sts	intlvl, yl
	brmi	syssws			;;; if we are at -1 again then switch stacks

sysext:					;;; else nested interrup
	pop	yl			;;; need to unwind first
	pop	yh
	pop	zl
	pop	zh
	out	SREG, r8		;;; Restore processor status
	pop	r8
	reti

syssws:
	lds	yl, usersp+0		;;; back to user stack pointer
	lds	yh, usersp+1
	out	SPL, yl
	out	SPH, yh

;
;	Whenever we enter sysret then the stack looks as follows
;
;	.dseg
; SP--->
;	.byte	1	; yl
;	.byte	1	; yh
;	.byte	1	; zl
;	.byte	1	; zh
;	.byte	1	; R8
;	.byte	1	; pch	
;	.byte	1	; pcl
;
;	R8 contains the save SREG value
;
sysret:
	lds	zl, curjob+0		;;; Z = curjob
	lds	zh, curjob+1
	lds	yl, runjob+0		;;; Y = runjob
	lds	yh, runjob+1
	cp	zl, yl
	cpc	zh, yh
	breq	sysext			;;; no context switch
	cp	zl, zero		;;; Test curjob
	cpc	zh, zero		;;; no current job
	breq	sysrun			;;;
	push	xh			;;; save context of current job
	push	xl			;;; 
	push	r25			;;; 
	push	r24			;;; 
	push	r23
	push	r22
	push	r21
	push	r20
	push	r19
	push	r18
	push	r17
	push	r16 
	push	r15 
	push	r14 
	push	r13 
	push	r12 
	push	r11 
	push	r10
	push	r9
	push	r8			;;; in fact SREG
	push	r7
	push	r6
	push	r5
	push	r4
	push	r3
	push	r2
	push	r1
	push	r0			;;; 
;
;	All Register are now saved we can use any register
;
	in	r0, SPL
	in	r1, SPH
	std	Z+jcb_stack+0, r0
	std	Z+jcb_stack+1, r1	;;; Save stack pointer of current context

sysrun:
;
;	All Register will later be restored from a different context 
;
	cp	yl, zero		;;; Test runjob
	cpc	yh, zero
	breq	sysnul			;;; No next job to run
	sts	curjob+0, yl
	sts	curjob+1, yh		;;; Set next job as current job
	ldd	r0, Y+jcb_stack+0	;;;
	ldd	r1, Y+jcb_stack+1
	out	SPL, r0
	out	SPH, r1			;;; Setup stack pointer of next context
	pop	r0			;;; restore registers of next context
	pop	r1
	pop	r2
	pop	r3
	pop	r4
	pop	r5
	pop	r6
	pop	r7
	pop	r8			;;; in fact SREG
	pop	r9
	pop	r10
	pop	r11
	pop	r12
	pop	r13
	pop	r14
	pop	r15
	pop	r16
	pop	r17
	pop	r18
	pop	r19
	pop	r20
	pop	r21
	pop	r22
	pop	r23
	pop	r24
	pop	r25
	pop	xl
	pop	xh
	pop	yl
	pop	yh
	pop	zl
	pop	zh			;;; 
	out	SREG, r8		;;; Restore processor status
	pop	r8			;;; the real r8
	reti
;
;	Preparing for the NULL job
;
sysnul:
	sts	curjob+0, zero
	sts	curjob+1, zero
	ldi	zl, low(nstack)
	ldi	zh, high(nstack)
	out	SPL, zl
	out	SPH, zh
	clr	zl
	clr	zh
	sei
;
;	The famous NULL job
;
null:
;	adiw	zh:zl, 1		;;; during tests to see that it is
;	cli				;;; beeing selected
;	sts	ncount+0, zl		;;;
;	sts	ncount+1, zh
;	sei
	rjmp	null			;;; not really required

;--------------------------------------------------------------------------
;
; 1 millisecond (depends on settings) timer interrupt service routine 
;
;
tick:
	push	r8			;;; 
	in	r8, SREG		;;;
	push	zh			;;;
	push	zl			;;; Interrupt Save

	lds	zl, hibjob+0
	lds	zh, hibjob+1		;;; get job to look at
	cp	zl, zero
	cpc	zh, zero
	brne	tick100			;;; found one

	pop	zl			;;; else do a quick exit
	pop	zh
	out	SREG, r8
	pop	r8
	reti

tick100:
	push	yh
	push	yl
	push	xh
	push	xl
	ldi	yl, low(hibjob)
	ldi	yh, high(hibjob)
tick110:			
	ldd	xl, Z+jcb_joblist+0
	ldd	xh, Z+jcb_joblist+1
	sbiw	xh:xl, 1
	std	Z+jcb_joblist+0,xl
	std	Z+jcb_joblist+1,xh
	brpl	tick120
	ldd	xl, Z+0
	ldd	xh, Z+1
	std	Y+0, xl
	std	Y+1, xh
	ldi	xl, low(runjob)
	ldi	xh, high(runjob)
	std	Z+jcb_joblist+0, xl
	std	Z+jcb_Joblist+1, xh
	rcall	link			;;; our link does not change registers
	movw	zh:zl, yh:yl
tick120:
	movw	yh:yl, zh:zl
	ldd	zl, Y+0
	ldd	zh, Y+1
	cp	zl, zero
	cpc	zh, zero
	brne	tick110
	pop	xl
	pop	xh
	rjmp	sysret

;--------------------------------------------------------------------------
;
;	link a job into a list, jobs are queued with descending priority, at
;	any given time the highest priority job always is the first in a 
;	queue.
;
;	link is only used within the core OS and is not supposed to be used
;	by any job.
;
;	Z	-->	job control block
;
link:
	push	r1
	push	r0
	push	yh
	push	yl
	ldd	yl, Z+jcb_joblist+0	;;; Get job queue address
	ldd	yh, Z+jcb_joblist+1	;;;
	ldd	r1, Z+jcb_priority	;;; Get current jobs priority
	push	zh
	push	zl			;;; Save current JCB

link010:
	ldd	zl, Y+0
	ldd	zh, Y+1			;;; Get next job in queue
	cp	zl, zero
	cpc	zh, zero		;;;
	breq	link020			;;; This is the last job insert here 
	ldd	r0, Z+jcb_priority	;;; get next job's priority
	cp	r0, r1			;;; compare our job's with next job's priority
	brlo	link020			;;; our job has higher piority insert here
	movw	yh:yl, zh:zl
	rjmp	link010
	
link020:
	pop	zl
	pop	zh			;;; Resture our JCB
	ldd	r0, Y+0
	ldd	r1, Y+1			;;; Link to next JCB (or zero if end of list)
	std	Z+0, r0
	std	Z+1, r1			;;; Link to JCB to be inserted
	std	Y+0, zl
	std	Y+1, zh			;;; Insert this JCB here
	pop	yl
	pop	yh
	pop	r0
	pop	r1
	ret	

;--------------------------------------------------------------------------
;
;	Operating Core Routines
;
;	Acquires a resource. A resource is a just a word in RAM initialised
;	with 0, which means the resource is free. The first job looking for
;	a resource will set the value of the resource to 1 , which means in 
;	use. In all other cases the job will be queued to the resource and
;	the next available job will be started.
;
;	Note that the values 0 and 1 can never occur as a job control block
;	address as RAM always starts at a higher address. In fact we assume
;	that job control blocks have never an address below 0x0100.
;
;	Z	-->	list to wait on
;
acquire:
	push	r8
	in	r8, SREG
	cli				;;; No further interrupts allowed
	push	zh
	push	zl
	push	yh
	push	yl
	ldd	yl, Z+0
	ldd	yh, Z+1
	cp	yl, zero
	cpc	yh, zero
	brne	acquire010
	std	Z+0, one
	std	Z+1, zero
	pop	yl
	pop	yh
	pop	zl
	pop	zh
	out	SREG, r8
	pop	r8
	reti

acquire010:
	push	xh
	push	xl
	lds	yl, runjob+0
	lds	yh, runjob+1
	ldd	xl, Y+0
	ldd	xh, Y+1
	sts	runjob+0, xl
	sts	runjob+1, xh
	std	Y+jcb_joblist+0, zl
	std	Y+jcb_joblist+0, zh
	ldd	xl, Z+0
	ldd	xh, Z+1
	cp	zl, one
	cpc	zh, zero
	brne	acquire020
	std	Z+0, yl
	std	Z+1, yh
	std	Y+0, zero
	std	Y+1, zero
	rjmp	acquire090

acquire020:
	movw	zh:zl, yh:yl
	rcall	link
acquire090:
	pop	xl
	pop	xh
	rjmp	sysret

;
;	Release does as the name says release a resource, only the current job
;	can release a resource and we do not check whether the current job
;	was the job that acquired the resource. If a resource was acquired
;	and in the meantime other jobs asked for this resource the first
;	job will be removed from the queue and inserted into the run job queue.
;
;	Z	-->	list to wait on
;
release:
	push	r8
	in	r8, SREG
	cli
	push	zh
	push	zl
	push	yh
	push	yl
	ldd	yl, Z+0
	ldd	yh, Z+1
	cp	yl, one
	cpc	yh, zero
	brne	release010
	std	Z+0, zero
	std	Z+1, zero
	pop	yl
	pop	yh
	pop	zl
	pop	zh
	out	SREG, r8
	pop	r8
	reti

release010:
	push	xh
	push	xl
	ldd	xl, Y+0
	ldd	xh, Y+1
	std	Z+0, xl
	std	Z+1, xh
	cp	xl, zero
	cpc	xh, zero
	brne	release020
	std	Z+0, zero
	std	Z+1, zero

release020:	
	pop	xl
	pop	xh
	movw	zh:zl, yh:yl
	ldi	yl, low(runjob)
	ldi	yh, high(runjob)
	std	Z+jcb_joblist+0, yl
	std	Z+jcb_joblist+1, yh
	rcall	link
	rjmp	sysret

;
;	block a job until an event occurs. If the event already occured in the 
;	past the block will have been set to 1 in this case the event will be
;	set to 0 and the job continues.
;
;
;	Z	-->	list to block onto
;
block:
	push	r8
	in	r8, SREG
	cli
	push	zh
	push	zl
	push	yh
	push	yl
	ldd	yl, Z+0
	ldd	yh, Z+1			;;; any job to unblock
	cp	yl, one
	cpc	yh, zero		;;;	
	brne	block010		;;; yes
	std	Z+0, zero
	std	Z+1, zero		;;; indicate it is now free
	pop	yl			;;; quick exit
	pop	yh
	pop	zl
	pop	zh
	out	SREG, r8
	pop	r8
	reti

block010:
	push	xh
	push	xl
	lds	yl, runjob+0		;;; remove current job from run job
	lds	yh, runjob+1

	ldd	xl, Y+0
	ldd	xh, Y+1			;;; next job
	sts	runjob+0, xl		;;; 
	sts	runjob+1, xh		;;; will become first in runjob queue

	std	Y+jcb_joblist+0, zl	;;;
	std	Y+jcb_joblist+1, zh	;;; set the block queue address
	pop	xl
	pop	xh
	movw	zh:zl, yh:yl		;;; link this job to the block queue
	rcall	link			;;; place it into the linked list
	rjmp	sysret

;	
;	unblock signals an external event and in case a job was waiting it
;	will remove the first job form the block queue and insert it into 
;	the run job queue.
;
;	as with the acquire/release resources a block is just a word in RAM
;	initialised with 0.
;
;	Should no job wait for the event we will bump the value for 0 to 1
;	indicating that the even has already taken place. Should a job then
;	wait for this event it will just continue after setting the event form
;	1 to 0. Note that an event that can occur multiple times never increases
;	the value more than 1
;
;	Z	-->	list to unblock from
;
unblock:
	push	r8
	in	r8, SREG
	cli
	push	zh
	push	zl
	push	yh
	push	yl
	ldd	yl, Z+0
	ldd	yh, Z+1			;;; Get job to unblock
	cp	yl, zero
	cpc	yh, zero		;;; 
	breq	unblock020		;;; Nope
	cp	yl, one
	cpc	yh, zero
	breq	unblock020		;;; Nope
	push	xh
	push	xl
	ldi	xl, low(runjob)		;;; queue the job to the run queu
	ldi	xh, high(runjob)
	std	Y+jcb_joblist+0, xl
	std	Y+jcb_joblist+1, xh
	ldd	xl, Y+0
	ldd	xh, Y+1			;;; get next job in block queue
	std	Z+0, xl			;;; 
	std	Z+1, xh			;;; and set it as next in block queue
	movw	zh:zl, yh:yl		;;; set address of jcb
	pop	xl
	pop	xh
	rcall	link			;;; link it into the runjob queue
	rjmp	sysret

unblock020:
	std	Z+0, one		;;; flag a pending unblock
	std	Z+1, zero
	pop	yl
	pop	yh
	pop	zl
	pop	zh
	out	SREG, r8
	pop	r8
	reti			;;; quick exit

;
;	puts the current job into the hibernate job queue where it will wait
;	until the number of ticks have been passed. "tick" will decrement
;	the timer of all hibernated jobs and should the value drop below 0
;	the job will be put by "tick" from the hibernate queue to the run job
;	queue.
;
;	Z	=	ticks to sleep
;
_sleep:
	push	r8
	in	r8, SREG
	cli
	push	zh
	push	zl
	push	yh
	push	yl
	lds	yl, runjob+0
	lds	yh, runjob+1		;;; Get Job
	std	Y+jcb_joblist+0, zl
	std	Y+jcb_joblist+1, zh	;;; Set Ticks (reuse joblist word in JCB)
	ldd	zl, Y+0
	ldd	zh, Y+1			;;; Get Next Job
	sts	runjob+0, zl
	sts	runjob+1, zh		;;; Make it first in runjob
	lds	zl, hibjob+0
	lds	zh, hibjob+1
	std	Y+0, zl
	std	Y+1, zh
	sts	hibjob+0, yl
	sts	hibjob+1, yh		;;; Hibernate the job
	rjmp	sysret
;
;	zl = priority
;
setpriority:
	push	r8
	in	r8, SREG
	cli
	push	zh
	push	zl
	push	yh
	push	yl
	lds	yl, runjob+0
	lds	yh, runjob+1
	std	Y+jcb_priority, zl
	ldd	zl, Y+0
	ldd	zh, Y+1
	movw	zh:zl, yh:yl
	rcall	link
	rjmp	sysret
;
;	creates a job. you need a job control block setup with the values showed
;	as below. jobs will inherit the register values of the calling process.
;	Each job needs it's own stack. The context of each job is saved on the
;	stack, a context is 35 bytes (regisers r0..r31, SREG, PC)
;
;	priority is a relative value to all existing prioritys, typically you
;	use low values from 0..., 
;
;	Note the first call to create starts the mini RTOS. Further jobs must
;	be created by this first job. If you want to first create all jobs you
;	need to make sure that the first job has the highest priority because
;	whenever you create a job with a priority higher than the current job
;	the new job will be executed.
;
;
;	Z	-->	Job Control Block
;			.byte	2	; Link to next Job Control Block
;			.byte	2	; Program start -> later job list
;			.byte	2	; Stack pointer top of stack
;			.byte	1	; Priority
;			.byte	1	; Flags	
;
;	Result
;	SP	-->	.byte	stacksize-35
;			.byte	r0
;			.byte	r1
;			.
;			.
;			.byte	zh
;			.byte	r8
;			.byte	pcl
;			.byte	pch
;	
;	The context we save is a copy of the current register values
;
;	Note:	All instructions up to sbiw do not alter the status register!

create:
	push	r8			; required by sysret
	push	zh			; .
	push	zl			; .
	push	yh			; .
	push	yl			; required by sysret

	push	r3			; Save Scratchpad
	push	r2
	push	r1
	push	r0

	ldd	r0, Z+jcb_joblist+0
	ldd	r1, Z+jcb_joblist+1	; Program start to scratchpad
	movw	r3:r2, yh:yl		; copy Y to scratchpad

	ldd	yl, Z+jcb_stack+0
	ldd	yh, Z+jcb_stack+1	; Get user stack 
;
;	Program start on stack first push low-byte and then push high-byte
;	Note:	ret, reti and pop use pre-increment 
;		call push use post-decrement
;		index registers use pre-decrement and post-increment
;		therfore the first item is not "pushed" and at the end
;		we decrement the index register
	st	 Y, r0			; Program Counter aka Start address
	st	-Y, r1			; 
	st	-Y, r8			; place after PC
	st	-Y, zh			; 
	st	-Y, zl			; 
	st	-Y, r3			; The copy of the original Y register 
	st	-Y, r2			;
	st	-Y, xh
	st	-Y, xl
	st	-Y, r25
	st	-Y, r24
	st	-Y, r23
	st	-Y, r22
	st	-Y, r21
	st	-Y, r20
	st	-Y, r19
	st	-Y, r18
	st	-Y, r17
	st	-Y, r16
	st	-Y, r15
	st	-Y, r14
	st	-Y, r13
	st	-Y, r12
	st	-Y, r11
	st	-Y, r10
	st	-Y, r9
	in	r8, SREG		; r8 has been saved after PC
	st	-Y, r8			; 
	st	-Y, r7
	st	-Y, r6
	st	-Y, r5
	st	-Y, r4
	
	pop	r0
	pop	r1
	pop	r2
	pop	r3			; Restore scratchpad
	
	st	-Y, r3
	st	-Y, r2
	st	-Y, r1
	st	-Y, r0
	sbiw	yh:yl, 1

	std	Z+jcb_stack+0, yl
	std	Z+jcb_stack+1, yh	
	
	ldi	temp, low(runjob)
	std	Z+jcb_joblist+0, temp
	ldi	temp, high(runjob)
	std	Z+jcb_joblist+1, temp
	rcall	link
	rjmp	sysret