diff options
Diffstat (limited to 'firmware/thread.c')
-rw-r--r-- | firmware/thread.c | 47 |
1 files changed, 41 insertions, 6 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index a803e4f78d..275f04d029 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -64,12 +64,15 @@ int *cop_stackend = stackend; | |||
64 | #endif | 64 | #endif |
65 | 65 | ||
66 | #if NUM_CORES > 1 | 66 | #if NUM_CORES > 1 |
67 | #if 0 | ||
67 | static long cores_locked IBSS_ATTR; | 68 | static long cores_locked IBSS_ATTR; |
68 | 69 | ||
69 | #define LOCK(...) do { } while (test_and_set(&cores_locked, 1)) | 70 | #define LOCK(...) do { } while (test_and_set(&cores_locked, 1)) |
70 | #define UNLOCK(...) cores_locked = 0 | 71 | #define UNLOCK(...) cores_locked = 0 |
72 | #endif | ||
71 | 73 | ||
72 | #warning "Core locking mechanism should be fixed on H10/4G!" | 74 | #warning "Core locking mechanism should be fixed on H10/4G!" |
75 | |||
73 | inline void lock_cores(void) | 76 | inline void lock_cores(void) |
74 | { | 77 | { |
75 | #if 0 | 78 | #if 0 |
@@ -125,15 +128,47 @@ static inline void store_context(void* addr) | |||
125 | * Load non-volatile context. | 128 | * Load non-volatile context. |
126 | *--------------------------------------------------------------------------- | 129 | *--------------------------------------------------------------------------- |
127 | */ | 130 | */ |
131 | static void start_thread(void (*thread_func)(void), const void* addr) __attribute__((naked)); | ||
132 | static void start_thread(void (*thread_func)(void), const void* addr) | ||
133 | { | ||
134 | /* r0 = thread_func, r1 = addr */ | ||
135 | #if NUM_CORES > 1 | ||
136 | asm volatile ( | ||
137 | "mov r2, #0 \n" | ||
138 | "str r2, [r1, #40] \n" | ||
139 | "ldr r1, =0xf000f044 \n" /* invalidate this core's cache */ | ||
140 | "ldr r2, [r1] \n" | ||
141 | "orr r2, r2, #6 \n" | ||
142 | "str r2, [r1] \n" | ||
143 | "ldr r1, =0x6000c000 \n" | ||
144 | "1: \n" | ||
145 | "ldr r2, [r1] \n" | ||
146 | "tst r2, #0x8000 \n" | ||
147 | "bne 1b \n" | ||
148 | "mov pc, r0 \n" | ||
149 | : : : "r1", "r2" | ||
150 | ); | ||
151 | #else | ||
152 | asm volatile ( | ||
153 | "mov r2, #0 \n" | ||
154 | "str r2, [r1, #40] \n" | ||
155 | "mov pc, r0 \n" | ||
156 | : : : "r1", "r2" | ||
157 | ); | ||
158 | #endif | ||
159 | (void)thread_func; | ||
160 | (void)addr; | ||
161 | (void)start_thread; | ||
162 | } | ||
163 | |||
128 | static inline void load_context(const void* addr) | 164 | static inline void load_context(const void* addr) |
129 | { | 165 | { |
130 | asm volatile( | 166 | asm volatile( |
131 | "ldmia %0, { r4-r11, sp, lr }\n" /* load regs r4 to r14 from context */ | 167 | "ldmia %0, { r4-r11, sp, lr } \n" /* load regs r4 to r14 from context */ |
132 | "ldr r0, [%0, #40] \n" /* load start pointer */ | 168 | "ldr r0, [%0, #40] \n" /* load start pointer */ |
133 | "mov r1, #0 \n" | 169 | "cmp r0, #0 \n" /* check for NULL */ |
134 | "cmp r0, r1 \n" /* check for NULL */ | 170 | "movne r1, %0 \n" /* if not already running, jump to start */ |
135 | "strne r1, [%0, #40] \n" /* if it's NULL, we're already running */ | 171 | "ldrne pc, =start_thread \n" |
136 | "movne pc, r0 \n" /* not already running, so jump to start */ | ||
137 | : : "r" (addr) : "r0", "r1" | 172 | : : "r" (addr) : "r0", "r1" |
138 | ); | 173 | ); |
139 | } | 174 | } |