Форум РадиоКот

Код: Выделить всё

; Copyright (C) 2011, Shestakov Evgeniy <mazur1978(at)mail.ru>

;===================
#define MAX_TASKS	3

Table_Tasks:
.dw		DRV_LCD
.dw		Proc_Menu
.dw		KBD_DRV
end_array
;===================

;===================
#define CONTEXT_TASK_SIZE 33
;===================

.dseg

;===================
QUANT_TASKS: // Количество задач.
.byte	1

CNT_TASKS: // Номер задачи.
.byte	1

REPLACE_STACK:
.byte	3

MAIN_SPH:
.byte	1
MAIN_SPL:
.byte	1

NUL_BUFFER_1:
.byte	1

#define HEADERS_TASKS_SIZE 8

HEADERS_TASKS:
.byte	HEADERS_TASKS_SIZE*MAX_TASKS

#define TASK_STACK_SIZE 64

STACKS_TASKS: ; Блок стеков задач(Стек вдвигается вверх)
.byte	TASK_STACK_SIZE*MAX_TASKS

.cseg

//************************************************************************
Set_Tasks:
	clr		CNT
//	ldx		STACKS_TASKS+(TASK_STACK_SIZE-(CONTEXT_TASK_SIZE+7))
//	ldy		HEADERS_TASKS
	ldz		Table_Tasks*2

Set_Tasks_Cycle:
; Адрес входа в задачу.
; Указатель на таблицу задач.
; Смещение указателя по номеру задачи
	lpm		r16, Z+
	lpm		r17, Z+
	cp		r16, RSER
	cpc		r17, RSER
	breq	Set_Tasks_End_1

; Сохранение указателя стека (сохранение регистров и SREG).
//	st		Y, XH
//	std		Y+1, XL

	inc		CNT
//	subi	XL, LOW (-TASK_STACK_SIZE)
//	sbci	XH, HIGH (-TASK_STACK_SIZE)
//	adiw	YL, HEADERS_TASKS_SIZE
	rjmp	Set_Tasks_Cycle
Set_Tasks_End_1:
	cpi		CNT, 0
	breq	Queue_Tasks_Empty
	sts		QUANT_TASKS, CNT
	clr		CNT
	rjmp	Load_Task

Queue_Tasks_Empty:
	rjmp	Main
//==================

//==================
Scheduler:
// Сохранение регистров и SREG.
// Смещение указателя стека. Уменьшение значения
// на выход (ret) из Scheduler и из задачи.
// Восстановление регистров и SREG.
	cli
	wdr
	sts		REPLACE_STACK, XL
	in		XL, SREG
	sts		REPLACE_STACK+1, XL
	sts		REPLACE_STACK+2, XH
	in		XL, SPL
	in		XH, SPH
	sbiw	XL, 4 ; Scheduler+Task
	out		SPH, XH
	out		SPL, XL
	lds		XH, REPLACE_STACK+2
	lds		XL, REPLACE_STACK+1
	out		SREG, XL
	lds		XL, REPLACE_STACK

; Сохранение контекста задачи. Регистров и SREG.
	PushRegs

; Вычисление по номеру задачи указателя заголовков задач.
	lds		r18, QUANT_TASKS
	lds		CNT, CNT_TASKS

; Переключение задачи.
	inc		CNT
	cp		CNT, r18
	brlo	Scheduler_A
	clr		CNT
Scheduler_A:
	sts		CNT_TASKS, CNT

Load_Task:
; Восстановление точки входа в задачу.
; Указатель на таблицу задач.
; Смещение указателя по номеру задачи
	clr		ZH
	mov		ZL, CNT
	lsl		ZL
	subi	ZL, LOW(-(Table_Tasks*2))
	sbci	ZH, HIGH(-(Table_Tasks*2))

// Умножение номера задачи на объем стека.
// Смещение указателя на конец стека текущей задачи.
	ldi		r16, TASK_STACK_SIZE
	mul		CNT, r16
	movw	XH:XL, r1:r0
	subi	XL, LOW(-(STACKS_TASKS+(TASK_STACK_SIZE-1)))
	sbci	XH, HIGH(-(STACKS_TASKS+(TASK_STACK_SIZE-1)))

; Указатель стека на конец стека текущей задачи.
	out		SPH, XH
	out		SPL, XL

	ldi		r16, LOW(Scheduler)
	push	r16
	ldi		r16, HIGH(Scheduler)
	push	r16

	lpm		r16, Z+
	lpm		r17, Z+
	push	r16
	push	r17

	in		XL, SPL
	in		XH, SPH

// Смещение указателя на кол-во регистров+SREG.
	sbiw	XL, CONTEXT_TASK_SIZE

	out		SPH, XH
	out		SPL, XL

; Восстановление контекста следующей задачи и выход
; (в стеке адрес точки входа в задачу).
	PopRegs
	reti
/************************************************************************/