as you wish, Sorer

(starting work on shared-memory system)

src/Wifi.cpp

@@ -41,6 +41,9 @@ u16 IO[0x1000>>1];
 
 u16 Random;
 
+// general, always-on microsecond counter
+u64 USTimestamp;
+
 u64 USCounter;
 u64 USCompare;
 bool BlockBeaconIRQ14;
@@ -214,6 +217,8 @@ void Reset()
     memset(&IOPORT(0x018), 0xFF, 6);
     memset(&IOPORT(0x020), 0xFF, 6);
 
+    USTimestamp = 0;
+
     USCounter = 0;
     USCompare = 0;
     BlockBeaconIRQ14 = false;
@@ -1207,6 +1212,8 @@ void MSTimer()
 
 void USTimer(u32 param)
 {
+    USTimestamp++;
+
     WifiAP::USTimer();
 
     bool switchOffPowerSaving = false;
@@ -1389,10 +1396,6 @@ void USTimer(u32 param)
                         {
                             SendMPReply(*(u16*)&RXBuffer[0xC + 24], *(u16*)&RXBuffer[0xC + 26]);
                         }
-                        /*else if (RXBuffer[0xC + 4] & 0x01)
-                        {
-                            DummyMPReplyFrame(*(u16*)&RXBuffer[0xC + 24], *(u16*)&RXBuffer[0xC + 26]);
-                        }*/
                     }
                 }
 
@@ -1454,26 +1457,15 @@ void RFTransfer_Type3()
 }
 
 
-// TODO: wifi waitstates
-
 u16 Read(u32 addr)
-{//printf("WIFI READ %08X\n", addr);
+{
     if (addr >= 0x04810000)
         return 0;
 
     addr &= 0x7FFE;
-    //printf("WIFI: read %08X\n", addr);
+
     if (addr >= 0x4000 && addr < 0x6000)
     {
-        //if (addr>=0x4000 && addr<0x400C) printf("READ TX HEADER 0480%04X, %08X\n", addr, NDS::GetPC(1));
-        //if (addr>=0x4234 && addr<0x4240) printf("READ TX HEADER 0480%04X, %08X\n", addr, NDS::GetPC(1));
-        //if (RXBuffer[12] == 0x0228)
-        {
-            //u16 headeraddr = IOPORT(W_RXBufReadCursor) << 1;
-            //if ((addr & 0x1FFE) == (headeraddr + 12 + 28))
-            //if (*(u16*)&RAM[(addr - 0x1C) & 0x1FFE] == 0x0228)
-            //    printf("READ SHITTY CMD HEADER %08X\n", NDS::GetPC(1));
-        }
         return *(u16*)&RAM[addr & 0x1FFE];
     }
     if (addr >= 0x2000 && addr < 0x4000)
@@ -1558,12 +1550,12 @@ u16 Read(u32 addr)
 }
 
 void Write(u32 addr, u16 val)
-{//printf("WIFI WRITE %08X %04X\n", addr, val);
+{
     if (addr >= 0x04810000)
         return;
 
     addr &= 0x7FFE;
-    //printf("WIFI: write %08X %04X\n", addr, val);
+
     if (addr >= 0x4000 && addr < 0x6000)
     {
         *(u16*)&RAM[addr & 0x1FFE] = val;
@@ -1762,7 +1754,7 @@ void Write(u32 addr, u16 val)
     case W_USCompare2: USCompare = (USCompare & 0xFFFF0000FFFFFFFF) | ((u64)val << 32); return;
     case W_USCompare3: USCompare = (USCompare & 0x0000FFFFFFFFFFFF) | ((u64)val << 48); return;
 
-    case W_CmdCount: /*printf("CMDCOUNT=%d (%04X)\n", val*10, val);*/ CmdCounter = val * 10; return;
+    case W_CmdCount: CmdCounter = val * 10; return;
 
     case W_BBCnt:
         IOPORT(W_BBCnt) = val;
@@ -1907,16 +1899,11 @@ void Write(u32 addr, u16 val)
     case 0x268:
         return;
 
-    case W_RXFilter:
-        //printf("wifi: stupid RX filter=%04X\n", val);
-        break;
-
     default:
         //printf("WIFI unk: write %08X %04X\n", addr, val);
         break;
     }
 
-    //printf("WIFI: write %08X %04X\n", addr, val);
     IOPORT(addr&0xFFF) = val;
 }
 

src/frontend/qt_sdl/LocalMP.cpp

@@ -73,14 +73,82 @@ struct MPQueueHeader
     u32 PacketWriteOffset;
 };
 
+struct MPPacketHeader
+{
+    u32 Magic;
+    u32 Length;
+    u32 SenderID;
+};
+
 QSharedMemory* MPQueue;
-QSystemSemaphore* MPQueueSem[16];
+//QSystemSemaphore* MPQueueSem[16];
+int InstanceID;
 u32 SyncReadOffset;
 u32 PacketReadOffset;
 
+const u32 kSyncStart = 0x0010;
+const u32 kSyncEnd = 0x0100;
+const u32 kPacketStart = 0x0100;
+const u32 kPacketEnd = 0x10000;
+
 #define NIFI_VER 2
 
 
+// we need to come up with our own abstraction layer for named semaphores
+// because QSystemSemaphore doesn't support waiting with a timeout
+// and, as such, is unsuitable to our needs
+
+//#ifdef _WIN32
+#if 1
+
+HANDLE SemPool[32];
+
+void SemPoolInit()
+{
+    for (int i = 0; i < 32; i++)
+        SemPool[i] = INVALID_HANDLE_VALUE;
+}
+
+bool SemInit(int num)
+{
+    if (SemPool[num] != INVALID_HANDLE_VALUE)
+        return true;
+
+    char semname[64];
+    sprintf(semname, "Local\\melonNIFI_Sem%d", num);
+
+    HANDLE sem = CreateSemaphore(nullptr, 0, 64, semname);
+    SemPool[num] = sem;
+
+    return sem != INVALID_HANDLE_VALUE;
+}
+
+void SemDeinit(int num)
+{
+    if (SemPool[num] != INVALID_HANDLE_VALUE)
+    {
+        CloseHandle(SemPool[num]);
+        SemPool[num] = INVALID_HANDLE_VALUE;
+    }
+}
+
+bool SemPost(int num)
+{
+    SemInit(num);
+    return ReleaseSemaphore(SemPool[num], 1, nullptr) != 0;
+}
+
+bool SemWait(int num, int timeout)
+{
+    return WaitForSingleObject(SemPool[num], timeout) == WAIT_OBJECT_0;
+}
+
+#else
+
+// TODO: code semaphore shit for other platforms!
+
+#endif // _WIN32
+
 
 void _logpacket(bool tx, u8* data, int len)
 {return;
@@ -112,7 +180,7 @@ void _logpacket(bool tx, u8* data, int len)
 
 bool Init()
 {
-    int opt_true = 1;
+    /*int opt_true = 1;
     int res0, res1;
 
 #ifdef __WIN32__
@@ -201,9 +269,14 @@ bool Init()
     MPUniqueID ^= *(u32*)&mac[2];
     printf("local MP unique ID: %08X\n", MPUniqueID);
 
-    return true;
+    return true;*/
 
-    /*MPQueue = new QSharedMemory("melonNIFI");
+    /*u8* mac = SPI_Firmware::GetWifiMAC();
+    MPUniqueID = *(u32*)&mac[0];
+    MPUniqueID ^= *(u32*)&mac[2];
+    printf("local MP unique ID: %08X\n", MPUniqueID);*/
+
+    MPQueue = new QSharedMemory("melonNIFI_FIFO");
 
     if (!MPQueue->attach())
     {
@@ -217,15 +290,42 @@ bool Init()
         MPQueue->lock();
         memset(MPQueue->data(), 0, MPQueue->size());
         MPQueueHeader* header = (MPQueueHeader*)MPQueue->data();
-        header->NumInstances = 1;
+        header->SyncWriteOffset = kSyncStart;
-        header->InstanceBitmask = 0x0001;
+        header->PacketWriteOffset = kPacketStart;
-        header->SyncWriteOffset = 0x0010;
-        header->PacketWriteOffset = 0x0100;
         MPQueue->unlock();
     }
-    printf("sharedmem good\n");*/
 
+    MPQueue->lock();
+    MPQueueHeader* header = (MPQueueHeader*)MPQueue->data();
 
+    u16 mask = header->InstanceBitmask;
+    for (int i = 0; i < 16; i++)
+    {
+        if (!(mask & (1<<i)))
+        {
+            InstanceID = i;
+            header->InstanceBitmask |= (1<<i);
+            break;
+        }
+    }
+    header->NumInstances++;
+
+    SyncReadOffset = header->SyncWriteOffset;
+    PacketReadOffset = header->PacketWriteOffset;
+
+    MPQueue->unlock();
+
+    /*for (int i = 0; i < 16; i++)
+    {
+        QString key = QString("melonSEMA%1").arg(i);
+        MPQueueSem[i] = new QSystemSemaphore(key, 0, (i==InstanceID) ? QSystemSemaphore::Create : QSystemSemaphore::Open)
+    }*/
+
+    SemPoolInit();
+    SemInit(InstanceID);
+    SemInit(16+InstanceID);
+
+    printf("MP comm init OK, instance ID %d\n", InstanceID);
 
     return true;
 }
@@ -242,86 +342,115 @@ void DeInit()
 #endif // __WIN32__
 }
 
-int SendPacket(u8* data, int len)
+void PacketFIFORead(void* buf, int len)
 {
-    if (MPSocket[0] < 0)
+    u8* data = (u8*)MPQueue->data();
-        return 0;
 
-    if (len > 2048-12)
+    u32 offset = PacketReadOffset;
+    if ((offset + len) >= kPacketEnd)
     {
-        printf("MP_SendPacket: error: packet too long (%d)\n", len);
+        u32 part1 = kPacketEnd - offset;
-        return 0;
+        memcpy(buf, &data[offset], part1);
+        memcpy(&((u8*)buf)[part1], &data[kPacketStart], len - part1);
+        offset = kPacketStart + len - part1;
+    }
+    else
+    {
+        memcpy(buf, &data[offset], len);
+        offset += len;
     }
 
-    *(u32*)&PacketBuffer[0] = htonl(0x4946494E); // NIFI
+    PacketReadOffset = offset;
-    PacketBuffer[4] = NIFI_VER;
-    PacketBuffer[5] = 0;
-    *(u16*)&PacketBuffer[6] = htons(len);
-    *(u32*)&PacketBuffer[8] = MPUniqueID;
-    memcpy(&PacketBuffer[12], data, len);
-
-    _logpacket(true, data, len);
-
-    int slen = sendto(MPSocket[0], (const char*)PacketBuffer, len+12, 0, &MPSendAddr[0], sizeof(sockaddr_t));
-    if (slen < 12) return 0;
-    return slen - 12;
 }
 
-int RecvPacket(u8* data, bool block)
+void PacketFIFOWrite(void* buf, int len)
 {
-    if (MPSocket[0] < 0)
+    u8* data = (u8*)MPQueue->data();
-        return 0;
+    MPQueueHeader* header = (MPQueueHeader*)&data[0];
 
-    fd_set fd;
+    u32 offset = header->PacketWriteOffset;
-    struct timeval tv;
+    if ((offset + len) >= kPacketEnd)
+    {
+        u32 part1 = kPacketEnd - offset;
+        memcpy(&data[offset], buf, part1);
+        memcpy(&data[kPacketStart], &((u8*)buf)[part1], len - part1);
+        offset = kPacketStart + len - part1;
+    }
+    else
+    {
+        memcpy(&data[offset], buf, len);
+        offset += len;
+    }
+
+    header->PacketWriteOffset = offset;
+}
+
+int SendPacket(u8* packet, int len)
+{
+    MPQueue->lock();
+    u8* data = (u8*)MPQueue->data();
+    MPQueueHeader* header = (MPQueueHeader*)&data[0];
+
+    u16 mask = header->InstanceBitmask;
+    u32 offset = header->PacketWriteOffset;
+
+    // TODO: check if the FIFO is full!
+
+    MPPacketHeader pktheader;
+    pktheader.Magic = 0x4946494E;
+    pktheader.Length = len;
+    pktheader.SenderID = InstanceID;
+
+    PacketFIFOWrite(&pktheader, sizeof(pktheader));
+    PacketFIFOWrite(packet, len);
+
+    for (int i = 0; i < 16; i++)
+    {
+        if (mask & (1<<i))
+            SemPost(i);
+    }
+
+    MPQueue->unlock();
+    return len;
+}
+
+int RecvPacket(u8* packet, bool block)
+{
+    MPQueue->lock();
+    u8* data = (u8*)MPQueue->data();
+    MPQueueHeader* header = (MPQueueHeader*)&data[0];
 
     for (;;)
     {
-        FD_ZERO(&fd);
+        if (!SemWait(InstanceID, block ? 500 : 0))
-        FD_SET(MPSocket[0], &fd);
-        tv.tv_sec = 0;
-        //tv.tv_usec = block ? 5000 : 0;
-        tv.tv_usec = block ? 500*1000 : 0;
-
-        if (!select(MPSocket[0]+1, &fd, 0, 0, &tv))
         {
+            MPQueue->unlock();
             return 0;
         }
 
-        sockaddr_t fromAddr;
+        MPPacketHeader pktheader;
-        socklen_t fromLen = sizeof(sockaddr_t);
+        PacketFIFORead(&pktheader, sizeof(pktheader));
-        int rlen = recvfrom(MPSocket[0], (char*)PacketBuffer, 2048, 0, &fromAddr, &fromLen);
-        if (rlen < 12+24)
-        {
-            continue;
-        }
-        rlen -= 12;
 
-        if (ntohl(*(u32*)&PacketBuffer[0]) != 0x4946494E)
+        if (pktheader.Magic != 0x4946494E)
         {
+            printf("MP: !!!! PACKET FIFO IS CRAPOED\n");
+            MPQueue->unlock();
+            return 0;
+        }
+
+        if (pktheader.SenderID == InstanceID)
+        {
+            // skip this packet
+            PacketReadOffset += pktheader.Length;
+            if (PacketReadOffset >= kPacketEnd)
+                PacketReadOffset += kPacketStart - kPacketEnd;
+
             continue;
         }
 
-        if (PacketBuffer[4] != NIFI_VER || PacketBuffer[5] != 0)
+        PacketFIFORead(packet, pktheader.Length);
-        {
+        MPQueue->unlock();
-            continue;
+        return pktheader.Length;
-        }
-
-        if (ntohs(*(u16*)&PacketBuffer[6]) != rlen)
-        {
-            continue;
-        }
-
-        if (*(u32*)&PacketBuffer[8] == MPUniqueID)
-        {
-            continue;
-        }
-
-        zanf = *(u16*)&PacketBuffer[12+6];
-        _logpacket(false, &PacketBuffer[12], rlen);
-
-        memcpy(data, &PacketBuffer[12], rlen);
-        return rlen;
     }
 }