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