diff options
-rw-r--r-- | firmware/thread.c | 34 |
1 files changed, 17 insertions, 17 deletions
diff --git a/firmware/thread.c b/firmware/thread.c index 040818f31c..0f5378de7b 100644 --- a/firmware/thread.c +++ b/firmware/thread.c | |||
@@ -276,17 +276,19 @@ void corelock_init(struct corelock *cl) | |||
276 | void corelock_lock(struct corelock *cl) __attribute__((naked)); | 276 | void corelock_lock(struct corelock *cl) __attribute__((naked)); |
277 | void corelock_lock(struct corelock *cl) | 277 | void corelock_lock(struct corelock *cl) |
278 | { | 278 | { |
279 | /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */ | ||
279 | asm volatile ( | 280 | asm volatile ( |
280 | "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ | 281 | "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ |
281 | "ldrb r1, [r1] \n" | 282 | "ldrb r1, [r1] \n" |
282 | "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */ | 283 | "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */ |
283 | "and r2, r1, #1 \n" /* r2 = othercore */ | 284 | "eor r2, r1, #0xff \n" /* r2 = othercore */ |
284 | "strb r2, [r0, #2] \n" /* cl->turn = othercore */ | 285 | "strb r2, [r0, #2] \n" /* cl->turn = othercore */ |
285 | "1: \n" | 286 | "1: \n" |
286 | "ldrb r3, [r0, r2] \n" /* cl->myl[othercore] == 0 ? */ | 287 | "ldrb r3, [r0, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */ |
287 | "cmp r3, #0 \n" | 288 | "cmp r3, #0 \n" /* yes? lock acquired */ |
288 | "ldrneb r3, [r0, #2] \n" /* || cl->turn == core ? */ | 289 | "bxeq lr \n" |
289 | "cmpne r3, r1, lsr #7 \n" | 290 | "ldrb r3, [r0, #2] \n" /* || cl->turn == core ? */ |
291 | "cmp r3, r1 \n" | ||
290 | "bxeq lr \n" /* yes? lock acquired */ | 292 | "bxeq lr \n" /* yes? lock acquired */ |
291 | "b 1b \n" /* keep trying */ | 293 | "b 1b \n" /* keep trying */ |
292 | : : "i"(&PROCESSOR_ID) | 294 | : : "i"(&PROCESSOR_ID) |
@@ -301,23 +303,21 @@ void corelock_lock(struct corelock *cl) | |||
301 | int corelock_try_lock(struct corelock *cl) __attribute__((naked)); | 303 | int corelock_try_lock(struct corelock *cl) __attribute__((naked)); |
302 | int corelock_try_lock(struct corelock *cl) | 304 | int corelock_try_lock(struct corelock *cl) |
303 | { | 305 | { |
306 | /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */ | ||
304 | asm volatile ( | 307 | asm volatile ( |
305 | "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ | 308 | "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ |
306 | "ldrb r1, [r1] \n" | 309 | "ldrb r1, [r1] \n" |
310 | "mov r3, r0 \n" | ||
307 | "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */ | 311 | "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */ |
308 | "and r2, r1, #1 \n" /* r2 = othercore */ | 312 | "eor r2, r1, #0xff \n" /* r2 = othercore */ |
309 | "strb r2, [r0, #2] \n" /* cl->turn = othercore */ | 313 | "strb r2, [r0, #2] \n" /* cl->turn = othercore */ |
310 | "1: \n" | 314 | "ldrb r0, [r3, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */ |
311 | "ldrb r3, [r0, r2] \n" /* cl->myl[othercore] == 0 ? */ | 315 | "eors r0, r0, r2 \n" /* yes? lock acquired */ |
312 | "cmp r3, #0 \n" | 316 | "bxne lr \n" |
313 | "ldrneb r3, [r0, #2] \n" /* || cl->turn == core? */ | 317 | "ldrb r0, [r3, #2] \n" /* || cl->turn == core? */ |
314 | "cmpne r3, r1, lsr #7 \n" | 318 | "ands r0, r0, r1 \n" |
315 | "moveq r0, #1 \n" /* yes? lock acquired */ | 319 | "streqb r0, [r3, r1, lsr #7] \n" /* if not, cl->myl[core] = 0 */ |
316 | "bxeq lr \n" | 320 | "bx lr \n" /* return result */ |
317 | "mov r2, #0 \n" /* cl->myl[core] = 0 */ | ||
318 | "strb r2, [r0, r1, lsr #7] \n" | ||
319 | "mov r0, r2 \n" | ||
320 | "bx lr \n" /* acquisition failed */ | ||
321 | : : "i"(&PROCESSOR_ID) | 321 | : : "i"(&PROCESSOR_ID) |
322 | ); | 322 | ); |
323 | 323 | ||