1     #asm
2     .cseg
3     .equ oldpc=pc
4     .org 0x3400
5     #endasm
6     os_thread* CreateThread(void (*entrypoint)(void), UCHAR priVal, UINT swStackSize, UINT hwStackSize)
7     {
8         os_thread* temp_thread;
9         register char i;
10
11        temp_thread = (os_thread*)g_freethreads;
12
13        if (temp_thread == NULL) return -1;
14
15        // pop a free thread from the list
16        g_freethreads = temp_thread->next;
17        g_freethreads->prev = NULL;
18
19        // push the free thread on the used threads list
20        temp_thread->next = g_usedthreads;
21        g_usedthreads->prev = temp_thread;
22        g_usedthreads = temp_thread;
23
24        // apply thread settings
25        g_usedthreads->priority = priVal;
26        g_usedthreads->state = THREADSTATE_RUNNING;
27        g_usedthreads->sleep_duration = 0;
28
29        // Allocate stacks for this thread
30        g_usedthreads->hwstack = os_alloc_stack(hwStackSize);
31        g_usedthreads->swstack = os_alloc_stack(swStackSize);
32
33        // we need to do some stack stuff, like put in 0's for registers and the stack address
34        *((UCHAR*)g_usedthreads->hwstack)-- = (UCHAR)(entrypoint >> 0);
35        *((UCHAR*)g_usedthreads->hwstack)-- = (UCHAR)(entrypoint >> 8);
36        for (i = 0; i < 30; i++)
37        {
38            *((UCHAR*)g_usedthreads->hwstack)-- = 0;
39        }
40        // put on SREG with interrupts enabled
41        *((UCHAR*)g_usedthreads->hwstack)-- = 0x80;
42
43        g_threadcount++;
44
45        return g_usedthreads;
46    }
47    void SleepThread(os_thread* thread, UINT sleepLength)
48    {
49        #asm("cli");
50        if (thread != NULL)
51        {
52            thread->sleep_duration = sleepLength;
53            // re-run the scheduler
54            os_schedule(0);
55        }
56        #asm("sei");
57    }
58    void SuspendThread(os_thread* thread)
59    {
60        #asm("cli");
61        if (thread != NULL)
62        {
63            thread->state = THREADSTATE_SUSPENDED;
64            // re-run the scheduler
65            //os_schedule(0);
66        }
67        #asm("sei");
68    }
69    void ResumeThread(os_thread* thread)
70    {
71        #asm("cli");
72        if (thread != NULL)
73        {
74            thread->state = THREADSTATE_RUNNING;
75            // re-run the scheduler
76            //os_schedule(1);
77        }
78        #asm("sei");
79    }
80    void TerminateThread(os_thread* thread)
81    {
82        #asm("cli");
83        if (thread != NULL)
84        {
85            thread->state = THREADSTATE_VOID;
86            // remove this thread from the list
87            if (thread->prev == NULL)
88            {
89                g_usedthreads = thread->next;
90                g_freethreads->prev = thread;
91                thread->next = g_freethreads;
92                g_freethreads = thread;
93            }
94            else
95            {
96                if (thread->prev != NULL)
97                        thread->prev->next = thread->next;
98
99                if (thread->next != NULL)
100                       thread->next->prev = thread->prev;
101
102               thread->prev = NULL;
103               thread->next = g_freethreads;
104               g_freethreads = thread;
105           }
106           g_threadcount--;
107           // re-run the scheduler
108           os_schedule(0);
109       }
110       #asm("sei");
111   }
112
113   os_thread* FindThread(UCHAR threadID)
114   {
115       os_thread *temp_thread, *found_thread;
116
117       temp_thread = g_usedthreads;
118       found_thread = NULL;
119       while (temp_thread != NULL)
120       {
121           if (temp_thread->threadID == threadID)
122           {
123               found_thread = temp_thread;
124               break;
125           }
126           temp_thread = g_usedthreads->next;
127       }
128       return found_thread;
129   }
130   #asm
131   .org oldpc
132   #endasm