diff options
Diffstat (limited to 'firmware/thread.c')
-rw-r--r-- | firmware/thread.c | 35 |
1 files changed, 35 insertions, 0 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index 2651a4f2d7..ea518ca4ee 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -16,8 +16,11 @@ | |||
16 | * KIND, either express or implied. | 16 | * KIND, either express or implied. |
17 | * | 17 | * |
18 | ****************************************************************************/ | 18 | ****************************************************************************/ |
19 | #include <stdbool.h> | ||
19 | #include "thread.h" | 20 | #include "thread.h" |
20 | #include "panic.h" | 21 | #include "panic.h" |
22 | #include "kernel.h" | ||
23 | #include "sh7034.h" | ||
21 | 24 | ||
22 | struct regs | 25 | struct regs |
23 | { | 26 | { |
@@ -30,6 +33,8 @@ struct regs | |||
30 | }; | 33 | }; |
31 | 34 | ||
32 | int num_threads; | 35 | int num_threads; |
36 | bool cpu_sleep_enabled; | ||
37 | static volatile int num_sleepers; | ||
33 | static int current_thread; | 38 | static int current_thread; |
34 | static struct regs thread_contexts[MAXTHREADS] __attribute__ ((section(".idata"))); | 39 | static struct regs thread_contexts[MAXTHREADS] __attribute__ ((section(".idata"))); |
35 | char *thread_name[MAXTHREADS]; | 40 | char *thread_name[MAXTHREADS]; |
@@ -95,6 +100,18 @@ void switch_thread(void) | |||
95 | int next; | 100 | int next; |
96 | unsigned int *stackptr; | 101 | unsigned int *stackptr; |
97 | 102 | ||
103 | #ifdef SIMULATOR | ||
104 | /* Do nothing */ | ||
105 | #else | ||
106 | |||
107 | while (cpu_sleep_enabled && num_sleepers == num_threads) | ||
108 | { | ||
109 | /* Enter sleep mode, woken up on interrupt */ | ||
110 | SBYCR &= 0x7F; | ||
111 | asm volatile ("sleep"); | ||
112 | } | ||
113 | |||
114 | #endif | ||
98 | next = current = current_thread; | 115 | next = current = current_thread; |
99 | if (++next >= num_threads) | 116 | if (++next >= num_threads) |
100 | next = 0; | 117 | next = 0; |
@@ -108,6 +125,22 @@ void switch_thread(void) | |||
108 | panicf("Stkov %s", thread_name[next]); | 125 | panicf("Stkov %s", thread_name[next]); |
109 | } | 126 | } |
110 | 127 | ||
128 | void cpu_sleep(bool enabled) | ||
129 | { | ||
130 | cpu_sleep_enabled = enabled; | ||
131 | } | ||
132 | |||
133 | void sleep_thread(void) | ||
134 | { | ||
135 | ++num_sleepers; | ||
136 | switch_thread(); | ||
137 | } | ||
138 | |||
139 | void wake_up_thread(void) | ||
140 | { | ||
141 | num_sleepers = 0; | ||
142 | } | ||
143 | |||
111 | /*--------------------------------------------------------------------------- | 144 | /*--------------------------------------------------------------------------- |
112 | * Create thread. | 145 | * Create thread. |
113 | * Return 0 if context area could be allocated, else -1. | 146 | * Return 0 if context area could be allocated, else -1. |
@@ -144,6 +177,7 @@ int create_thread(void* function, void* stack, int stack_size, char *name) | |||
144 | regs->sr = 0; | 177 | regs->sr = 0; |
145 | regs->pr = function; | 178 | regs->pr = function; |
146 | } | 179 | } |
180 | wake_up_thread(); | ||
147 | return 0; | 181 | return 0; |
148 | } | 182 | } |
149 | 183 | ||
@@ -154,6 +188,7 @@ void init_threads(void) | |||
154 | thread_name[0] = main_thread_name; | 188 | thread_name[0] = main_thread_name; |
155 | thread_stack[0] = stackbegin; | 189 | thread_stack[0] = stackbegin; |
156 | thread_stack_size[0] = (int)stackend - (int)stackbegin; | 190 | thread_stack_size[0] = (int)stackend - (int)stackbegin; |
191 | num_sleepers = 0; | ||
157 | } | 192 | } |
158 | 193 | ||
159 | int thread_stack_usage(int threadnum) | 194 | int thread_stack_usage(int threadnum) |