GSP: Use atomics in interrupt reading loop

diff --git a/libctru/source/services/gsp.c b/libctru/source/services/gsp.c
index 023471244c82ef5c665a60487df232833a8c75b5..4dcfadf94d23ddf682b093918010524397a9582b 100644 (file)
--- a/libctru/source/services/gsp.c
+++ b/libctru/source/services/gsp.c
@@ -82,6 +82,50 @@ void gspWaitForEvent(GSP_Event id, bool nextEvent)
                svcClearEvent(gspEvents[id]);
 }
 
+static int popInterrupt()
+{
+       int curEvt;
+       u32 strexFailed;
+       do {
+               union {
+                       struct {
+                               u8 cur;
+                               u8 count;
+                               u8 err;
+                               u8 unused;
+                       };
+                       u32 as_u32;
+               } header;
+
+               u32* gsp_header_ptr = (u32*)(gspEventData + 0);
+
+               // Do a load on all header fields as an atomic unit
+               __asm__ volatile (
+                               "ldrex %[result], %[addr]" :
+                               [result]"=r"(header.as_u32) :
+                               [addr]"Q"(*gsp_header_ptr));
+
+               if (__builtin_expect(header.count == 0, 0)) {
+                       __asm__ volatile ("clrex");
+                       return -1;
+               }
+
+               curEvt = gspEventData[0xC + header.cur];
+
+               header.cur += 1;
+               if (header.cur >= 0x34) header.cur -= 0x34;
+               header.count -= 1;
+               header.err = 0; // Should this really be set?
+
+               __asm__ volatile (
+                               "strex %[result], %[val], %[addr]" :
+                               [result]"=&r"(strexFailed), [addr]"=Q"(*gsp_header_ptr) :
+                               [val]"r"(header.as_u32));
+       } while (__builtin_expect(strexFailed, 0));
+
+       return curEvt;
+}
+
 void gspEventThreadMain(void *arg)
 {
        while (gspRunEvents)
@@ -89,24 +133,18 @@ void gspEventThreadMain(void *arg)
                svcWaitSynchronization(gspEvent, U64_MAX);
                svcClearEvent(gspEvent);
 
-               int count = gspEventData[1];
-               int cur = gspEventData[0];
-               int last = cur + count;
-               while (last >= 0x34) last -= 0x34;
-               int i;
-               for (i = 0; i < count; i ++)
+               while (true)
                {
-                       int curEvt = gspEventData[0xC + cur];
-                       cur ++;
-                       if (cur >= 0x34) cur -= 0x34;
-                       if (curEvt >= GSPEVENT_MAX) continue;
-                       svcSignalEvent(gspEvents[curEvt]);
-                       gspEventCounts[curEvt]++;
-               }
+                       int curEvt = popInterrupt();
 
-               gspEventData[0] = last;
-               gspEventData[1] -= count;
-               gspEventData[2] = 0;
+                       if (curEvt == -1)
+                               break;
+
+                       if (curEvt < GSPEVENT_MAX) {
+                               svcSignalEvent(gspEvents[curEvt]);
+                               gspEventCounts[curEvt]++;
+                       }
+               }
        }
        svcExitThread();
 }