moss/task.c at master · Xeom/moss · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include "task.h"

#include <avr/interrupt.h>

#include "conf.h"
#include "mem.h"

typedef enum
{
    f_alive = 0x01,
    f_running = 0x02
} os_task_flags;

#define OS_ERR(a, b)

typedef struct os_task_s os_task;

struct os_task_s
{
    uint8_t flags;
    void *mem;
    uint8_t *sptr;
    void *param;
};

int8_t os_task_curr = 0;

os_task os_tasks[OS_NUM_TASKS] = { [0] = { .flags = f_alive } };

void os_yield(void)
{
    static volatile int8_t prev, new;
    void *sptr;

    cli();

    prev = os_task_curr;
    new  = prev;

    do
    {
        new += 1;
        if (new == OS_NUM_TASKS) new = 0;
    } while ((os_tasks[new].flags & f_alive) == 0);

    os_task_curr = new;

    asm volatile(
        "push r2 \n push r3 \n push r4 \n push r5 \n"
        "push r6 \n push r7 \n push r8 \n push r9 \n"
        "push r10\n push r11\n push r12\n push r13\n"
        "push r14\n push r15\n push r16\n push r17\n"

        "push r28\n push r29\n"

        "in r18, __SP_L__\n"
        "in r19, __SP_H__\n"

        "out __SP_L__, %A1\n"
        "out __SP_H__, %B1\n"

        "pop r29\n pop r28\n"

        "pop r17\n pop r16\n pop r15\n pop r14\n"
        "pop r13\n pop r12\n pop r11\n pop r10\n"
        "pop r9 \n pop r8 \n pop r7 \n pop r6 \n"
        "pop r5 \n pop r4 \n pop r3 \n pop r2 \n"

        "mov %A0, r18\n"
        "mov %B0, r19\n"
        : "=&r" (sptr)
        : "r"  (os_tasks[new].sptr)
        : "r18", "r19"
    );

    os_tasks[prev].sptr = sptr;

    sei();

    if (!(os_tasks[new].flags & f_running))
    {
        os_tasks[new].flags |= f_running;

        asm volatile(
            "mov r24, %A0\n"
            "mov r25, %B0\n"
            :
            : "r" (os_tasks[new].param)
            :
        );
    }
}

static int8_t os_alloc_taskn(void)
{
    int8_t rtn;

    for (rtn = 0; rtn < OS_NUM_TASKS; rtn++)
    {
        if (!(os_tasks[rtn].flags & f_alive))
            return rtn;
    }

    OS_ERR(tsk_mem, "alloc_taskn: too many tasks");

    return -1;
}

int8_t os_new_task(void (*fptr)(void *), size_t size, void *param)
{
    os_task *tsk;
    int8_t   tskn;
    void    *stack;

    tskn = os_alloc_taskn();

    if (tskn == -1) return -1;

    tsk = &os_tasks[tskn];

    stack = os_alloc_task(size, tskn);

    tsk->flags  = f_alive;
    tsk->mem    = stack;
    tsk->param  = param;

    /* Reset stack pointer */
    tsk->sptr = tsk->mem + size - 1;

    /* Put function return addr */
    *(tsk->sptr--) = 0xff &  (int16_t)(fptr);
    *(tsk->sptr--) = 0xff & ((int16_t)(fptr) >> 8);

    /* Decrement for register space */
    tsk->sptr -= 18;

    return 0;
}

int8_t os_kill_task(int8_t taskn)
{
    os_task *tsk;

    if (taskn < 0 || taskn >= OS_NUM_TASKS)
        return -1;

    tsk = &os_tasks[taskn];

    tsk->flags &= ~f_alive;

    os_free_tsk(taskn);

    if (taskn == os_task_curr)
        os_yield();

    return 0;
}