diff options
Diffstat (limited to 'firmware')
-rw-r--r-- | firmware/thread.c | 219 |
1 files changed, 119 insertions, 100 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index b7769535f7..7ddceaa9a1 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -26,24 +26,25 @@ | |||
26 | #if CONFIG_CPU == MCF5249 | 26 | #if CONFIG_CPU == MCF5249 |
27 | struct regs | 27 | struct regs |
28 | { | 28 | { |
29 | unsigned int d[6]; /* d2-d7 */ | 29 | unsigned int d[6]; /* d2-d7 */ |
30 | unsigned int a[5]; /* a2-a6 */ | 30 | unsigned int a[5]; /* a2-a6 */ |
31 | void *sp; /* Stack pointer (a7) */ | 31 | void *sp; /* Stack pointer (a7) */ |
32 | void *start; /* Thread start address, or NULL when started */ | ||
32 | }; | 33 | }; |
33 | #elif CONFIG_CPU == SH7034 | 34 | #elif CONFIG_CPU == SH7034 |
34 | struct regs | 35 | struct regs |
35 | { | 36 | { |
36 | unsigned int r[7]; /* Registers r8 thru r14 */ | 37 | unsigned int r[7]; /* Registers r8 thru r14 */ |
37 | void *sp; /* Stack pointer (r15) */ | 38 | void *sp; /* Stack pointer (r15) */ |
38 | void* pr; /* Procedure register */ | 39 | void *pr; /* Procedure register */ |
40 | void *start; /* Thread start address, or NULL when started */ | ||
39 | }; | 41 | }; |
40 | |||
41 | #elif CONFIG_CPU == TCC730 | 42 | #elif CONFIG_CPU == TCC730 |
42 | struct regs | 43 | struct regs |
43 | { | 44 | { |
44 | void *sp; /* Stack pointer (a15) */ | 45 | void *sp; /* Stack pointer (a15) */ |
45 | void *start; /* Thread start address */ | 46 | void *start; /* Thread start address */ |
46 | int started; /* 0 when not started */ | 47 | int started; /* 0 when not started */ |
47 | }; | 48 | }; |
48 | #endif | 49 | #endif |
49 | 50 | ||
@@ -74,7 +75,10 @@ static inline void load_context(const void* addr) __attribute__ ((always_inline) | |||
74 | */ | 75 | */ |
75 | static inline void store_context(void* addr) | 76 | static inline void store_context(void* addr) |
76 | { | 77 | { |
77 | asm volatile ("movem.l %%d2-%%d7/%%a2-%%a7,(%0)\n\t" : : "a" (addr)); | 78 | asm volatile ( |
79 | "movem.l %%d2-%%d7/%%a2-%%a7,(%0)\n" | ||
80 | : : "a" (addr) | ||
81 | ); | ||
78 | } | 82 | } |
79 | 83 | ||
80 | /*--------------------------------------------------------------------------- | 84 | /*--------------------------------------------------------------------------- |
@@ -83,7 +87,16 @@ static inline void store_context(void* addr) | |||
83 | */ | 87 | */ |
84 | static inline void load_context(const void* addr) | 88 | static inline void load_context(const void* addr) |
85 | { | 89 | { |
86 | asm volatile ("movem.l (%0),%%d2-%%d7/%%a2-%%a7\n\t" : : "a" (addr)); | 90 | asm volatile ( |
91 | "movem.l (%0),%%d2-%%d7/%%a2-%%a7\n" /* Load context */ | ||
92 | "move.l (48,%0),%%d0 \n" /* Get start address */ | ||
93 | "beq.b .running \n" /* NULL -> already running */ | ||
94 | "clr.l (48,%0) \n" /* Clear start address.. */ | ||
95 | "move.l %%d0,%0 \n" | ||
96 | "jmp (%0) \n" /* ..and start the thread */ | ||
97 | ".running: \n" | ||
98 | : : "a" (addr) : "d0" /* only! */ | ||
99 | ); | ||
87 | } | 100 | } |
88 | 101 | ||
89 | #elif CONFIG_CPU == SH7034 | 102 | #elif CONFIG_CPU == SH7034 |
@@ -93,36 +106,49 @@ static inline void load_context(const void* addr) | |||
93 | */ | 106 | */ |
94 | static inline void store_context(void* addr) | 107 | static inline void store_context(void* addr) |
95 | { | 108 | { |
96 | asm volatile ("add #36, %0\n\t" | 109 | asm volatile ( |
97 | "sts.l pr, @-%0\n\t" | 110 | "add #36,%0 \n" |
98 | "mov.l r15, @-%0\n\t" | 111 | "sts.l pr, @-%0 \n" |
99 | "mov.l r14, @-%0\n\t" | 112 | "mov.l r15,@-%0 \n" |
100 | "mov.l r13, @-%0\n\t" | 113 | "mov.l r14,@-%0 \n" |
101 | "mov.l r12, @-%0\n\t" | 114 | "mov.l r13,@-%0 \n" |
102 | "mov.l r11, @-%0\n\t" | 115 | "mov.l r12,@-%0 \n" |
103 | "mov.l r10, @-%0\n\t" | 116 | "mov.l r11,@-%0 \n" |
104 | "mov.l r9, @-%0\n\t" | 117 | "mov.l r10,@-%0 \n" |
105 | "mov.l r8, @-%0" : : "r" (addr)); | 118 | "mov.l r9, @-%0 \n" |
119 | "mov.l r8, @-%0 \n" | ||
120 | : : "r" (addr) | ||
121 | ); | ||
106 | } | 122 | } |
107 | 123 | ||
108 | /*--------------------------------------------------------------------------- | 124 | /*--------------------------------------------------------------------------- |
109 | * Load non-volatile context. | 125 | * Load non-volatile context. |
110 | *--------------------------------------------------------------------------- | 126 | *--------------------------------------------------------------------------- |
111 | */ | 127 | */ |
112 | static inline void load_context(const void* addr) | 128 | static inline void load_context(const void* addr) |
113 | { | 129 | { |
114 | asm volatile ("mov.l @%0+,r8\n\t" | 130 | asm volatile ( |
115 | "mov.l @%0+,r9\n\t" | 131 | "mov.l @%0+,r8 \n" |
116 | "mov.l @%0+,r10\n\t" | 132 | "mov.l @%0+,r9 \n" |
117 | "mov.l @%0+,r11\n\t" | 133 | "mov.l @%0+,r10 \n" |
118 | "mov.l @%0+,r12\n\t" | 134 | "mov.l @%0+,r11 \n" |
119 | "mov.l @%0+,r13\n\t" | 135 | "mov.l @%0+,r12 \n" |
120 | "mov.l @%0+,r14\n\t" | 136 | "mov.l @%0+,r13 \n" |
121 | "mov.l @%0+,r15\n\t" | 137 | "mov.l @%0+,r14 \n" |
122 | "mov.l @%0,%0\n\t" | 138 | "mov.l @%0+,r15 \n" |
123 | "lds %0,pr\n\t" | 139 | "lds.l @%0+,pr \n" |
124 | "mov.l %0, @(0, r15)" : "+r" (addr)); | 140 | "mov.l @%0,r0 \n" /* Get start address */ |
141 | "tst r0,r0 \n" | ||
142 | "bt .running \n" /* NULL -> already running */ | ||
143 | "lds r0,pr \n" | ||
144 | "mov #0,r0 \n" | ||
145 | "rts \n" /* Start the thread */ | ||
146 | "mov.l r0,@%0 \n" /* Clear start address */ | ||
147 | ".running: \n" | ||
148 | : : "r" (addr) : "r0" /* only! */ | ||
149 | ); | ||
125 | } | 150 | } |
151 | |||
126 | #elif CONFIG_CPU == TCC730 | 152 | #elif CONFIG_CPU == TCC730 |
127 | /*--------------------------------------------------------------------------- | 153 | /*--------------------------------------------------------------------------- |
128 | * Store non-volatile context. | 154 | * Store non-volatile context. |
@@ -177,7 +203,6 @@ static inline void load_context(const void* addr) | |||
177 | void switch_thread(void) | 203 | void switch_thread(void) |
178 | { | 204 | { |
179 | int current; | 205 | int current; |
180 | int next; | ||
181 | unsigned int *stackptr; | 206 | unsigned int *stackptr; |
182 | 207 | ||
183 | #ifdef SIMULATOR | 208 | #ifdef SIMULATOR |
@@ -189,35 +214,35 @@ void switch_thread(void) | |||
189 | /* Enter sleep mode, woken up on interrupt */ | 214 | /* Enter sleep mode, woken up on interrupt */ |
190 | #if CONFIG_CPU == MCF5249 | 215 | #if CONFIG_CPU == MCF5249 |
191 | asm volatile ("stop #0x2000"); | 216 | asm volatile ("stop #0x2000"); |
217 | #elif CONFIG_CPU == SH7034 | ||
218 | SBYCR &= 0x7F; | ||
219 | asm volatile ("sleep"); | ||
192 | #elif CONFIG_CPU == TCC730 | 220 | #elif CONFIG_CPU == TCC730 |
193 | /* Sleep mode is triggered by the SYS instr on CalmRisc16. | 221 | /* Sleep mode is triggered by the SYS instr on CalmRisc16. |
194 | * Unfortunately, the manual doesn't specify which arg to use. | 222 | * Unfortunately, the manual doesn't specify which arg to use. |
195 | __asm__ volatile ("sys #0x0f"); | 223 | __asm__ volatile ("sys #0x0f"); |
196 | 0x1f seems to trigger a reset; | 224 | 0x1f seems to trigger a reset; |
197 | 0x0f is the only one other argument used by Archos. | 225 | 0x0f is the only one other argument used by Archos. |
198 | */ | 226 | */ |
199 | #else | ||
200 | SBYCR &= 0x7F; | ||
201 | asm volatile ("sleep"); | ||
202 | #endif | 227 | #endif |
203 | } | 228 | } |
204 | 229 | ||
205 | #endif | 230 | #endif |
206 | next = current = current_thread; | 231 | current = current_thread; |
207 | |||
208 | if (++next >= num_threads) | ||
209 | next = 0; | ||
210 | current_thread = next; | ||
211 | store_context(&thread_contexts[current]); | 232 | store_context(&thread_contexts[current]); |
212 | 233 | ||
234 | #if CONFIG_CPU != TCC730 | ||
213 | /* Check if the current thread stack is overflown */ | 235 | /* Check if the current thread stack is overflown */ |
214 | stackptr = thread_stack[current]; | 236 | stackptr = thread_stack[current]; |
215 | #if ! (defined(IRIVER_H100) || (CONFIG_CPU == TCC730)) | ||
216 | if(stackptr[0] != DEADBEEF) | 237 | if(stackptr[0] != DEADBEEF) |
217 | panicf("Stkov %s", thread_name[current]); | 238 | panicf("Stkov %s", thread_name[current]); |
218 | #endif | 239 | #endif |
219 | 240 | ||
220 | load_context(&thread_contexts[next]); | 241 | if (++current >= num_threads) |
242 | current = 0; | ||
243 | |||
244 | current_thread = current; | ||
245 | load_context(&thread_contexts[current]); | ||
221 | } | 246 | } |
222 | 247 | ||
223 | void sleep_thread(void) | 248 | void sleep_thread(void) |
@@ -239,47 +264,39 @@ void wake_up_thread(void) | |||
239 | int create_thread(void* function, void* stack, int stack_size, | 264 | int create_thread(void* function, void* stack, int stack_size, |
240 | const char *name) | 265 | const char *name) |
241 | { | 266 | { |
242 | unsigned int i; | 267 | unsigned int i; |
243 | unsigned int stacklen; | 268 | unsigned int stacklen; |
244 | unsigned int *stackptr; | 269 | unsigned int *stackptr; |
245 | struct regs *regs; | 270 | struct regs *regs; |
246 | 271 | ||
247 | if (num_threads >= MAXTHREADS) | 272 | if (num_threads >= MAXTHREADS) |
248 | return -1; | 273 | return -1; |
249 | 274 | ||
250 | /* Munge the stack to make it easy to spot stack overflows */ | 275 | /* Munge the stack to make it easy to spot stack overflows */ |
251 | stacklen = stack_size / sizeof(int); | 276 | stacklen = stack_size / sizeof(int); |
252 | stackptr = stack; | 277 | stackptr = stack; |
253 | for(i = 0;i < stacklen;i++) | 278 | for(i = 0;i < stacklen;i++) |
254 | { | 279 | { |
255 | stackptr[i] = DEADBEEF; | 280 | stackptr[i] = DEADBEEF; |
256 | } | 281 | } |
257 | 282 | ||
258 | /* Store interesting information */ | 283 | /* Store interesting information */ |
259 | thread_name[num_threads] = name; | 284 | thread_name[num_threads] = name; |
260 | thread_stack[num_threads] = stack; | 285 | thread_stack[num_threads] = stack; |
261 | thread_stack_size[num_threads] = stack_size; | 286 | thread_stack_size[num_threads] = stack_size; |
262 | regs = &thread_contexts[num_threads]; | 287 | regs = &thread_contexts[num_threads]; |
263 | #if CONFIG_CPU == MCF5249 | 288 | #if (CONFIG_CPU == MCF5249) || (CONFIG_CPU == SH7034) |
264 | store_context(regs); | 289 | /* Align stack to an even 32 bit boundary */ |
265 | regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3); | 290 | regs->sp = (void*)(((unsigned int)stack + stack_size) & ~3); |
266 | /* Put the return address on the stack */ | ||
267 | *(unsigned long *)(regs->sp) = (int)function; | ||
268 | #elif CONFIG_CPU == SH7034 | ||
269 | store_context(regs); | ||
270 | /* Subtract 4 to leave room for the PR push in load_context() | ||
271 | Align it on an even 32 bit boundary */ | ||
272 | regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3); | ||
273 | regs->pr = function; | ||
274 | #elif CONFIG_CPU == TCC730 | 291 | #elif CONFIG_CPU == TCC730 |
275 | /* Align stack on word boundary */ | 292 | /* Align stack on word boundary */ |
276 | regs->sp = (void*)(((unsigned long)stack + stack_size - 2) & ~1); | 293 | regs->sp = (void*)(((unsigned long)stack + stack_size - 2) & ~1); |
277 | regs->start = (void*)function; | 294 | regs->started = 0; |
278 | regs->started = 0; | ||
279 | #endif | 295 | #endif |
296 | regs->start = (void*)function; | ||
280 | 297 | ||
281 | wake_up_thread(); | 298 | wake_up_thread(); |
282 | return num_threads++; /* return the current ID, e.g for remove_thread() */ | 299 | return num_threads++; /* return the current ID, e.g for remove_thread() */ |
283 | } | 300 | } |
284 | 301 | ||
285 | /*--------------------------------------------------------------------------- | 302 | /*--------------------------------------------------------------------------- |
@@ -316,7 +333,9 @@ void init_threads(void) | |||
316 | thread_name[0] = main_thread_name; | 333 | thread_name[0] = main_thread_name; |
317 | thread_stack[0] = stackbegin; | 334 | thread_stack[0] = stackbegin; |
318 | thread_stack_size[0] = (int)stackend - (int)stackbegin; | 335 | thread_stack_size[0] = (int)stackend - (int)stackbegin; |
319 | #if CONFIG_CPU == TCC730 | 336 | #if (CONFIG_CPU == MCF5249) || (CONFIG_CPU == SH7034) |
337 | thread_contexts[0].start = 0; /* thread 0 already running */ | ||
338 | #elif CONFIG_CPU == TCC730 | ||
320 | thread_contexts[0].started = 1; | 339 | thread_contexts[0].started = 1; |
321 | #endif | 340 | #endif |
322 | num_sleepers = 0; | 341 | num_sleepers = 0; |
@@ -324,18 +343,18 @@ void init_threads(void) | |||
324 | 343 | ||
325 | int thread_stack_usage(int threadnum) | 344 | int thread_stack_usage(int threadnum) |
326 | { | 345 | { |
327 | unsigned int i; | 346 | unsigned int i; |
328 | unsigned int *stackptr = thread_stack[threadnum]; | 347 | unsigned int *stackptr = thread_stack[threadnum]; |
329 | 348 | ||
330 | if(threadnum >= num_threads) | 349 | if(threadnum >= num_threads) |
331 | return -1; | 350 | return -1; |
332 | 351 | ||
333 | for(i = 0;i < thread_stack_size[threadnum]/sizeof(int);i++) | 352 | for(i = 0;i < thread_stack_size[threadnum]/sizeof(int);i++) |
334 | { | 353 | { |
335 | if(stackptr[i] != DEADBEEF) | 354 | if(stackptr[i] != DEADBEEF) |
336 | break; | 355 | break; |
337 | } | 356 | } |
338 | 357 | ||
339 | return ((thread_stack_size[threadnum] - i * 4) * 100) / | 358 | return ((thread_stack_size[threadnum] - i * sizeof(int)) * 100) / |
340 | thread_stack_size[threadnum]; | 359 | thread_stack_size[threadnum]; |
341 | } | 360 | } |