diff options
Diffstat (limited to 'firmware/thread.c')
-rw-r--r-- | firmware/thread.c | 71 |
1 files changed, 30 insertions, 41 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index f5167872de..fc8370b42d 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -18,33 +18,25 @@ | |||
18 | ****************************************************************************/ | 18 | ****************************************************************************/ |
19 | #include "thread.h" | 19 | #include "thread.h" |
20 | 20 | ||
21 | typedef union | 21 | struct regs |
22 | { | 22 | { |
23 | struct regs_t | 23 | unsigned int r[7]; /* Registers r8 thru r14 */ |
24 | { | 24 | void *sp; /* Stack pointer (r15) */ |
25 | unsigned int r[7]; /* Registers r8 thru r14 */ | 25 | unsigned int mach; |
26 | void *sp; /* Stack pointer (r15) */ | 26 | unsigned int macl; |
27 | unsigned int mach; | 27 | unsigned int sr; /* Status register */ |
28 | unsigned int macl; | 28 | void* pr; /* Procedure register */ |
29 | unsigned int sr; /* Status register */ | 29 | }; |
30 | void* pr; /* Procedure register */ | ||
31 | } regs; | ||
32 | } ctx_t; | ||
33 | |||
34 | typedef struct | ||
35 | { | ||
36 | int num_threads; | ||
37 | int current; | ||
38 | ctx_t ctx[MAXTHREADS]; | ||
39 | } thread_t; | ||
40 | 30 | ||
41 | static thread_t threads; | 31 | static int num_threads; |
32 | static int current_thread; | ||
33 | static struct regs thread_contexts[MAXTHREADS]; | ||
42 | 34 | ||
43 | /*--------------------------------------------------------------------------- | 35 | /*--------------------------------------------------------------------------- |
44 | * Store non-volatile context. | 36 | * Store non-volatile context. |
45 | *--------------------------------------------------------------------------- | 37 | *--------------------------------------------------------------------------- |
46 | */ | 38 | */ |
47 | static inline void stctx(void* addr) | 39 | static inline void store_context(void* addr) |
48 | { | 40 | { |
49 | asm volatile ("add #48, %0\n\t" | 41 | asm volatile ("add #48, %0\n\t" |
50 | "sts.l pr, @-%0\n\t" | 42 | "sts.l pr, @-%0\n\t" |
@@ -65,7 +57,7 @@ static inline void stctx(void* addr) | |||
65 | * Load non-volatile context. | 57 | * Load non-volatile context. |
66 | *--------------------------------------------------------------------------- | 58 | *--------------------------------------------------------------------------- |
67 | */ | 59 | */ |
68 | static inline void ldctx(void* addr) | 60 | static inline void load_context(void* addr) |
69 | { | 61 | { |
70 | asm volatile ("mov.l @%0+,r8\n\t" | 62 | asm volatile ("mov.l @%0+,r8\n\t" |
71 | "mov.l @%0+,r9\n\t" | 63 | "mov.l @%0+,r9\n\t" |
@@ -89,16 +81,15 @@ static inline void ldctx(void* addr) | |||
89 | */ | 81 | */ |
90 | void switch_thread(void) | 82 | void switch_thread(void) |
91 | { | 83 | { |
92 | int ct; | 84 | int current; |
93 | int nt; | 85 | int next; |
94 | thread_t* t = &threads; | ||
95 | 86 | ||
96 | nt = ct = t->current; | 87 | next = current = current_thread; |
97 | if (++nt >= t->num_threads) | 88 | if (++next >= num_threads) |
98 | nt = 0; | 89 | next = 0; |
99 | t->current = nt; | 90 | current_thread = next; |
100 | stctx(&t->ctx[ct]); | 91 | store_context(&thread_contexts[current]); |
101 | ldctx(&t->ctx[nt]); | 92 | load_context(&thread_contexts[next]); |
102 | } | 93 | } |
103 | 94 | ||
104 | /*--------------------------------------------------------------------------- | 95 | /*--------------------------------------------------------------------------- |
@@ -106,27 +97,25 @@ void switch_thread(void) | |||
106 | * Return 0 if context area could be allocated, else -1. | 97 | * Return 0 if context area could be allocated, else -1. |
107 | *--------------------------------------------------------------------------- | 98 | *--------------------------------------------------------------------------- |
108 | */ | 99 | */ |
109 | int create_thread(void* fp, void* sp, int stk_size) | 100 | int create_thread(void* function, void* stack, int stack_size) |
110 | { | 101 | { |
111 | thread_t* t = &threads; | 102 | if (num_threads >= MAXTHREADS) |
112 | |||
113 | if (t->num_threads >= MAXTHREADS) | ||
114 | return -1; | 103 | return -1; |
115 | else | 104 | else |
116 | { | 105 | { |
117 | ctx_t* ctxp = &t->ctx[t->num_threads++]; | 106 | struct regs* regs = &thread_contexts[num_threads++]; |
118 | stctx(ctxp); | 107 | store_context(regs); |
119 | /* Subtract 4 to leave room for the PR push in ldctx() | 108 | /* Subtract 4 to leave room for the PR push in ldctx() |
120 | Align it on an even 32 bit boundary */ | 109 | Align it on an even 32 bit boundary */ |
121 | ctxp->regs.sp = (void*)(((unsigned int)sp + stk_size - 4) & ~3); | 110 | regs->sp = (void*)(((unsigned int)stack + stack_size - 4) & ~3); |
122 | ctxp->regs.sr = 0; | 111 | regs->sr = 0; |
123 | ctxp->regs.pr = fp; | 112 | regs->pr = function; |
124 | } | 113 | } |
125 | return 0; | 114 | return 0; |
126 | } | 115 | } |
127 | 116 | ||
128 | void init_threads(void) | 117 | void init_threads(void) |
129 | { | 118 | { |
130 | threads.num_threads = 1; /* We have 1 thread to begin with */ | 119 | num_threads = 1; /* We have 1 thread to begin with */ |
131 | threads.current = 0; /* The current thread is number 0 */ | 120 | current_thread = 0; /* The current thread is number 0 */ |
132 | } | 121 | } |