diff options
-rw-r--r-- | firmware/thread.c | 48 | ||||
-rw-r--r-- | firmware/thread.h | 4 |
2 files changed, 47 insertions, 5 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index d1cd8da6ef..a46351cb1f 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -17,6 +17,7 @@ | |||
17 | * | 17 | * |
18 | ****************************************************************************/ | 18 | ****************************************************************************/ |
19 | #include "thread.h" | 19 | #include "thread.h" |
20 | #include "panic.h" | ||
20 | 21 | ||
21 | struct regs | 22 | struct regs |
22 | { | 23 | { |
@@ -28,9 +29,16 @@ struct regs | |||
28 | void* pr; /* Procedure register */ | 29 | void* pr; /* Procedure register */ |
29 | }; | 30 | }; |
30 | 31 | ||
31 | static int num_threads; | 32 | int num_threads; |
32 | static int current_thread; | 33 | static int current_thread; |
33 | static struct regs thread_contexts[MAXTHREADS]; | 34 | static struct regs thread_contexts[MAXTHREADS]; |
35 | char *thread_name[MAXTHREADS]; | ||
36 | void *thread_stack[MAXTHREADS]; | ||
37 | int thread_stack_size[MAXTHREADS]; | ||
38 | static char main_thread_name[] = "main"; | ||
39 | |||
40 | extern int stackbegin[]; | ||
41 | extern int stackend[]; | ||
34 | 42 | ||
35 | /*--------------------------------------------------------------------------- | 43 | /*--------------------------------------------------------------------------- |
36 | * Store non-volatile context. | 44 | * Store non-volatile context. |
@@ -83,6 +91,7 @@ void switch_thread(void) | |||
83 | { | 91 | { |
84 | int current; | 92 | int current; |
85 | int next; | 93 | int next; |
94 | unsigned int *stackptr; | ||
86 | 95 | ||
87 | next = current = current_thread; | 96 | next = current = current_thread; |
88 | if (++next >= num_threads) | 97 | if (++next >= num_threads) |
@@ -90,6 +99,11 @@ void switch_thread(void) | |||
90 | current_thread = next; | 99 | current_thread = next; |
91 | store_context(&thread_contexts[current]); | 100 | store_context(&thread_contexts[current]); |
92 | load_context(&thread_contexts[next]); | 101 | load_context(&thread_contexts[next]); |
102 | |||
103 | stackptr = thread_stack[next]; | ||
104 | |||
105 | if(stackptr[0] != 0xdeadbeef) | ||
106 | panicf("Stkov %s", thread_name[next]); | ||
93 | } | 107 | } |
94 | 108 | ||
95 | /*--------------------------------------------------------------------------- | 109 | /*--------------------------------------------------------------------------- |
@@ -97,11 +111,12 @@ void switch_thread(void) | |||
97 | * Return 0 if context area could be allocated, else -1. | 111 | * Return 0 if context area could be allocated, else -1. |
98 | *--------------------------------------------------------------------------- | 112 | *--------------------------------------------------------------------------- |
99 | */ | 113 | */ |
100 | int create_thread(void* function, void* stack, int stack_size) | 114 | int create_thread(void* function, void* stack, int stack_size, char *name) |
101 | { | 115 | { |
102 | unsigned int i; | 116 | unsigned int i; |
103 | unsigned int stacklen; | 117 | unsigned int stacklen; |
104 | unsigned int *stackptr; | 118 | unsigned int *stackptr; |
119 | struct regs *regs; | ||
105 | 120 | ||
106 | if (num_threads >= MAXTHREADS) | 121 | if (num_threads >= MAXTHREADS) |
107 | return -1; | 122 | return -1; |
@@ -115,9 +130,13 @@ int create_thread(void* function, void* stack, int stack_size) | |||
115 | stackptr[i] = 0xdeadbeef; | 130 | stackptr[i] = 0xdeadbeef; |
116 | } | 131 | } |
117 | 132 | ||
118 | struct regs* regs = &thread_contexts[num_threads++]; | 133 | /* Store interesting information */ |
134 | thread_name[num_threads] = name; | ||
135 | thread_stack[num_threads] = stack; | ||
136 | thread_stack_size[num_threads] = stack_size; | ||
137 | regs = &thread_contexts[num_threads++]; | ||
119 | store_context(regs); | 138 | store_context(regs); |
120 | /* Subtract 4 to leave room for the PR push in ldctx() | 139 | /* Subtract 4 to leave room for the PR push in load_context() |
121 | Align it on an even 32 bit boundary */ | 140 | Align it on an even 32 bit boundary */ |
122 | regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3); | 141 | regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3); |
123 | regs->sr = 0; | 142 | regs->sr = 0; |
@@ -130,4 +149,25 @@ void init_threads(void) | |||
130 | { | 149 | { |
131 | num_threads = 1; /* We have 1 thread to begin with */ | 150 | num_threads = 1; /* We have 1 thread to begin with */ |
132 | current_thread = 0; /* The current thread is number 0 */ | 151 | current_thread = 0; /* The current thread is number 0 */ |
152 | thread_name[0] = main_thread_name; | ||
153 | thread_stack[0] = stackbegin; | ||
154 | thread_stack_size[0] = (int)stackend - (int)stackbegin; | ||
155 | } | ||
156 | |||
157 | int thread_stack_usage(int threadnum) | ||
158 | { | ||
159 | unsigned int i; | ||
160 | unsigned int *stackptr = thread_stack[threadnum]; | ||
161 | |||
162 | if(threadnum >= num_threads) | ||
163 | return -1; | ||
164 | |||
165 | for(i = 0;i < thread_stack_size[threadnum]/sizeof(int);i++) | ||
166 | { | ||
167 | if(stackptr[i] != 0xdeadbeef) | ||
168 | break; | ||
169 | } | ||
170 | |||
171 | return ((thread_stack_size[threadnum] - i * 4) * 100) / | ||
172 | thread_stack_size[threadnum]; | ||
133 | } | 173 | } |
diff --git a/firmware/thread.h b/firmware/thread.h index 62cd4607d2..c0eba42597 100644 --- a/firmware/thread.h +++ b/firmware/thread.h | |||
@@ -20,9 +20,11 @@ | |||
20 | #define THREAD_H | 20 | #define THREAD_H |
21 | 21 | ||
22 | #define MAXTHREADS 16 | 22 | #define MAXTHREADS 16 |
23 | #define DEFAULT_STACK_SIZE 0x800 /* Bytes */ | ||
23 | 24 | ||
24 | int create_thread(void* function, void* stack, int stack_size); | 25 | int create_thread(void* function, void* stack, int stack_size, char *name); |
25 | void switch_thread(void); | 26 | void switch_thread(void); |
26 | void init_threads(void); | 27 | void init_threads(void); |
28 | int thread_stack_usage(int threadnum); | ||
27 | 29 | ||
28 | #endif | 30 | #endif |