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)
 void corelock_lock(struct corelock *cl) __attribute__((naked));
 void corelock_lock(struct corelock *cl)
 {
+    /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */
     asm volatile (
         "mov    r1, %0               \n" /* r1 = PROCESSOR_ID */
         "ldrb   r1, [r1]             \n"
         "strb   r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */
-        "and    r2, r1, #1           \n" /* r2 = othercore */
+        "eor    r2, r1, #0xff        \n" /* r2 = othercore */
         "strb   r2, [r0, #2]         \n" /* cl->turn = othercore */
     "1:                              \n"
-        "ldrb   r3, [r0, r2]         \n" /* cl->myl[othercore] == 0 ? */
-        "cmp    r3, #0               \n"
-        "ldrneb r3, [r0, #2]         \n" /* || cl->turn == core ? */
-        "cmpne  r3, r1, lsr #7       \n"
+        "ldrb   r3, [r0, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */
+        "cmp    r3, #0               \n" /* yes? lock acquired */
+        "bxeq   lr                   \n"
+        "ldrb   r3, [r0, #2]         \n" /* || cl->turn == core ? */
+        "cmp    r3, r1               \n"
         "bxeq   lr                   \n" /* yes? lock acquired */
         "b      1b                   \n" /* keep trying */
         : : "i"(&PROCESSOR_ID)
@@ -301,23 +303,21 @@ void corelock_lock(struct corelock *cl)
 int corelock_try_lock(struct corelock *cl) __attribute__((naked));
 int corelock_try_lock(struct corelock *cl)
 {
+    /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */
     asm volatile (
         "mov    r1, %0               \n" /* r1 = PROCESSOR_ID */
         "ldrb   r1, [r1]             \n"
+        "mov    r3, r0               \n"
         "strb   r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */
-        "and    r2, r1, #1           \n" /* r2 = othercore */
+        "eor    r2, r1, #0xff        \n" /* r2 = othercore */
         "strb   r2, [r0, #2]         \n" /* cl->turn = othercore */
-    "1:                              \n"
-        "ldrb   r3, [r0, r2]         \n" /* cl->myl[othercore] == 0 ? */
-        "cmp    r3, #0               \n"
-        "ldrneb r3, [r0, #2]         \n" /* || cl->turn == core? */
-        "cmpne  r3, r1, lsr #7       \n"
-        "moveq  r0, #1               \n" /* yes? lock acquired */
-        "bxeq   lr                   \n"
-        "mov    r2, #0               \n" /* cl->myl[core] = 0 */
-        "strb   r2, [r0, r1, lsr #7] \n"
-        "mov    r0, r2               \n"
-        "bx     lr                   \n" /* acquisition failed */
+        "ldrb   r0, [r3, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */
+        "eors   r0, r0, r2           \n" /* yes? lock acquired */
+        "bxne   lr                   \n"
+        "ldrb   r0, [r3, #2]         \n" /* || cl->turn == core? */
+        "ands   r0, r0, r1           \n"
+        "streqb r0, [r3, r1, lsr #7] \n" /* if not, cl->myl[core] = 0 */
+        "bx     lr                   \n" /* return result */
         : : "i"(&PROCESSOR_ID)
     );