diff options
Diffstat (limited to 'firmware/thread.c')
-rw-r--r-- | firmware/thread.c | 42 |
1 files changed, 29 insertions, 13 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index face2d2445..9a2dbd3033 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -42,12 +42,12 @@ struct regs | |||
42 | void *start; /* Thread start address, or NULL when started */ | 42 | void *start; /* Thread start address, or NULL when started */ |
43 | }; | 43 | }; |
44 | #elif CONFIG_CPU == PP5020 | 44 | #elif CONFIG_CPU == PP5020 |
45 | /* TODO: define struct regs */ | ||
46 | struct regs | 45 | struct regs |
47 | { | 46 | { |
48 | void *sp; /* Stack pointer (a15) */ | 47 | unsigned int r[9]; /* Registers r4-r12 */ |
49 | void *start; /* Thread start address */ | 48 | void *sp; /* Stack pointer (r13) */ |
50 | int started; /* 0 when not started */ | 49 | unsigned int lr; /* r14 (lr) */ |
50 | void *start; /* Thread start address, or NULL when started */ | ||
51 | }; | 51 | }; |
52 | #elif CONFIG_CPU == TCC730 | 52 | #elif CONFIG_CPU == TCC730 |
53 | struct regs | 53 | struct regs |
@@ -79,17 +79,35 @@ static inline void store_context(void* addr) __attribute__ ((always_inline)); | |||
79 | static inline void load_context(const void* addr) __attribute__ ((always_inline)); | 79 | static inline void load_context(const void* addr) __attribute__ ((always_inline)); |
80 | 80 | ||
81 | #if CONFIG_CPU == PP5020 | 81 | #if CONFIG_CPU == PP5020 |
82 | 82 | /*--------------------------------------------------------------------------- | |
83 | /* TODO: Implement store_context and load_context */ | 83 | * Store non-volatile context. |
84 | 84 | *--------------------------------------------------------------------------- | |
85 | */ | ||
85 | static inline void store_context(void* addr) | 86 | static inline void store_context(void* addr) |
86 | { | 87 | { |
87 | (void)addr; | 88 | asm volatile( |
89 | "stmia %0, { r4-r14 }\n" | ||
90 | : : "r" (addr) | ||
91 | ); | ||
88 | } | 92 | } |
89 | 93 | ||
94 | /*--------------------------------------------------------------------------- | ||
95 | * Load non-volatile context. | ||
96 | *--------------------------------------------------------------------------- | ||
97 | */ | ||
90 | static inline void load_context(const void* addr) | 98 | static inline void load_context(const void* addr) |
91 | { | 99 | { |
92 | (void)addr; | 100 | asm volatile( |
101 | "ldmia %0, { r4-r14 } \n" /* load regs r4 to r14 from context */ | ||
102 | "ldr r0, [%0, #44] \n" /* load start pointer */ | ||
103 | "mov r1, #0 \n" | ||
104 | "cmp r0, r1 \n" /* check for NULL */ | ||
105 | "beq .running \n" /* if it's NULL, we're already running */ | ||
106 | "str r1, [%0, #44] \n" | ||
107 | "mov pc, r0 \n" /* not already running, so jump to start */ | ||
108 | ".running: \n" | ||
109 | : : "r" (addr) : "r0", "r1" | ||
110 | ); | ||
93 | } | 111 | } |
94 | 112 | ||
95 | 113 | ||
@@ -312,7 +330,7 @@ int create_thread(void (*function)(void), void* stack, int stack_size, | |||
312 | thread_stack[num_threads] = stack; | 330 | thread_stack[num_threads] = stack; |
313 | thread_stack_size[num_threads] = stack_size; | 331 | thread_stack_size[num_threads] = stack_size; |
314 | regs = &thread_contexts[num_threads]; | 332 | regs = &thread_contexts[num_threads]; |
315 | #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) | 333 | #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) || (CONFIG_CPU == PP5020) |
316 | /* Align stack to an even 32 bit boundary */ | 334 | /* Align stack to an even 32 bit boundary */ |
317 | regs->sp = (void*)(((unsigned int)stack + stack_size) & ~3); | 335 | regs->sp = (void*)(((unsigned int)stack + stack_size) & ~3); |
318 | #elif CONFIG_CPU == TCC730 | 336 | #elif CONFIG_CPU == TCC730 |
@@ -360,12 +378,10 @@ void init_threads(void) | |||
360 | thread_name[0] = main_thread_name; | 378 | thread_name[0] = main_thread_name; |
361 | thread_stack[0] = stackbegin; | 379 | thread_stack[0] = stackbegin; |
362 | thread_stack_size[0] = (int)stackend - (int)stackbegin; | 380 | thread_stack_size[0] = (int)stackend - (int)stackbegin; |
363 | #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) | 381 | #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) || (CONFIG_CPU == PP5020) |
364 | thread_contexts[0].start = 0; /* thread 0 already running */ | 382 | thread_contexts[0].start = 0; /* thread 0 already running */ |
365 | #elif CONFIG_CPU == TCC730 | 383 | #elif CONFIG_CPU == TCC730 |
366 | thread_contexts[0].started = 1; | 384 | thread_contexts[0].started = 1; |
367 | #elif CONFIG_CPU == PP5020 | ||
368 | thread_contexts[0].start = 0; /* thread 0 already running */ | ||
369 | #endif | 385 | #endif |
370 | num_sleepers = 0; | 386 | num_sleepers = 0; |
371 | } | 387 | } |